From 7af84b18478130b17ffa01802fd349a6d5b1dc12 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 19 Aug 2022 10:18:48 +0900 Subject: [PATCH] fix(obstacle_stop_planner): fix hunting collision point (#1627) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- planning/obstacle_stop_planner/src/node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index fd345df707767..70b6eef958694 100644 --- 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); }