From 3f88e10c02c9b3ba90559afd0b0332035ffa565b Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Sun, 2 Jun 2024 21:26:18 +0900
Subject: [PATCH] feat(start_planner): shift path points to keep distance from
 left boundary

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../util.hpp                                  |  2 ++
 .../src/start_planner_module.cpp              | 11 ++++++++--
 .../src/util.cpp                              | 20 +++++++++++++++++++
 3 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp
index b9f0b964ed428..7e490ae45eb8b 100644
--- a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp
+++ b/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(
diff --git a/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
index c13afdf2b86df..ac875a86c4321 100644
--- a/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -1115,9 +1115,16 @@ PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
   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;
diff --git a/planning/autoware_behavior_path_start_planner_module/src/util.cpp b/planning/autoware_behavior_path_start_planner_module/src/util.cpp
index 51a6093936154..8594e9d3ebbb2 100644
--- a/planning/autoware_behavior_path_start_planner_module/src/util.cpp
+++ b/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)
 {