fix: add error handling when path is invalid (#934)

* fix(behavior_path): delete duplicated * add error handling * fix: when path size is 1
tier4 · Oct 13, 2023 · 368e66f · 368e66f
1 parent 84497a7
commit 368e66f
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 23 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;