Revert "feat(start_planner): shift path points to keep distance from …

…left boundary"

This reverts commit 89a17c8.

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp

@@ -49,8 +49,6 @@ 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/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -1115,16 +1115,9 @@ PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
   auto path =
     planner_data_->route_handler->getCenterLinePath(pull_out_lanes, start_distance, end_distance);
 
-  // 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);
-
+  // shift all path points laterally to align with the start pose
   for (auto & path_point : path.points) {
-    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);
+    path_point.point.pose = calcOffsetPose(path_point.point.pose, 0, arc_position_pose.distance, 0);
   }
 
   return path;

planning/behavior_path_start_planner_module/src/util.cpp

@@ -18,7 +18,6 @@
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "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>
@@ -84,25 +83,6 @@ 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)
 {