From 2cd2ede4dbcae3394dd1bd463f3f140dc401ebd5 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 6 Oct 2023 13:47:11 +0900 Subject: [PATCH] consider rss distance Signed-off-by: kosuke55 --- .../src/scene_module/lane_change/normal.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 0cc5f8b35afe5..708f880f41ec6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -256,10 +256,15 @@ void NormalLaneChange::insertStopPoint( // If the lane change space is occupied by a stationary obstacle, move the stop line closer. // TODO(Horibe): We need to loop this process because the new space could be occupied as well. stopping_distance = std::min( - distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer, stopping_distance); + distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer - + calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params), + stopping_distance); } const auto stopping_distance_baselink = stopping_distance - getCommonParam().base_link2front; + if (stopping_distance_baselink > 0.0) { const auto stop_point = utils::insertStopPoint(stopping_distance_baselink, path); setStopPose(stop_point.point.pose);