fix(avoidance): fix avoidance path chattering (autowarefoundation#2012)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
boyali · Oct 19, 2022 · a8a10b5 · a8a10b5
1 parent 453bc2f
commit a8a10b5
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -42,6 +42,7 @@ namespace behavior_path_planner
 {
 using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
+using motion_utils::findNearestSegmentIndex;
 using tier4_autoware_utils::calcDistance2d;
 using tier4_autoware_utils::calcLateralDeviation;
 using tier4_planning_msgs::msg::AvoidanceDebugFactor;
@@ -140,8 +141,11 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb
     // if the resampled path has only 1 point, use original path.
     data.reference_path = center_path;
   }
+
+  const size_t nearest_segment_index =
+    findNearestSegmentIndex(data.reference_path.points, data.reference_pose.position);
   data.ego_closest_path_index =
-    findNearestIndex(data.reference_path.points, data.reference_pose.position);
+    std::min(nearest_segment_index + 1, data.reference_path.points.size() - 1);
 
   // arclength from ego pose (used in many functions)
   data.arclength_from_ego = util::calcPathArcLengthArray(