From 79324ed3141c5188f89607971ef4036bb498609f Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 9 Jan 2024 13:17:40 +0900 Subject: [PATCH] fix(crosswalk): stopping besides the stop line (#6015) * stopping besides the stop line Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b8a34bf5a9d00..3549232bdbed0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -393,7 +393,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return {}; } - // Check if the ego should stop beyond the stop line. + // Check if the ego should stop at the stop line or the other points. const bool stop_at_stop_line = dist_ego_to_stop < nearest_stop_info->second && nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; @@ -404,9 +404,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return createStopFactor(*default_stop_pose, stop_factor_points); } } else { - // Stop beyond the stop line const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, planner_param_.stop_distance_from_object); + ego_path.points, nearest_stop_info->first, + -base_link2front - planner_param_.stop_distance_from_object); if (stop_pose) { return createStopFactor(*stop_pose, stop_factor_points); }