fix(obstacle_stop_planner): fix hunting collision point (autowarefoun…

…dation#1627) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tzhong518 · Oct 14, 2022 · 1285073 · 1285073
1 parent e409c6b
commit 1285073
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp
@@ -870,6 +870,10 @@ void ObstacleStopPlannerNode::insertVelocity(
     }
   } else if (!no_hunting_collision_point) {
     if (latest_stop_point_) {
+      // update stop point index with the current trajectory
+      latest_stop_point_.get().index = findFirstNearestSegmentIndexWithSoftConstraints(
+        output, getPose(latest_stop_point_.get().point), node_param_.ego_nearest_dist_threshold,
+        node_param_.ego_nearest_yaw_threshold);
       insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag);
       debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::TargetStop);
       debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::Stop);