From f392ea054b41d65d93e033cb52e055a834931d84 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 04ace8575d730..f6d367b25e9b5 100644 --- 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);