fix(crosswalk): stopping besides the stop line (autowarefoundation#6015) (#1221)

yuki-takagi-66 · web-flow · commit d43d0aba25fa · 2024-04-03T17:15:03.000+09:00
* stopping besides the stop line

Signed-off-by: Yuki Takagi &lt;yuki.takagi@tier4.jp&gt;
diff --git 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<StopFactor> 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<StopFactor> 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);
     }

Original file line number	Diff line number	Diff line change
`@@ -393,7 +393,7 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(`
`393`	`393`	`return {};`
`394`	`394`	`}`
`395`	`395`
`396`		`- // Check if the ego should stop beyond the stop line.`
	`396`	`+ // Check if the ego should stop at the stop line or the other points.`
`397`	`397`	`const bool stop_at_stop_line =`
`398`	`398`	`dist_ego_to_stop < nearest_stop_info->second &&`
`399`	`399`	`nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold;`
`@@ -404,9 +404,9 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(`
`404`	`404`	`return createStopFactor(*default_stop_pose, stop_factor_points);`
`405`	`405`	`}`
`406`	`406`	`} else {`
`407`		`- // Stop beyond the stop line`
`408`	`407`	`const auto stop_pose = calcLongitudinalOffsetPose(`
`409`		`- ego_path.points, nearest_stop_info->first, planner_param_.stop_distance_from_object);`
	`408`	`+ ego_path.points, nearest_stop_info->first,`
	`409`	`+ -base_link2front - planner_param_.stop_distance_from_object);`
`410`	`410`	`if (stop_pose) {`
`411`	`411`	`return createStopFactor(*stop_pose, stop_factor_points);`
`412`	`412`	`}`