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 <kosuke.tnp@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

* Update planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

* style(pre-commit): autofix

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -690,9 +690,16 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
   const auto pull_out_lane_stop_objects = utils::path_safety_checker::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);