fix(start_planner): keep max back distace whithin lanes (autowarefoun…

…dation#4962) * fix(start_planner): keep max back distace whithin lanes Signed-off-by: kosuke55 <[email protected]> * Update planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp Co-authored-by: Kyoichi Sugahara <[email protected]> * Update planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp * style(pre-commit): autofix --------- Signed-off-by: kosuke55 <[email protected]> Co-authored-by: Kyoichi Sugahara <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Sep 12, 2023 · 190009e · 190009e
1 parent f02176d
commit 190009e
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -744,9 +744,16 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
   const auto pull_out_lane_stop_objects =
     utils::filterObjectsByVelocity(pull_out_lane_objects, parameters_->th_moving_object_velocity);
 
+  // Set the maximum backward distance less than the distance from the vehicle's base_link to the
+  // lane's rearmost point to prevent lane departure.
+  const double s_current =
+    lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length;
+  const double max_back_distance = std::clamp(
+    s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance);
+
   // check collision between footprint and object at the backed pose
   const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
-  for (double back_distance = 0.0; back_distance <= parameters_->max_back_distance;
+  for (double back_distance = 0.0; back_distance <= max_back_distance;
        back_distance += parameters_->backward_search_resolution) {
     const auto backed_pose = calcLongitudinalOffsetPose(
       start_pose_candidates.points, current_pose.position, -back_distance);