fix(obstacle_stop_planner): fix hunting collision point (#1627)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tier4 · Nov 30, 2022 · 7af84b1 · 7af84b1
1 parent d1f8b18
commit 7af84b1
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
@@ -866,6 +866,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::Stop);
     }