diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
index 5eb71126bb838..95b2095f87164 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
@@ -25,8 +25,8 @@
         high_curvature_threshold: 0.1
         bus_stop_area:
           use_bus_stop_area: false
-          goal_search_interval: 0.5
-          lateral_offset_interval: 0.25
+          goal_search_interval: 0.75
+          lateral_offset_interval: 0.3
 
       # occupancy grid map
       occupancy_grid:
@@ -52,6 +52,7 @@
       # pull over
       pull_over:
         minimum_request_length: 0.0
+        pull_over_prepare_length: 100.0
         pull_over_velocity: 3.0
         pull_over_minimum_velocity: 1.38
         decide_path_distance: 10.0
@@ -59,7 +60,7 @@
         maximum_jerk: 1.0
         path_priority: "efficient_path" # "efficient_path" or "close_goal"
         efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking)
-        lane_departure_check_expansion_margin: 0.0
+        lane_departure_check_expansion_margin: 0.2
 
         # shift parking
         shift_parking:
@@ -73,21 +74,21 @@
         # parallel parking path
         parallel_parking:
           path_interval: 1.0
-          max_steer_angle: 0.35  # 20deg
+          max_steer_angle: 0.4 #22.9deg
           forward:
             enable_arc_forward_parking: true
             after_forward_parking_straight_distance: 2.0
             forward_parking_velocity: 1.38
             forward_parking_lane_departure_margin: 0.0
             forward_parking_path_interval: 1.0
-            forward_parking_max_steer_angle: 0.35  # 20deg
+            forward_parking_max_steer_angle: 0.4  # 22.9deg
           backward:
             enable_arc_backward_parking: true
             after_backward_parking_straight_distance: 2.0
             backward_parking_velocity: -1.38
             backward_parking_lane_departure_margin: 0.0
             backward_parking_path_interval: 1.0
-            backward_parking_max_steer_angle: 0.35  # 20deg
+            backward_parking_max_steer_angle: 0.4  # 22.9deg
 
         # freespace parking
         freespace_parking:
@@ -130,6 +131,10 @@
             neighbor_radius: 8.0
             margin: 1.0
 
+        bezier_parking:
+          pull_over_angle_threshold: 0.5
+          after_shift_straight_distance: 1.5
+
       stop_condition:
         maximum_deceleration_for_stop: 1.0
         maximum_jerk_for_stop: 1.0
@@ -139,13 +144,13 @@
           min_velocity: 1.0
           min_acceleration: 1.0
           max_velocity: 1.0
-          time_horizon_for_front_object: 10.0
-          time_horizon_for_rear_object: 10.0
+          time_horizon_for_front_object: 5.0
+          time_horizon_for_rear_object: 5.0
           time_resolution: 0.5
           delay_until_departure: 1.0
         # For target object filtering
         target_filtering:
-          safety_check_time_horizon: 10.0
+          safety_check_time_horizon: 5.0
           safety_check_time_resolution: 1.0
           # detection range
           object_check_forward_distance: 100.0
@@ -179,7 +184,7 @@
           # safety check configuration
           enable_safety_check: true
           method: "integral_predicted_polygon"
-          keep_unsafe_time: 3.0
+          keep_unsafe_time: 0.5
           # collision check parameters
           publish_debug_marker: false
           rss_params:
@@ -192,7 +197,7 @@
             forward_margin: 1.0
             backward_margin: 1.0
             lat_margin: 1.0
-            time_horizon: 10.0
+            time_horizon: 5.0
           # hysteresis factor to expand/shrink polygon with the value
           hysteresis_factor_expand_rate: 1.0
           collision_check_yaw_diff_threshold: 3.1416
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
index 84367fdd91b80..f2cc7e2f4ef65 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -18,6 +18,7 @@
 #include "autoware/behavior_path_goal_planner_module/decision_state.hpp"
 #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp"
 #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
+#include "autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp"
 #include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp"
 #include "autoware/behavior_path_goal_planner_module/thread_data.hpp"
 #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp"
@@ -86,8 +87,8 @@ struct PullOverContextData
   explicit PullOverContextData(
     const bool is_stable_safe_path, const PredictedObjects & static_objects,
     const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state,
-    const bool is_stopped, const LaneParkingResponse & lane_parking_response,
-    const FreespaceParkingResponse & freespace_parking_response)
+    const bool is_stopped, LaneParkingResponse && lane_parking_response,
+    FreespaceParkingResponse && freespace_parking_response)
   : is_stable_safe_path(is_stable_safe_path),
     static_target_objects(static_objects),
     dynamic_target_objects(dynamic_objects),
@@ -112,8 +113,8 @@ struct PullOverContextData
   void update(
     const bool is_stable_safe_path_, const PredictedObjects static_target_objects_,
     const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_,
-    const bool is_stopped_, const LaneParkingResponse & lane_parking_response_,
-    const FreespaceParkingResponse & freespace_parking_response_)
+    const bool is_stopped_, LaneParkingResponse && lane_parking_response_,
+    FreespaceParkingResponse && freespace_parking_response_)
   {
     is_stable_safe_path = is_stable_safe_path_;
     static_target_objects = static_target_objects_;
@@ -196,6 +197,21 @@ class LaneParkingPlanner
     original_upstream_module_output_;  //<! upstream_module_output used for generating last
                                        // pull_over_path_candidates(only updated when new candidates
                                        // are generated)
+  std::shared_ptr<BezierPullOver> bezier_pull_over_planner_;
+  const double pull_over_angle_threshold;
+
+  bool switch_bezier_{false};
+  void normal_pullover_planning_helper(
+    const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
+    const BehaviorModuleOutput & upstream_module_output,
+    const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
+    std::vector<PullOverPath> & path_candidates);
+  void bezier_planning_helper(
+    const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
+    const BehaviorModuleOutput & upstream_module_output,
+    const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
+    std::vector<PullOverPath> & path_candidates,
+    std::optional<std::vector<size_t>> & sorted_indices) const;
 };
 
 class FreespaceParkingPlanner
@@ -302,6 +318,7 @@ class GoalPlannerModule : public SceneModuleInterface
 private:
   std::pair<LaneParkingResponse, FreespaceParkingResponse> syncWithThreads();
 
+  bool trigger_thread_on_approach_{false};
   // NOTE: never access to following variables except in updateData()!!!
   std::mutex lane_parking_mutex_;
   std::optional<LaneParkingRequest> lane_parking_request_;
@@ -433,7 +450,8 @@ class GoalPlannerModule : public SceneModuleInterface
   std::optional<PullOverPath> selectPullOverPath(
     const PullOverContextData & context_data,
     const std::vector<PullOverPath> & pull_over_path_candidates,
-    const GoalCandidates & goal_candidates) const;
+    const GoalCandidates & goal_candidates,
+    const std::optional<std::vector<size_t>> sorted_bezier_indices_opt) const;
 
   // lanes and drivable area
   std::vector<DrivableLanes> generateDrivableLanes() const;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp
index 440e57b53d07f..814cffd388703 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp
@@ -88,6 +88,7 @@ struct GoalPlannerParameters
 
   // pull over general params
   double pull_over_minimum_request_length{0.0};
+  double pull_over_prepare_length{0.0};
   double pull_over_velocity{0.0};
   double pull_over_minimum_velocity{0.0};
   double decide_path_distance{0.0};
@@ -119,6 +120,12 @@ struct GoalPlannerParameters
   AstarParam astar_parameters{};
   RRTStarParam rrt_star_parameters{};
 
+  struct BezierParking
+  {
+    double pull_over_angle_threshold;
+    double after_shift_straight_distance;
+  } bezier_parking;
+
   // stop condition
   double maximum_deceleration_for_stop{0.0};
   double maximum_jerk_for_stop{0.0};
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp
index 5af69894c0a2e..7f404a4055b26 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp
@@ -68,6 +68,10 @@ struct PullOverPath
   const std::vector<double> & parking_path_curvatures() const { return parking_path_curvatures_; }
   double full_path_max_curvature() const { return full_path_max_curvature_; }
   double parking_path_max_curvature() const { return parking_path_max_curvature_; }
+  double parking_path_curvature_total_derivative() const
+  {
+    return parking_path_curvature_total_derivative_;
+  }
   size_t path_idx() const { return path_idx_; }
 
   bool incrementPathIndex();
@@ -94,7 +98,7 @@ struct PullOverPath
     const PathWithLaneId & full_path, const PathWithLaneId & parking_path,
     const std::vector<double> & full_path_curvatures,
     const std::vector<double> & parking_path_curvatures, const double full_path_max_curvature,
-    const double parking_path_max_curvature,
+    const double parking_path_max_curvature, const double parking_path_curvature_total_derivative,
     const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
   PullOverPlannerType type_;
@@ -109,6 +113,7 @@ struct PullOverPath
   std::vector<double> parking_path_curvatures_;
   double full_path_max_curvature_;
   double parking_path_max_curvature_;
+  double parking_path_curvature_total_derivative_;
 
   // accelerate with constant acceleration to the target velocity
   size_t path_idx_;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp
index 7173b275e26fb..af20c5f6debe5 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp
@@ -43,7 +43,8 @@ class LaneParkingRequest
   void update(
     const PlannerData & planner_data, const ModuleStatus & current_status,
     const BehaviorModuleOutput & upstream_module_output,
-    const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data);
+    const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data,
+    const bool trigger_thread_on_approach);
 
   const autoware::universe_utils::LinearRing2d vehicle_footprint_;
   const GoalCandidates goal_candidates_;
@@ -56,6 +57,7 @@ class LaneParkingRequest
   }
   const std::optional<PullOverPath> & get_pull_over_path() const { return pull_over_path_; }
   const PathDecisionState & get_prev_data() const { return prev_data_; }
+  bool trigger_thread_on_approach() const { return trigger_thread_on_approach_; }
 
 private:
   std::shared_ptr<PlannerData> planner_data_;
@@ -63,12 +65,14 @@ class LaneParkingRequest
   BehaviorModuleOutput upstream_module_output_;
   std::optional<PullOverPath> pull_over_path_;  //<! pull over path selected by main thread
   PathDecisionState prev_data_;
+  bool trigger_thread_on_approach_{false};
 };
 
 struct LaneParkingResponse
 {
   std::vector<PullOverPath> pull_over_path_candidates;
   std::optional<Pose> closest_start_pose;
+  std::optional<std::vector<size_t>> sorted_bezier_indices_opt;
 };
 
 class FreespaceParkingRequest
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
index 0a92f6d99dfeb..31e3dc89767be 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -29,6 +29,8 @@
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
 #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
 
+#include <autoware/universe_utils/math/normalization.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
@@ -38,6 +40,7 @@
 #include <algorithm>
 #include <cstddef>
 #include <deque>
+#include <execution>
 #include <functional>
 #include <limits>
 #include <map>
@@ -276,7 +279,8 @@ LaneParkingPlanner::LaneParkingPlanner(
   request_(request),
   response_(response),
   is_lane_parking_cb_running_(is_lane_parking_cb_running),
-  logger_(logger)
+  logger_(logger),
+  pull_over_angle_threshold(parameters.bezier_parking.pull_over_angle_threshold)
 {
   for (const std::string & planner_type : parameters.efficient_path_order) {
     if (planner_type == "SHIFT" && parameters.enable_shift_parking) {
@@ -290,6 +294,8 @@ LaneParkingPlanner::LaneParkingPlanner(
     }
   }
 
+  bezier_pull_over_planner_ = std::make_shared<BezierPullOver>(node, parameters);
+
   if (pull_over_planners_.empty()) {
     RCLCPP_ERROR(logger_, "Not found enabled planner");
   }
@@ -312,18 +318,18 @@ void LaneParkingPlanner::onTimer()
   }
   // end of critical section
   if (!local_request_opt) {
-    RCLCPP_ERROR(logger_, "main thread has not yet set request for LaneParkingPlanner");
+    RCLCPP_DEBUG(logger_, "main thread has not yet set request for LaneParkingPlanner");
     return;
   }
   const auto & local_request = local_request_opt.value();
   const auto & goal_candidates = local_request.goal_candidates_;
   const auto & local_planner_data = local_request.get_planner_data();
-  const auto & current_status = local_request.get_current_status();
   const auto & upstream_module_output = local_request.get_upstream_module_output();
   const auto & pull_over_path_opt = local_request.get_pull_over_path();
   const auto & prev_data = local_request.get_prev_data();
+  const auto trigger_thread_on_approach = local_request.trigger_thread_on_approach();
 
-  if (current_status == ModuleStatus::IDLE) {
+  if (!trigger_thread_on_approach) {
     return;
   }
 
@@ -387,19 +393,59 @@ void LaneParkingPlanner::onTimer()
     /*forward_only_in_route*/ false);
   std::vector<PullOverPath> path_candidates{};
   std::optional<Pose> closest_start_pose{};
-  double min_start_arc_length = std::numeric_limits<double>::max();
+  std::optional<std::vector<size_t>> sorted_indices_opt{std::nullopt};
+  if (parameters_.bus_stop_area.use_bus_stop_area && switch_bezier_) {
+    bezier_planning_helper(
+      local_planner_data, goal_candidates, upstream_module_output, current_lanes,
+      closest_start_pose, path_candidates, sorted_indices_opt);
+  } else {
+    normal_pullover_planning_helper(
+      local_planner_data, goal_candidates, upstream_module_output, current_lanes,
+      closest_start_pose, path_candidates);
+  }
+
+  // set response
+  {
+    original_upstream_module_output_ = upstream_module_output;
+    std::lock_guard<std::mutex> guard(mutex_);
+    response_.pull_over_path_candidates = path_candidates;
+    if (closest_start_pose) {
+      response_.closest_start_pose = closest_start_pose;
+    }
+    RCLCPP_INFO(
+      getLogger(), "generated %lu pull over path candidates",
+      response_.pull_over_path_candidates.size());
+    response_.sorted_bezier_indices_opt = std::move(sorted_indices_opt);
+  }
+}
+
+void LaneParkingPlanner::normal_pullover_planning_helper(
+  const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
+  const BehaviorModuleOutput & upstream_module_output,
+  const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
+  std::vector<PullOverPath> & path_candidates)
+{
+  // todo: currently non centerline input path is supported only by shift pull over
+  const bool is_center_line_input_path = goal_planner_utils::isReferencePath(
+    upstream_module_output.reference_path, upstream_module_output.path, 0.1);
+  RCLCPP_DEBUG(
+    getLogger(), "the input path of pull over planner is center line: %d",
+    is_center_line_input_path);
+
+  double min_start_arc_length = std::numeric_limits<double>::infinity();
   const auto planCandidatePaths = [&](
                                     const std::shared_ptr<PullOverPlannerBase> & planner,
                                     const GoalCandidate & goal_candidate) {
-    const auto pull_over_path = planner->plan(
-      goal_candidate, path_candidates.size(), local_planner_data, upstream_module_output);
+    // normal pull_over
+    const auto pull_over_path =
+      planner->plan(goal_candidate, path_candidates.size(), planner_data, upstream_module_output);
     if (pull_over_path) {
       // calculate absolute maximum curvature of parking path(start pose to end pose) for path
       // priority
       path_candidates.push_back(*pull_over_path);
       // calculate closest pull over start pose for stop path
       const double start_arc_length =
-        lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose()).length;
+        lanelet::utils::getArcCoordinates(current_lanelets, pull_over_path->start_pose()).length;
       if (start_arc_length < min_start_arc_length) {
         min_start_arc_length = start_arc_length;
         // closest start pose is stop point when not finding safe path
@@ -408,13 +454,6 @@ void LaneParkingPlanner::onTimer()
     }
   };
 
-  // todo: currently non centerline input path is supported only by shift pull over
-  const bool is_center_line_input_path = goal_planner_utils::isReferencePath(
-    upstream_module_output.reference_path, upstream_module_output.path, 0.1);
-  RCLCPP_DEBUG(
-    getLogger(), "the input path of pull over planner is center line: %d",
-    is_center_line_input_path);
-
   // plan candidate paths and set them to the member variable
   if (parameters_.path_priority == "efficient_path") {
     for (const auto & planner : pull_over_planners_) {
@@ -436,22 +475,103 @@ void LaneParkingPlanner::onTimer()
         planCandidatePaths(planner, goal_candidate);
       }
     }
-  } else {
-    RCLCPP_ERROR(
-      getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.",
-      parameters_.path_priority.c_str());
-    throw std::domain_error("[pull_over] invalid path_priority");
   }
 
-  // set response
-  {
-    original_upstream_module_output_ = upstream_module_output;
-    std::lock_guard<std::mutex> guard(mutex_);
-    response_.pull_over_path_candidates = std::move(path_candidates);
-    response_.closest_start_pose = closest_start_pose;
+  if (closest_start_pose) {
+    const auto original_pose = planner_data->route_handler->getOriginalGoalPose();
+    if (
+      parameters_.bus_stop_area.use_bus_stop_area &&
+      std::fabs(autoware::universe_utils::normalizeRadian(
+        autoware::universe_utils::getRPY(original_pose).z -
+        autoware::universe_utils::getRPY(closest_start_pose.value()).z)) >
+        pull_over_angle_threshold) {
+      // reset and try bezier next time
+      switch_bezier_ = true;
+      path_candidates.clear();
+      RCLCPP_INFO(getLogger(), "will generate Bezier Paths next");
+    }
+  } else if (parameters_.bus_stop_area.use_bus_stop_area && path_candidates.size() == 0) {
+    switch_bezier_ = true;
     RCLCPP_INFO(
-      getLogger(), "generated %lu pull over path candidates",
-      response_.pull_over_path_candidates.size());
+      getLogger(), "Could not find any shift pull over paths, will generate Bezier Paths next");
+  }
+}
+
+void LaneParkingPlanner::bezier_planning_helper(
+  const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
+  const BehaviorModuleOutput & upstream_module_output,
+  const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
+  std::vector<PullOverPath> & path_candidates,
+  std::optional<std::vector<size_t>> & sorted_indices_opt) const
+{
+  autoware::universe_utils::StopWatch timer;
+  timer.tic("bezier");
+  for (const auto & goal_candidate : goal_candidates) {
+    auto bezier_pull_over_paths = bezier_pull_over_planner_->plans(
+      goal_candidate, path_candidates.size(), planner_data, upstream_module_output);
+    std::copy(
+      std::make_move_iterator(bezier_pull_over_paths.begin()),
+      std::make_move_iterator(bezier_pull_over_paths.end()), std::back_inserter(path_candidates));
+  }
+  RCLCPP_INFO(
+    getLogger(), "there are %lu bezier paths (calculated in %f [sec])", path_candidates.size(),
+    timer.toc("bezier"));
+
+  sorted_indices_opt = std::vector<size_t>();
+  auto & sorted_indices = sorted_indices_opt.value();
+  sorted_indices.reserve(path_candidates.size());
+
+  std::unordered_map<int, GoalCandidate> goal_candidate_map;
+  for (const auto & goal_candidate : goal_candidates) {
+    goal_candidate_map[goal_candidate.id] = goal_candidate;
+  }
+  for (size_t i = 0; i < path_candidates.size(); ++i) {
+    const auto & path = path_candidates[i];
+    const auto goal_candidate_it = goal_candidate_map.find(path.goal_id());
+    if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) {
+      sorted_indices.push_back(i);
+    }
+  }
+  const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects(
+    *(planner_data->dynamic_object), *(planner_data->route_handler), parameters_,
+    planner_data->parameters.vehicle_width);
+  const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
+    dynamic_target_objects, parameters_.th_moving_object_velocity);
+  sortPullOverPaths(
+    planner_data, parameters_, path_candidates, goal_candidates, static_target_objects, getLogger(),
+    sorted_indices);
+
+  const auto clip_size = std::min<size_t>(path_candidates.size(), 100);
+  const auto path_candidates_tmp = std::move(path_candidates);
+  // take upto 100 elements
+  sorted_indices.resize(clip_size);
+  for (const auto sorted_index : sorted_indices) {
+    path_candidates.push_back(path_candidates_tmp[sorted_index]);
+  }
+  sorted_indices.clear();
+  // now path_candidates are sorted from 0 to 100
+  for (unsigned i = 0; i < clip_size; ++i) {
+    sorted_indices.push_back(i);
+  }
+
+  std::stable_sort(
+    std::execution::par, sorted_indices.begin(), sorted_indices.end(),
+    [&](const size_t a_i, const size_t b_i) {
+      const auto & a = path_candidates[a_i];
+      const auto & b = path_candidates[b_i];
+      return a.parking_path_curvature_total_derivative() <
+             b.parking_path_curvature_total_derivative();
+    });
+
+  double min_start_arc_length = std::numeric_limits<double>::infinity();
+  for (const auto & bezier_pull_over_path : path_candidates) {
+    const double start_arc_length =
+      lanelet::utils::getArcCoordinates(current_lanelets, bezier_pull_over_path.start_pose())
+        .length;
+    if (start_arc_length < min_start_arc_length) {
+      min_start_arc_length = start_arc_length;
+      closest_start_pose = bezier_pull_over_path.start_pose();
+    }
   }
 }
 
@@ -471,7 +591,7 @@ void FreespaceParkingPlanner::onTimer()
   }
   // end of critical section
   if (!local_request_opt) {
-    RCLCPP_ERROR(logger_, "main thread has not yet set request for FreespaceParkingPlanner");
+    RCLCPP_DEBUG(logger_, "main thread has not yet set request for FreespaceParkingPlanner");
     return;
   }
   const auto & local_request = local_request_opt.value();
@@ -502,24 +622,9 @@ void FreespaceParkingPlanner::onTimer()
     return;
   }
 
-  const double vehicle_width = local_planner_data->parameters.vehicle_width;
-  const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE;
-  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
-    *(local_planner_data->route_handler), left_side_parking, parameters.backward_goal_search_length,
-    parameters.forward_goal_search_length);
-  const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
-    pull_over_lanes, left_side_parking, parameters.detection_bound_offset,
-    parameters.margin_from_boundary + parameters.max_lateral_offset + vehicle_width);
-
-  PredictedObjects dynamic_target_objects{};
-  for (const auto & object : local_planner_data->dynamic_object->objects) {
-    const auto object_polygon = universe_utils::toPolygon2d(object);
-    if (
-      objects_extraction_polygon.has_value() &&
-      boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) {
-      dynamic_target_objects.objects.push_back(object);
-    }
-  }
+  const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects(
+    *(local_planner_data->dynamic_object), *(local_planner_data->route_handler), parameters,
+    local_planner_data->parameters.vehicle_width);
   const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
     dynamic_target_objects, parameters.th_moving_object_velocity);
 
@@ -572,7 +677,7 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
     // count is affected
     lane_parking_request_.value().update(
       *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path,
-      path_decision_controller_.get_current_state());
+      path_decision_controller_.get_current_state(), trigger_thread_on_approach_);
     // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as
     // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since
     // behavior_path_planner::run() updates
@@ -614,19 +719,31 @@ void GoalPlannerModule::updateData()
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
-  // extract static and dynamic objects in extraction polygon for path collision check
-  const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects(
-    *(planner_data_->dynamic_object), *(planner_data_->route_handler), *parameters_,
-    planner_data_->parameters.vehicle_width);
-  const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
-    dynamic_target_objects, parameters_->th_moving_object_velocity);
-
   // update goal searcher and generate goal candidates
   if (goal_candidates_.empty()) {
     goal_candidates_ = generateGoalCandidates();
   }
 
-  auto [lane_parking_response, freespace_parking_response] = syncWithThreads();
+  const lanelet::ConstLanelets current_lanes =
+    utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
+
+  if (
+    !trigger_thread_on_approach_ &&
+    utils::isAllowedGoalModification(planner_data_->route_handler) &&
+    goal_planner_utils::is_goal_reachable_on_path(
+      current_lanes, *(planner_data_->route_handler), left_side_parking_)) {
+    const double self_to_goal_arc_length = utils::getSignedDistance(
+      planner_data_->self_odometry->pose.pose, planner_data_->route_handler->getOriginalGoalPose(),
+      current_lanes);
+    if (self_to_goal_arc_length < parameters_->pull_over_prepare_length) {
+      trigger_thread_on_approach_ = true;
+      [[maybe_unused]] const auto send_only_request = syncWithThreads();
+      RCLCPP_INFO(
+        getLogger(), "start preparing goal candidates once for goal ahead of %f meter",
+        self_to_goal_arc_length);
+      return;
+    }
+  }
 
   if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) {
     return;
@@ -657,11 +774,19 @@ void GoalPlannerModule::updateData()
     ego_predicted_path_params_, objects_filtering_params_, safety_check_params_);
   debug_data_.collision_check = collision_check_map;
   // update to latest state
+  // extract static and dynamic objects in extraction polygon for path collision check
+  const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects(
+    *(planner_data_->dynamic_object), *(planner_data_->route_handler), *parameters_,
+    planner_data_->parameters.vehicle_width);
+  const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
+    dynamic_target_objects, parameters_->th_moving_object_velocity);
+
   path_decision_controller_.transit_state(
     found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects,
     modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_,
     goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded);
 
+  auto [lane_parking_response, freespace_parking_response] = syncWithThreads();
   if (context_data_) {
     context_data_.value().update(
       path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
@@ -669,7 +794,7 @@ void GoalPlannerModule::updateData()
       isStopped(
         odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time,
         parameters_->th_stopped_velocity),
-      lane_parking_response, freespace_parking_response);
+      std::move(lane_parking_response), std::move(freespace_parking_response));
   } else {
     context_data_.emplace(
       path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
@@ -677,7 +802,7 @@ void GoalPlannerModule::updateData()
       isStopped(
         odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time,
         parameters_->th_stopped_velocity),
-      lane_parking_response, freespace_parking_response);
+      std::move(lane_parking_response), std::move(freespace_parking_response));
   }
   auto & ctx_data = context_data_.value();
 
@@ -935,7 +1060,7 @@ void sortPullOverPaths(
 
   // Sort pull_over_path_candidates based on the order in goal_candidates
   std::stable_sort(
-    sorted_path_indices.begin(), sorted_path_indices.end(),
+    std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(),
     [&](const size_t a_i, const size_t b_i) {
       const auto & a = pull_over_path_candidates[a_i];
       const auto & b = pull_over_path_candidates[b_i];
@@ -987,7 +1112,7 @@ void sortPullOverPaths(
 
     // sorts in descending order so the item with larger margin comes first
     std::stable_sort(
-      sorted_path_indices.begin(), sorted_path_indices.end(),
+      std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(),
       [&](const size_t a_i, const size_t b_i) {
         const auto & a = pull_over_path_candidates[a_i];
         const auto & b = pull_over_path_candidates[b_i];
@@ -1022,7 +1147,7 @@ void sortPullOverPaths(
 
     // NOTE: this is just partition sort based on curvature threshold within each sub partitions
     std::stable_sort(
-      sorted_path_indices.begin(), sorted_path_indices.end(),
+      std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(),
       [&](const size_t a_i, const size_t b_i) {
         const auto & a = pull_over_path_candidates[a_i];
         const auto & b = pull_over_path_candidates[b_i];
@@ -1043,7 +1168,7 @@ void sortPullOverPaths(
     // the collision check margin and curvature priority.
     if (parameters.path_priority == "efficient_path") {
       std::stable_sort(
-        sorted_path_indices.begin(), sorted_path_indices.end(),
+        std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(),
         [&](const size_t a_i, const size_t b_i) {
           // if any of following conditions are met, sort by path type priority
           // - both are soft margin
@@ -1077,7 +1202,7 @@ void sortPullOverPaths(
      */
     if (parameters.path_priority == "efficient_path") {
       std::stable_sort(
-        sorted_path_indices.begin(), sorted_path_indices.end(),
+        std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(),
         [&](const size_t a_i, const size_t b_i) {
           const auto & a = pull_over_path_candidates[a_i];
           const auto & b = pull_over_path_candidates[b_i];
@@ -1093,7 +1218,8 @@ void sortPullOverPaths(
 std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
   const PullOverContextData & context_data,
   const std::vector<PullOverPath> & pull_over_path_candidates,
-  const GoalCandidates & goal_candidates) const
+  const GoalCandidates & goal_candidates,
+  const std::optional<std::vector<size_t>> sorted_bezier_indices_opt) const
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
@@ -1105,8 +1231,11 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
   // STEP1: Filter valid paths before sorting
   // NOTE: Since copying pull over path takes time, it is handled by indices
   std::vector<size_t> sorted_path_indices;
-  sorted_path_indices.reserve(pull_over_path_candidates.size());
-
+  if (!sorted_bezier_indices_opt) {
+    sorted_path_indices.reserve(pull_over_path_candidates.size());
+  } else {
+    sorted_path_indices = sorted_bezier_indices_opt.value();
+  }
   // STEP1-1: Extract paths which have safe goal
   // Create a map of goal_id to GoalCandidate for quick access
   std::unordered_map<int, GoalCandidate> goal_candidate_map;
@@ -1199,6 +1328,7 @@ void GoalPlannerModule::setOutput(
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
   output.reference_path = getPreviousModuleOutput().reference_path;
+  output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
 
   if (!selected_pull_over_path_with_velocity_opt) {
     // situation : not safe against static objects use stop_path
@@ -1359,6 +1489,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate(
   output.modified_goal = pull_over_output.modified_goal;
   output.path = generateStopPath(context_data, detail);
   output.reference_path = getPreviousModuleOutput().reference_path;
+  output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
 
   const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
     output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters);
@@ -1412,7 +1543,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData
     // if the final path is not decided and enough time has passed since last path update,
     // select safe path from lane parking pull over path candidates
     // and set it to thread_safe_data_
-    RCLCPP_DEBUG(getLogger(), "Update pull over path candidates");
+    RCLCPP_INFO(getLogger(), "Update pull over path candidates");
 
     context_data.pull_over_path_opt = std::nullopt;
     context_data.last_path_update_time = std::nullopt;
@@ -1426,8 +1557,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData
     // Select a path that is as safe as possible and has a high priority.
     const auto & pull_over_path_candidates =
       context_data.lane_parking_response.pull_over_path_candidates;
-    const auto lane_pull_over_path_opt =
-      selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates);
+    const auto lane_pull_over_path_opt = selectPullOverPath(
+      context_data, pull_over_path_candidates, goal_candidates,
+      context_data.lane_parking_response.sorted_bezier_indices_opt);
 
     // update thread_safe_data_
     const auto & pull_over_path_opt =
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
index 66192a00a99fa..b973cfd7ed549 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
@@ -214,12 +214,9 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
         (transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontLeftIndex) +
          transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontRightIndex)) /
         2.0;
-      lanelet::ConstLanelet vehicle_front_closest_lanelet;
-      lanelet::utils::query::getClosestLanelet(
-        pull_over_lanes, search_pose, &vehicle_front_closest_lanelet);
+      const auto pull_over_lanelet = lanelet::utils::combineLaneletsShape(pull_over_lanes);
       const auto vehicle_front_pose_for_bound_opt = goal_planner_utils::calcClosestPose(
-        left_side_parking_ ? vehicle_front_closest_lanelet.leftBound()
-                           : vehicle_front_closest_lanelet.rightBound(),
+        left_side_parking_ ? pull_over_lanelet.leftBound() : pull_over_lanelet.rightBound(),
         autoware::universe_utils::createPoint(
           vehicle_front_midpoint.x(), vehicle_front_midpoint.y(), search_pose.position.z));
       if (!vehicle_front_pose_for_bound_opt) {
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
index a99252c923328..aeaf6836cdba3 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
@@ -139,6 +139,7 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
     const std::string ns = base_ns + "pull_over.";
     p.pull_over_minimum_request_length =
       node->declare_parameter<double>(ns + "minimum_request_length");
+    p.pull_over_prepare_length = node->declare_parameter<double>(ns + "pull_over_prepare_length");
     p.pull_over_velocity = node->declare_parameter<double>(ns + "pull_over_velocity");
     p.pull_over_minimum_velocity =
       node->declare_parameter<double>(ns + "pull_over_minimum_velocity");
@@ -266,6 +267,15 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
     p.rrt_star_parameters.margin = node->declare_parameter<double>(ns + "margin");
   }
 
+  // bezier parking
+  {
+    const std::string ns = base_ns + "pull_over.bezier_parking.";
+    p.bezier_parking.pull_over_angle_threshold =
+      node->declare_parameter<double>(ns + "pull_over_angle_threshold");
+    p.bezier_parking.after_shift_straight_distance =
+      node->declare_parameter<double>(ns + "after_shift_straight_distance");
+  }
+
   // stop condition
   {
     p.maximum_deceleration_for_stop =
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp
index ec3984a6d9443..3347f2e4a6bdf 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp
@@ -33,8 +33,12 @@ namespace autoware::behavior_path_planner
 {
 BezierPullOver::BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters)
 : PullOverPlannerBase(node, parameters),
-  lane_departure_checker_(
-    lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_),
+  lane_departure_checker_{[&]() {
+    auto lane_departure_checker_params = lane_departure_checker::Param{};
+    lane_departure_checker_params.footprint_extra_margin =
+      parameters.lane_departure_check_expansion_margin;
+    return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_};
+  }()},
   left_side_parking_(parameters.parking_policy == ParkingPolicy::LEFT_SIDE)
 {
 }
@@ -76,7 +80,8 @@ std::optional<PullOverPath> BezierPullOver::plan(
 }
 
 std::vector<PullOverPath> BezierPullOver::plans(
-  [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id,
+  [[maybe_unused]] const GoalCandidate & modified_goal_pose,
+  [[maybe_unused]] const size_t initial_id,
   [[maybe_unused]] const std::shared_ptr<const PlannerData> planner_data,
   [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output)
 {
@@ -101,10 +106,12 @@ std::vector<PullOverPath> BezierPullOver::plans(
 
   // find safe one from paths with different jerk
   std::vector<PullOverPath> paths;
+  auto id = initial_id;
   for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) {
     auto pull_over_paths = generateBezierPath(
       modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes,
       lateral_jerk);
+    id += pull_over_paths.size();
     std::copy(
       std::make_move_iterator(pull_over_paths.begin()),
       std::make_move_iterator(pull_over_paths.end()), std::back_inserter(paths));
@@ -114,17 +121,20 @@ std::vector<PullOverPath> BezierPullOver::plans(
 }
 
 std::vector<PullOverPath> BezierPullOver::generateBezierPath(
-  const GoalCandidate & goal_candidate, const size_t id,
+  const GoalCandidate & goal_candidate, const size_t initial_id,
   const std::shared_ptr<const PlannerData> planner_data,
   const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes,
   const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const
 {
   const double pull_over_velocity = parameters_.pull_over_velocity;
+  const double after_shift_straight_distance =
+    parameters_.bezier_parking.after_shift_straight_distance;
 
   const auto & goal_pose = goal_candidate.goal_pose;
 
   // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy
-  const Pose shift_end_pose = goal_pose;
+  const Pose shift_end_pose =
+    autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0);
 
   // calculate lateral shift of previous module path terminal pose from road lane reference path
   const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline(
@@ -174,21 +184,33 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
     -before_shifted_pull_over_distance);
 
   std::vector<std::tuple<double, double, double>> params;
-  const size_t n_sample_v_init = 4;
-  const size_t n_sample_v_final = 4;
-  const size_t n_sample_acc = 3;
+  const size_t n_sample_v_init = 3;
+  const size_t n_sample_v_final = 3;
+  const size_t n_sample_acc = 2;
   for (unsigned i = 0; i <= n_sample_v_init; ++i) {
     for (unsigned j = 0; j <= n_sample_v_final; j++) {
       for (unsigned k = 0; k <= n_sample_acc; k++) {
-        const double v_init_coeff = i * 0.25;
-        const double v_final_coeff = j * 0.25;
-        const double acc_coeff = k * (10.0 / 3);
+        const double v_init_coeff = i * (1.0 / n_sample_v_init);
+        const double v_final_coeff = j * 0.25 / (1.0 / n_sample_v_final);
+        const double acc_coeff = k * (10.0 / n_sample_acc);
         params.emplace_back(v_init_coeff, v_final_coeff, acc_coeff);
       }
     }
   }
 
+  const auto drivable_lanes =
+    utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes);
+  const auto & dp = planner_data->drivable_area_expansion_parameters;
+  const auto expanded_lanes = utils::expandLanelets(
+    drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
+    dp.drivable_area_types_to_skip);
+  const auto combined_drivable = utils::combineDrivableLanes(
+    expanded_lanes, upstream_module_output.drivable_area_info.drivable_lanes);
+  const auto parking_lot_polygons =
+    lanelet::utils::query::getAllParkingLots(planner_data->route_handler->getLaneletMapPtr());
+
   std::vector<PullOverPath> bezier_paths;
+  auto id = initial_id;
   for (const auto & [v_init_coeff, v_final_coeff, acc_coeff] : params) {
     // set path shifter and generate shifted path
     PathShifter path_shifter{};
@@ -213,7 +235,7 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
     const auto from_idx = from_idx_opt.value();
     const auto to_idx = to_idx_opt.value();
     const auto span = static_cast<unsigned>(
-      std::max<int>(static_cast<int>(to_idx) - static_cast<int>(from_idx), 0));
+      std::max<int>(static_cast<int>(to_idx) - static_cast<int>(from_idx) + 1, 0));
     const auto reference_curvature =
       autoware::motion_utils::calcCurvature(processed_prev_module_path->points);
     const auto & from_pose = shifted_path.path.points[from_idx].point.pose;
@@ -224,7 +246,10 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
       reference_curvature.at(from_idx),
       tf2::getYaw(from_pose.orientation)};
     const autoware::sampler_common::State final{
-      {to_pose.position.x, to_pose.position.y}, {0.0, 0.0}, 0.0, tf2::getYaw(to_pose.orientation)};
+      {to_pose.position.x, to_pose.position.y},
+      {0.0, 0.0},
+      reference_curvature.at(to_idx),
+      tf2::getYaw(to_pose.orientation)};
     // setting the initial velocity to higher gives straight forward path (the steering does not
     // change)
     const auto bezier_path =
@@ -237,10 +262,20 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
       p.point.pose.orientation =
         universe_utils::createQuaternionFromRPY(0.0, 0.0, bezier_points[i].z());
     }
+
+    // interpolate between shift end pose to goal pose
+    for (const auto & toward_goal_straight_pose :
+         utils::interpolatePose(shift_end_pose, goal_pose, 0.5)) {
+      PathPointWithLaneId p = shifted_path.path.points.back();
+      p.point.pose = toward_goal_straight_pose;
+      shifted_path.path.points.push_back(p);
+    }
+
     shifted_path.path.points =
       autoware::motion_utils::removeOverlapPoints(shifted_path.path.points);
-
-    autoware::motion_utils::insertOrientation(shifted_path.path.points, true);
+    /* insertOrientation erases original goal pose, so not used
+      autoware::motion_utils::insertOrientation(shifted_path.path.points, true);
+    */
 
     // combine road lanes and shoulder lanes to find closest lanelet id
     const auto lanes = std::invoke([&]() -> lanelet::ConstLanelets {
@@ -289,11 +324,9 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
 
     // check if the parking path will leave drivable area and lanes
     const bool is_in_parking_lots = std::invoke([&]() -> bool {
-      const auto & p = planner_data->parameters;
-      const auto parking_lot_polygons =
-        lanelet::utils::query::getAllParkingLots(planner_data->route_handler->getLaneletMapPtr());
       const auto path_footprints = goal_planner_utils::createPathFootPrints(
-        pull_over_path.parking_path(), p.base_link2front, p.base_link2rear, p.vehicle_width);
+        pull_over_path.parking_path(), planner_data->parameters.base_link2front,
+        planner_data->parameters.base_link2rear, planner_data->parameters.vehicle_width);
       const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) {
         return std::any_of(
           parking_lot_polygons.begin(), parking_lot_polygons.end(),
@@ -309,14 +342,6 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
         });
     });
     const bool is_in_lanes = std::invoke([&]() -> bool {
-      const auto drivable_lanes =
-        utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes);
-      const auto & dp = planner_data->drivable_area_expansion_parameters;
-      const auto expanded_lanes = utils::expandLanelets(
-        drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
-        dp.drivable_area_types_to_skip);
-      const auto combined_drivable = utils::combineDrivableLanes(
-        expanded_lanes, upstream_module_output.drivable_area_info.drivable_lanes);
       return !lane_departure_checker_.checkPathWillLeaveLane(
         utils::transformToLanelets(combined_drivable), pull_over_path.parking_path());
     });
@@ -324,6 +349,7 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
       continue;
     }
     bezier_paths.push_back(std::move(pull_over_path));
+    id++;
   }
 
   return bezier_paths;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp
index 31d8afffbdabe..acb6ee1926a83 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp
@@ -13,7 +13,9 @@
 // limitations under the License.
 
 #include <autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp>
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
 
+#include <algorithm>
 #include <utility>
 #include <vector>
 
@@ -78,11 +80,24 @@ std::optional<PullOverPath> PullOverPath::create(
   std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path);
   std::tie(parking_path_curvatures, parking_path_max_curvature) =
     calculateCurvaturesAndMax(parking_path);
+  const double parking_path_curvature_total_derivative = [&]() {
+    const auto parking_path_curvature_base = autoware::motion_utils::calcSignedArcLengthPartialSum(
+      parking_path.points, 1, parking_path.points.size() - 1);
+    const auto size = std::min(parking_path_curvatures.size(), parking_path_curvature_base.size());
+    double total_abs_kappa_diff = 0;
+    for (unsigned i = 0; i + 1 < size; ++i) {
+      total_abs_kappa_diff += std::fabs(
+        (parking_path_curvatures.at(i + 1) - parking_path_curvatures.at(i)) /
+        (parking_path_curvature_base.at(i + 1) - parking_path_curvature_base.at(i)));
+    }
+    return total_abs_kappa_diff;
+  }();
 
   return PullOverPath(
     type, id, start_pose, modified_goal_pose, partial_paths, full_path, parking_path,
     full_path_curvatures, parking_path_curvatures, full_path_max_curvature,
-    parking_path_max_curvature, pairs_terminal_velocity_and_accel);
+    parking_path_max_curvature, parking_path_curvature_total_derivative,
+    pairs_terminal_velocity_and_accel);
 }
 
 PullOverPath::PullOverPath(const PullOverPath & other)
@@ -108,7 +123,7 @@ PullOverPath::PullOverPath(
   const PathWithLaneId & full_path, const PathWithLaneId & parking_path,
   const std::vector<double> & full_path_curvatures,
   const std::vector<double> & parking_path_curvatures, const double full_path_max_curvature,
-  const double parking_path_max_curvature,
+  const double parking_path_max_curvature, const double parking_path_curvature_total_derivative,
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel)
 : type_(type),
   modified_goal_pose_(modified_goal_pose),
@@ -121,6 +136,7 @@ PullOverPath::PullOverPath(
   parking_path_curvatures_(parking_path_curvatures),
   full_path_max_curvature_(full_path_max_curvature),
   parking_path_max_curvature_(parking_path_max_curvature),
+  parking_path_curvature_total_derivative_(parking_path_curvature_total_derivative),
   path_idx_(0),
   pairs_terminal_velocity_and_accel_(pairs_terminal_velocity_and_accel)
 {
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp
index 89458c199c130..c334fec7a1bd9 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp
@@ -22,7 +22,8 @@ namespace autoware::behavior_path_planner
 void LaneParkingRequest::update(
   const PlannerData & planner_data, const ModuleStatus & current_status,
   const BehaviorModuleOutput & upstream_module_output,
-  const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data)
+  const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data,
+  const bool trigger_thread_on_approach)
 {
   planner_data_ = std::make_shared<PlannerData>(planner_data);
   planner_data_->route_handler = std::make_shared<RouteHandler>(*(planner_data.route_handler));
@@ -30,6 +31,7 @@ void LaneParkingRequest::update(
   upstream_module_output_ = upstream_module_output;
   pull_over_path_ = pull_over_path;
   prev_data_ = prev_data;
+  trigger_thread_on_approach_ = trigger_thread_on_approach;
 }
 
 void FreespaceParkingRequest::initializeOccupancyGridMap(