feat(start_planner): shift path points to keep distance from left boundary

planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp

@@ -49,6 +49,8 @@ PathWithLaneId getBackwardPath(
   const Pose & current_pose, const Pose & backed_pose, const double velocity);
 lanelet::ConstLanelets getPullOutLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length);
+double calcLateralOffsetFromLeftBoundary(
+  const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose);
 Pose getBackedPose(
   const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance);
 std::optional<PathWithLaneId> extractCollisionCheckSection(

planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -1115,9 +1115,16 @@
   auto path =
     planner_data_->route_handler->getCenterLinePath(pull_out_lanes, start_distance, end_distance);
 
-  // shift all path points laterally to align with the start pose
+  // Calculate the offset and shift each path point to maintain a constant distance from the left
+  // boundary of the lane, aligned with the start pose
+  const double dis_left_bound_to_start_pose =
+    start_planner_utils::calcLateralOffsetFromLeftBoundary(pull_out_lanes, start_pose);
+
   for (auto & path_point : path.points) {
-    path_point.point.pose = calcOffsetPose(path_point.point.pose, 0, arc_position_pose.distance, 0);
+    const double dis_left_bound_to_center =
+      start_planner_utils::calcLateralOffsetFromLeftBoundary(pull_out_lanes, path_point.point.pose);
+    const double shift_length = dis_left_bound_to_start_pose - dis_left_bound_to_center;
+    path_point.point.pose = calcOffsetPose(path_point.point.pose, 0, shift_length, 0);
   }
 
   return path;

planning/autoware_behavior_path_start_planner_module/src/util.cpp

@@ -18,6 +18,7 @@
 #include "autoware_behavior_path_planner_common/utils/path_utils.hpp"
 #include "autoware_behavior_path_planner_common/utils/utils.hpp"
 
+#include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <motion_utils/trajectory/path_with_lane_id.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -83,6 +84,25 @@ Pose getBackedPose(
   return backed_pose;
 }
 
+double calcLateralOffsetFromLeftBoundary(
+  const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose)
+{
+  lanelet::Lanelet closest_lanelet;
+  lanelet::utils::query::getClosestLanelet(lanelets, search_pose, &closest_lanelet);
+
+  const auto & left_boundary = closest_lanelet.leftBound();
+
+  std::vector<geometry_msgs::msg::Point> left_boundary_path;
+  left_boundary_path.reserve(left_boundary.size());
+
+  for (const auto & boundary_point : left_boundary) {
+    left_boundary_path.emplace_back(
+      tier4_autoware_utils::createPoint(boundary_point.x(), boundary_point.y(), 0.0));
+  }
+
+  return motion_utils::calcLateralOffset(left_boundary_path, search_pose.position);
+}
+
 lanelet::ConstLanelets getPullOutLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length)
 {