From 368e66f810b4fc52e78ef3df358e1f05f4a3558e Mon Sep 17 00:00:00 2001
From: Hiroki OTA <hiroki.ota@tier4.jp>
Date: Fri, 13 Oct 2023 19:06:35 +0900
Subject: [PATCH] fix: add error handling when path is invalid (#934)

* fix(behavior_path): delete duplicated

* add error handling

* fix: when path size is 1
---
 .../behavior_path_planner/src/utilities.cpp   | 23 -------------------
 .../obstacle_avoidance_planner/node.hpp       | 12 ++++++++++
 .../obstacle_avoidance_planner/src/node.cpp   |  3 +++
 planning/obstacle_stop_planner/src/node.cpp   |  4 ++++
 4 files changed, 19 insertions(+), 23 deletions(-)

diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
index 84f384cf3b72..1ae670216938 100644
--- a/planning/behavior_path_planner/src/utilities.cpp
+++ b/planning/behavior_path_planner/src/utilities.cpp
@@ -26,29 +26,6 @@
 #include <string>
 #include <vector>
 
-namespace tier4_autoware_utils
-{
-template <class T>
-double calcLateralOffset(
-  const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx)
-{
-  validateNonEmpty(points);
-
-  const auto p_front = getPoint(points.at(seg_idx));
-  const auto p_back = getPoint(points.at(seg_idx + 1));
-
-  const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0};
-  const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0};
-
-  if (segment_vec.norm() == 0.0) {
-    throw std::runtime_error("Same points are given.");
-  }
-
-  const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec);
-  return cross_vec(2) / segment_vec.norm();
-}
-}  // namespace tier4_autoware_utils
-
 namespace drivable_area_utils
 {
 double quantize(const double val, const double resolution)
diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
index 5c1036b9782f..f5f43312ead0 100644
--- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
+++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
@@ -53,6 +53,10 @@ template <typename T>
 boost::optional<geometry_msgs::msg::Pose> lerpPose(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return {};
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =
@@ -94,6 +98,10 @@ template <typename T>
 double lerpTwistX(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return 0.0;
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =
@@ -116,6 +124,10 @@ template <typename T>
 double lerpPoseZ(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return 0.0;
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =
diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp
index eb75fcbee900..c5f642ebb797 100644
--- a/planning/obstacle_avoidance_planner/src/node.cpp
+++ b/planning/obstacle_avoidance_planner/src/node.cpp
@@ -1093,6 +1093,9 @@ void ObstacleAvoidancePlanner::calcVelocity(
   const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points) const
 {
+  if (path_points.size() < 2) {
+    return;
+  }
   for (size_t i = 0; i < traj_points.size(); i++) {
     const size_t nearest_seg_idx = [&]() {
       const auto opt_seg_idx = tier4_autoware_utils::findNearestSegmentIndex(
diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp
index 324a37bfa955..44de6e2cdf9c 100644
--- a/planning/obstacle_stop_planner/src/node.cpp
+++ b/planning/obstacle_stop_planner/src/node.cpp
@@ -753,6 +753,10 @@ void ObstacleStopPlannerNode::insertVelocity(
   const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info,
   const double current_acc, const double current_vel, const StopParam & stop_param)
 {
+  if (output.size() < 3) {
+    return;
+  }
+
   if (planner_data.stop_require) {
     // insert stop point
     const auto traj_end_idx = output.size() - 1;