fix(behavior_path_planner): pull over dies after curve (autowarefound…

…ation#1963) Signed-off-by: kosuke55 <[email protected]> Signed-off-by: kosuke55 <[email protected]>
tier4 · Nov 10, 2022 · 4108569 · 4108569
1 parent 6e9b2bb
commit 4108569
Showing 1 changed file with 12 additions and 7 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp
@@ -24,7 +24,6 @@
 #include <vector>
 
 namespace behavior_path_planner
-
 {
 ShiftPullOver::ShiftPullOver(
   rclcpp::Node & node, const PullOverParameters & parameters,
@@ -108,15 +107,21 @@ std::vector<PullOverPath> ShiftPullOver::generatePullOverPaths(
   const double jerk_resolution =
     std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_over_sampling_num;
 
-  const double distance_to_shoulder_lane_boundary =
-    util::getDistanceToShoulderBoundary(shoulder_lanes, current_pose);
-  const double offset_from_current_pose =
-    distance_to_shoulder_lane_boundary + common_parameters.vehicle_width / 2 + margin;
+  // calc lateral offset from road lane center line to shoulder target line.
+  lanelet::ConstLanelet goal_closest_road_lane;
+  lanelet::utils::query::getClosestLanelet(road_lanes, goal_pose, &goal_closest_road_lane);
+  const auto closest_center_pose =
+    lanelet::utils::getClosestCenterPose(goal_closest_road_lane, goal_pose.position);
+  const double distance_from_shoulder_left_bound =
+    util::getDistanceToShoulderBoundary(shoulder_lanes, closest_center_pose);
+  const double offset_from_road_line_center =
+    distance_from_shoulder_left_bound + common_parameters.vehicle_width / 2 + margin;
 
   // shift end point in shoulder lane
   const auto shift_end_point = std::invoke([&]() {
     const auto arc_position_goal = lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose);
-    const double s_start = arc_position_goal.length - after_pull_over_straight_distance;
+    const double s_start =
+      std::max(arc_position_goal.length - after_pull_over_straight_distance, 0.0);
     const double s_end = s_start + std::numeric_limits<double>::epsilon();
     const auto path = route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end, true);
     return path.points.front();
@@ -130,7 +135,7 @@ std::vector<PullOverPath> ShiftPullOver::generatePullOverPaths(
     PullOverPath candidate_path;
 
     const double pull_over_distance = path_shifter.calcLongitudinalDistFromJerk(
-      abs(offset_from_current_pose), lateral_jerk, pull_over_velocity);
+      std::abs(offset_from_road_line_center), lateral_jerk, pull_over_velocity);
 
     // calculate straight distance before pull over
     const double straight_distance = std::invoke([&]() {