diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md
index 5e93c86c9be1c..485ec4cb2a359 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md
@@ -107,7 +107,7 @@ If the target path contains a goal, modify the points of the path so that the pa
 - Route is set with `allow_goal_modification=true` .
   - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service.
   - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz.
-- ego-vehicle is in the same lane as the goal.
+- The terminal point of the current path is in the same lane sequence as the goal. If goal is on the road shoulder, then it is in the adjacent road lane sequence.
 
 <img src="https://user-images.githubusercontent.com/39142679/237929950-989ca6c3-d48c-4bb5-81e5-e8d6a38911aa.png" width="600">
 
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 4c23292f63e61..98ae9e2a941d3 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
@@ -266,6 +266,7 @@ struct GoalPlannerDebugData
   FreespacePlannerDebugData freespace_planner{};
   std::vector<Polygon2d> ego_polygons_expanded{};
   lanelet::ConstLanelet expanded_pull_over_lane_between_ego{};
+  Polygon2d objects_extraction_polygon{};
 };
 
 struct LastApprovalData
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
index 5d056add4665e..6bdad3a3063ef 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
@@ -31,13 +31,12 @@ class GoalSearcher : public GoalSearcherBase
 public:
   GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);
 
-  GoalCandidates search(
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) override;
+  GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) override;
   void update(
     GoalCandidates & goal_candidates,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const override;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const override;
 
   // todo(kosuke55): Functions for this specific use should not be in the interface,
   // so it is better to consider interface design when we implement other goal searchers.
@@ -47,7 +46,8 @@ class GoalSearcher : public GoalSearcherBase
   bool isSafeGoalWithMarginScaleFactor(
     const GoalCandidate & goal_candidate, const double margin_scale_factor,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const override;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const override;
 
 private:
   void countObjectsToAvoid(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp
index baead4c229efd..b7d6ffe0710fe 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp
@@ -53,13 +53,12 @@ class GoalSearcherBase
   const Pose & getReferenceGoal() const { return reference_goal_pose_; }
 
   MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
-  virtual GoalCandidates search(
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) = 0;
+  virtual GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) = 0;
   virtual void update(
     [[maybe_unused]] GoalCandidates & goal_candidates,
     [[maybe_unused]] const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data) const
+    [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data,
+    [[maybe_unused]] const PredictedObjects & objects) const
   {
     return;
   }
@@ -69,7 +68,8 @@ class GoalSearcherBase
   virtual bool isSafeGoalWithMarginScaleFactor(
     const GoalCandidate & goal_candidate, const double margin_scale_factor,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const = 0;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const = 0;
 
 protected:
   GoalPlannerParameters parameters_{};
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/shift_pull_over.hpp
index 15b8d7ac81fae..a7873680529f7 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/shift_pull_over.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/shift_pull_over.hpp
@@ -43,7 +43,7 @@ class ShiftPullOver : public PullOverPlannerBase
   std::optional<PathWithLaneId> cropPrevModulePath(
     const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const;
   std::optional<PullOverPath> generatePullOverPath(
-    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
     const Pose & goal_pose, const double lateral_jerk) const;
   static double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double after_shifted_arc_length, const double dr);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
index 3c717a90c93ef..c4491753cb0a3 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
@@ -62,6 +62,19 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
   const geometry_msgs::msg::Pose ego_pose,
   const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset,
   const double inner_road_offset);
+
+/*
+ * @brief generate polygon to extract objects
+ * @param pull_over_lanes pull over lanes
+ * @param left_side left side or right side
+ * @param outer_offset outer offset from pull over lane boundary
+ * @param inner_offset inner offset from pull over lane boundary
+ * @return polygon to extract objects
+ */
+std::optional<Polygon2d> generateObjectExtractionPolygon(
+  const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, const double outer_offset,
+  const double inner_offset);
+
 PredictedObjects extractObjectsInExpandedPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance, double bound_offset, const PredictedObjects & objects);
@@ -82,12 +95,31 @@ bool isReferencePath(
 std::optional<PathWithLaneId> cropPath(const PathWithLaneId & path, const Pose & end_pose);
 PathWithLaneId cropForwardPoints(
   const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length);
+
+/**
+ * @brief extend target_path by extend_length
+ * @param target_path original target path to extend
+ * @param reference_path reference path to extend
+ * @param extend_length length to extend
+ * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of
+ *                                       target_path has zero velocity
+ * @return extended path
+ */
 PathWithLaneId extendPath(
-  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
-  const double extend_length);
+  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
+  const double extend_length, const bool remove_connected_zero_velocity);
+/**
+ * @brief extend target_path to extend_pose
+ * @param target_path original target path to extend
+ * @param reference_path reference path to extend
+ * @param extend_pose pose to extend
+ * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of
+ *                                       target_path has zero velocity
+ * @return extended path
+ */
 PathWithLaneId extendPath(
-  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
-  const Pose & extend_pose);
+  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
+  const Pose & extend_pose, const bool remove_connected_zero_velocity);
 
 std::vector<Polygon2d> createPathFootPrints(
   const PathWithLaneId & path, const double base_to_front, const double base_to_rear,
@@ -109,6 +141,23 @@ MarkerArray createLaneletPolygonMarkerArray(
 MarkerArray createNumObjectsToAvoidTextsMarkerArray(
   const GoalCandidates & goal_candidates, std::string && ns,
   const std_msgs::msg::ColorRGBA & color);
+
+/**
+ * @brief combine two points
+ * @param points lane points
+ * @param points_next next lane points
+ * @return combined points
+ */
+lanelet::Points3d combineLanePoints(
+  const lanelet::Points3d & points, const lanelet::Points3d & points_next);
+/** @brief Create a lanelet that represents the departure check area.
+ * @param [in] pull_over_lanes Lanelets that the vehicle will pull over to.
+ * @param [in] route_handler RouteHandler object.
+ * @return Lanelet that goal footprints should be inside.
+ */
+lanelet::Lanelet createDepartureCheckLanelet(
+  const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler,
+  const bool left_side_parking);
 }  // namespace autoware::behavior_path_planner::goal_planner_utils
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp
index 62c51408a000b..ee28b64d5bac3 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp
@@ -51,7 +51,6 @@ std::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
   if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }
-  const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes);
 
   const auto & p = parallel_parking_parameters_;
   const double max_steer_angle =
@@ -65,10 +64,12 @@ std::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
     return {};
   }
 
+  const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
+    pull_over_lanes, *planner_data_->route_handler, left_side_parking_);
   const auto arc_path = planner_.getArcPath();
 
   // check lane departure with road and shoulder lanes
-  if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {};
+  if (lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, arc_path)) return {};
 
   PullOverPath pull_over_path{};
   pull_over_path.type = getPlannerType();
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 cfe5c797d7130..92aa83ede9f4e 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
@@ -283,6 +283,10 @@ void GoalPlannerModule::onTimer()
       RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path");
       return true;
     }
+    // TODO(someone): The generated path inherits the velocity of the path of the previous module.
+    // Therefore, if the velocity of the path of the previous module changes (e.g. stop points are
+    // inserted, deleted), the path should be regenerated.
+
     return false;
   });
   if (!need_update) {
@@ -455,18 +459,33 @@ void GoalPlannerModule::updateData()
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
-  // extract static and dynamic objects in expanded pull over lanes
+  // extract static and dynamic objects in extraction polygon for path coliision check
   {
     const auto & p = parameters_;
     const auto & rh = *(planner_data_->route_handler);
     const auto objects = *(planner_data_->dynamic_object);
-    const auto static_target_objects =
-      goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
-        rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
-        p->detection_bound_offset, objects, p->th_moving_object_velocity);
-    const auto dynamic_target_objects = goal_planner_utils::extractObjectsInExpandedPullOverLanes(
-      rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
-      p->detection_bound_offset, objects);
+    const double vehicle_width = planner_data_->parameters.vehicle_width;
+    const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+      rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length);
+    const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
+      pull_over_lanes, left_side_parking_, p->detection_bound_offset,
+      p->margin_from_boundary + p->max_lateral_offset + vehicle_width);
+    if (objects_extraction_polygon.has_value()) {
+      debug_data_.objects_extraction_polygon = objects_extraction_polygon.value();
+    }
+    PredictedObjects dynamic_target_objects{};
+    for (const auto & object : objects.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 static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
+      dynamic_target_objects, p->th_moving_object_velocity);
+
+    // these objects are used for path collision check not for safety check
     thread_safe_data_.set_static_target_objects(static_target_objects);
     thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects);
   }
@@ -577,15 +596,25 @@ bool GoalPlannerModule::isExecutionRequested() const
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
   const Pose goal_pose = route_handler->getOriginalGoalPose();
 
-  // check if goal_pose is in current_lanes.
-  lanelet::ConstLanelet current_lane{};
-  // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
+  // check if goal_pose is in current_lanes or neighboring road lanes
   const lanelet::ConstLanelets current_lanes =
     utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
-  lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &current_lane);
-  const bool goal_is_in_current_lanes = std::any_of(
-    current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) {
-      return lanelet::utils::isInLanelet(goal_pose, current_lane);
+  const auto getNeighboringLane =
+    [&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
+    return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true)
+                              : route_handler->getRightLanelet(lane, false, true);
+  };
+  lanelet::ConstLanelets goal_check_lanes = current_lanes;
+  for (const auto & lane : current_lanes) {
+    auto neighboring_lane = getNeighboringLane(lane);
+    while (neighboring_lane) {
+      goal_check_lanes.push_back(neighboring_lane.value());
+      neighboring_lane = getNeighboringLane(neighboring_lane.value());
+    }
+  }
+  const bool goal_is_in_current_segment_lanes = std::any_of(
+    goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) {
+      return lanelet::utils::isInLanelet(goal_pose, lane);
     });
 
   // check that goal is in current neighbor shoulder lane
@@ -602,7 +631,7 @@ bool GoalPlannerModule::isExecutionRequested() const
 
   // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner,
   // because goal arc coordinates cannot be calculated.
-  if (!goal_is_in_current_lanes && !goal_is_in_current_shoulder_lanes) {
+  if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) {
     return false;
   }
 
@@ -610,7 +639,7 @@ bool GoalPlannerModule::isExecutionRequested() const
   // 1) goal_pose is in current_lanes, plan path to the original fixed goal
   // 2) goal_pose is NOT in current_lanes, do not execute goal_planner
   if (!utils::isAllowedGoalModification(route_handler)) {
-    return goal_is_in_current_lanes;
+    return goal_is_in_current_segment_lanes;
   }
 
   // if goal arc coordinates can be calculated, check if goal is in request_length
@@ -632,7 +661,12 @@ bool GoalPlannerModule::isExecutionRequested() const
     parameters_->forward_goal_search_length);
   lanelet::ConstLanelet target_lane{};
   lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane);
-  if (!isCrossingPossible(current_lane, target_lane)) {
+
+  lanelet::ConstLanelet previous_module_terminal_lane{};
+  route_handler->getClosestLaneletWithinRoute(
+    getPreviousModuleOutput().path.points.back().point.pose, &previous_module_terminal_lane);
+
+  if (!isCrossingPossible(previous_module_terminal_lane, target_lane)) {
     return false;
   }
 
@@ -735,7 +769,9 @@ bool GoalPlannerModule::planFreespacePath(
 {
   GoalCandidates goal_candidates{};
   goal_candidates = thread_safe_data_.get_goal_candidates();
-  goal_searcher->update(goal_candidates, occupancy_grid_map, planner_data);
+  goal_searcher->update(
+    goal_candidates, occupancy_grid_map, planner_data,
+    thread_safe_data_.get_static_target_objects());
   thread_safe_data_.set_goal_candidates(goal_candidates);
   debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size();
   debug_data_.freespace_planner.is_planning = true;
@@ -819,7 +855,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const
   // calculate goal candidates
   const auto & route_handler = planner_data_->route_handler;
   if (utils::isAllowedGoalModification(route_handler)) {
-    return goal_searcher_->search(occupancy_grid_map_, planner_data_);
+    return goal_searcher_->search(planner_data_);
   }
 
   // NOTE:
@@ -1313,9 +1349,9 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus(
     // check goal pose collision
     const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose();
     if (
-      modified_goal_opt &&
-      !goal_searcher->isSafeGoalWithMarginScaleFactor(
-        modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data)) {
+      modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor(
+                             modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map,
+                             planner_data, thread_safe_data_.get_static_target_objects())) {
       RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe");
       return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
     }
@@ -1456,6 +1492,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
     return getPreviousModuleOutput();
   }
 
+  const bool is_freespace =
+    thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE;
   if (
     hasNotDecidedPath(
       planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_,
@@ -1471,7 +1509,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
 
     // update goal candidates
     auto goal_candidates = thread_safe_data_.get_goal_candidates();
-    goal_searcher_->update(goal_candidates, occupancy_grid_map_, planner_data_);
+    goal_searcher_->update(
+      goal_candidates, occupancy_grid_map_, planner_data_,
+      thread_safe_data_.get_static_target_objects());
 
     // update pull over path candidates
     const auto pull_over_path_candidates = sortPullOverPathCandidatesByGoalPriority(
@@ -1502,7 +1542,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
 
   // return to lane parking if it is possible
   if (
-    thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
+    is_freespace && thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
     canReturnToLaneParking()) {
     thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());
   }
@@ -1666,8 +1706,12 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
     const double s_end = s_current + common_parameters.forward_path_length;
     return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true);
   });
+
+  // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it by
+  // setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is the
+  // role of the goal planner, and the intermediate zero velocity after extension is unnecessary.
   const auto extended_prev_path = goal_planner_utils::extendPath(
-    getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length);
+    getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length, true);
 
   // calculate search start offset pose from the closest goal candidate pose with
   // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible
@@ -2247,14 +2291,10 @@ bool GoalPlannerModule::isCrossingPossible(
     end_lane_sequence = route_handler->getLaneletSequence(end_lane, dist, dist, false);
   }
 
-  // Lambda function to get the neighboring lanelet based on left_side_parking_
-  auto getNeighboringLane =
+  const auto getNeighboringLane =
     [&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
-    const auto neighboring_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane)
-                                                     : route_handler->getRightShoulderLanelet(lane);
-    if (neighboring_lane) return neighboring_lane;
-    return left_side_parking_ ? route_handler->getLeftLanelet(lane)
-                              : route_handler->getRightLanelet(lane);
+    return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true)
+                              : route_handler->getRightLanelet(lane, false, true);
   };
 
   // Iterate through start_lane_sequence to find a path to end_lane_sequence
@@ -2544,6 +2584,24 @@ void GoalPlannerModule::setDebugData()
     // Visualize goal candidates
     const auto goal_candidates = thread_safe_data_.get_goal_candidates();
     add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color));
+
+    // Visualize objects extraction polygon
+    auto marker = autoware::universe_utils::createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "objects_extraction_polygon", 0, Marker::LINE_LIST,
+      autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0),
+      autoware::universe_utils::createMarkerColor(0.0, 1.0, 1.0, 0.999));
+    const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
+    for (size_t i = 0; i < debug_data_.objects_extraction_polygon.outer().size(); ++i) {
+      const auto & current_point = debug_data_.objects_extraction_polygon.outer().at(i);
+      const auto & next_point = debug_data_.objects_extraction_polygon.outer().at(
+        (i + 1) % debug_data_.objects_extraction_polygon.outer().size());
+      marker.points.push_back(
+        autoware::universe_utils::createPoint(current_point.x(), current_point.y(), ego_z));
+      marker.points.push_back(
+        autoware::universe_utils::createPoint(next_point.x(), next_point.y(), ego_z));
+    }
+
+    debug_marker_.markers.push_back(marker);
   }
 
   // Visualize previous module output
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 04ee5278912ac..4f0a18e01d034 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
@@ -97,9 +97,7 @@ GoalSearcher::GoalSearcher(
 {
 }
 
-GoalCandidates GoalSearcher::search(
-  const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const std::shared_ptr<const PlannerData> & planner_data)
+GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & planner_data)
 {
   GoalCandidates goal_candidates{};
 
@@ -117,11 +115,8 @@ GoalCandidates GoalSearcher::search(
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, parameters_.backward_goal_search_length,
     parameters_.forward_goal_search_length);
-  auto lanes = utils::getExtendedCurrentLanes(
-    planner_data, backward_length, forward_length,
-    /*forward_only_in_route*/ false);
-  lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
-
+  const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
+    pull_over_lanes, *route_handler, left_side_parking_);
   const auto goal_arc_coords =
     lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_);
   const double s_start = std::max(0.0, goal_arc_coords.length - backward_length);
@@ -179,7 +174,8 @@ GoalCandidates GoalSearcher::search(
         break;
       }
 
-      if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) {
+      if (!boost::geometry::within(
+            transformed_vehicle_footprint, departure_check_lane.polygon2d().basicPolygon())) {
         continue;
       }
 
@@ -195,8 +191,6 @@ GoalCandidates GoalSearcher::search(
   }
   createAreaPolygons(original_search_poses, planner_data);
 
-  update(goal_candidates, occupancy_grid_map, planner_data);
-
   return goal_candidates;
 }
 
@@ -268,16 +262,10 @@ void GoalSearcher::countObjectsToAvoid(
 void GoalSearcher::update(
   GoalCandidates & goal_candidates,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const std::shared_ptr<const PlannerData> & planner_data) const
+  const std::shared_ptr<const PlannerData> & planner_data, const PredictedObjects & objects) const
 {
-  const auto pull_over_lane_stop_objects =
-    goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
-      *(planner_data->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
-      parameters_.forward_goal_search_length, parameters_.detection_bound_offset,
-      *(planner_data->dynamic_object), parameters_.th_moving_object_velocity);
-
   if (parameters_.prioritize_goals_before_objects) {
-    countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects, planner_data);
+    countObjectsToAvoid(goal_candidates, objects, planner_data);
   }
 
   if (parameters_.goal_priority == "minimum_weighted_distance") {
@@ -297,7 +285,7 @@ void GoalSearcher::update(
     const Pose goal_pose = goal_candidate.goal_pose;
 
     // check collision with footprint
-    if (checkCollision(goal_pose, pull_over_lane_stop_objects, occupancy_grid_map)) {
+    if (checkCollision(goal_pose, objects, occupancy_grid_map)) {
       goal_candidate.is_safe = false;
       continue;
     }
@@ -305,7 +293,7 @@ void GoalSearcher::update(
     // check longitudinal margin with pull over lane objects
     constexpr bool filter_inside = true;
     const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance(
-      goal_pose, planner_data->parameters.vehicle_width, pull_over_lane_stop_objects,
+      goal_pose, planner_data->parameters.vehicle_width, objects,
       parameters_.object_recognition_collision_check_hard_margins.back(), filter_inside);
     if (checkCollisionWithLongitudinalDistance(
           goal_pose, target_objects, occupancy_grid_map, planner_data)) {
@@ -323,33 +311,25 @@ void GoalSearcher::update(
 bool GoalSearcher::isSafeGoalWithMarginScaleFactor(
   const GoalCandidate & goal_candidate, const double margin_scale_factor,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const std::shared_ptr<const PlannerData> & planner_data) const
+  const std::shared_ptr<const PlannerData> & planner_data, const PredictedObjects & objects) const
 {
   if (!parameters_.use_object_recognition) {
     return true;
   }
 
   const Pose goal_pose = goal_candidate.goal_pose;
-
-  const auto pull_over_lane_stop_objects =
-    goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
-      *(planner_data->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
-      parameters_.forward_goal_search_length, parameters_.detection_bound_offset,
-      *(planner_data->dynamic_object), parameters_.th_moving_object_velocity);
-
   const double margin =
     parameters_.object_recognition_collision_check_hard_margins.back() * margin_scale_factor;
 
   if (utils::checkCollisionBetweenFootprintAndObjects(
-        vehicle_footprint_, goal_pose, pull_over_lane_stop_objects, margin)) {
+        vehicle_footprint_, goal_pose, objects, margin)) {
     return false;
   }
 
   // check longitudinal margin with pull over lane objects
   constexpr bool filter_inside = true;
   const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance(
-    goal_pose, planner_data->parameters.vehicle_width, pull_over_lane_stop_objects, margin,
-    filter_inside);
+    goal_pose, planner_data->parameters.vehicle_width, objects, margin, filter_inside);
   if (checkCollisionWithLongitudinalDistance(
         goal_pose, target_objects, occupancy_grid_map, planner_data)) {
     return false;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp
index ba1ebfade0127..4038194f4072f 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp
@@ -123,7 +123,7 @@ std::optional<PathWithLaneId> ShiftPullOver::cropPrevModulePath(
 }
 
 std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
-  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
   const Pose & goal_pose, const double lateral_jerk) const
 {
   const double pull_over_velocity = parameters_.pull_over_velocity;
@@ -148,8 +148,12 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
       lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length >
       lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length;
     if (extend_previous_module_path) {  // case1
+      // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it
+      // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is
+      // the role of the goal planner, and the intermediate zero velocity after extension is
+      // unnecessary.
       return goal_planner_utils::extendPath(
-        prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose);
+        prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose, true);
     } else {  // case2
       return goal_planner_utils::cropPath(prev_module_path, shift_end_pose);
     }
@@ -212,7 +216,7 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
     p.point.longitudinal_velocity_mps = 0.0;
     p.point.pose = goal_pose;
     p.lane_ids = shifted_path.path.points.back().lane_ids;
-    for (const auto & lane : shoulder_lanes) {
+    for (const auto & lane : pull_over_lanes) {
       p.lane_ids.push_back(lane.id());
     }
     shifted_path.path.points.push_back(p);
@@ -234,7 +238,7 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
       }
     }
     // add shoulder lane_id if not found
-    for (const auto & lane : shoulder_lanes) {
+    for (const auto & lane : pull_over_lanes) {
       if (
         std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) ==
         point.lane_ids.end()) {
@@ -279,18 +283,12 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
         return is_footprint_in_any_polygon(footprint);
       });
   });
-  const bool is_in_lanes = std::invoke([&]() -> bool {
-    const auto drivable_lanes =
-      utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_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, previous_module_output_.drivable_area_info.drivable_lanes);
-    return !lane_departure_checker_.checkPathWillLeaveLane(
-      utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath());
-  });
+
+  const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
+    pull_over_lanes, *planner_data_->route_handler, left_side_parking_);
+  const bool is_in_lanes = !lane_departure_checker_.checkPathWillLeaveLane(
+    {departure_check_lane}, pull_over_path.getParkingPath());
+
   if (!is_in_parking_lots && !is_in_lanes) {
     return {};
   }
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
index 4526c584afd12..0907118b4bd03 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
@@ -38,6 +38,7 @@
 namespace autoware::behavior_path_planner::goal_planner_utils
 {
 
+using autoware::universe_utils::calcDistance2d;
 using autoware::universe_utils::calcOffsetPose;
 using autoware::universe_utils::createDefaultMarker;
 using autoware::universe_utils::createMarkerColor;
@@ -137,6 +138,126 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
                        -outer_road_offset);
 }
 
+std::optional<Polygon2d> generateObjectExtractionPolygon(
+  const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, const double outer_offset,
+  const double inner_offset)
+{
+  // generate base boundary poses without orientation
+  std::vector<Pose> base_boundary_poses{};
+  for (const auto & lane : pull_over_lanes) {
+    const auto & bound = left_side ? lane.leftBound() : lane.rightBound();
+    for (const auto & p : bound) {
+      Pose pose{};
+      pose.position = createPoint(p.x(), p.y(), p.z());
+      if (std::any_of(base_boundary_poses.begin(), base_boundary_poses.end(), [&](const auto & p) {
+            return calcDistance2d(p.position, pose.position) < 0.1;
+          })) {
+        continue;
+      }
+      base_boundary_poses.push_back(pose);
+    }
+  }
+  if (base_boundary_poses.size() < 2) {
+    return std::nullopt;
+  }
+
+  // set orientation to next point
+  for (auto it = base_boundary_poses.begin(); it != std::prev(base_boundary_poses.end()); ++it) {
+    const auto & p = it->position;
+    const auto & next_p = std::next(it)->position;
+    const double yaw = autoware::universe_utils::calcAzimuthAngle(p, next_p);
+    it->orientation = autoware::universe_utils::createQuaternionFromYaw(yaw);
+  }
+  base_boundary_poses.back().orientation =
+    base_boundary_poses[base_boundary_poses.size() - 2].orientation;
+
+  // generate outer and inner boundary poses
+  std::vector<Point> outer_boundary_points{};
+  std::vector<Point> inner_boundary_points{};
+  const double outer_direction_sign = left_side ? 1.0 : -1.0;
+  for (const auto & base_pose : base_boundary_poses) {
+    const Pose outer_pose = calcOffsetPose(base_pose, 0, outer_direction_sign * outer_offset, 0);
+    const Pose inner_pose = calcOffsetPose(base_pose, 0, -outer_direction_sign * inner_offset, 0);
+    outer_boundary_points.push_back(outer_pose.position);
+    inner_boundary_points.push_back(inner_pose.position);
+  }
+
+  // remove self intersection
+  // if bound is intersected, remove them and insert intersection point
+  using BoostPoint = boost::geometry::model::d2::point_xy<double>;
+  using LineString = boost::geometry::model::linestring<BoostPoint>;
+  const auto remove_self_intersection = [](const std::vector<Point> & bound) {
+    constexpr double INTERSECTION_CHECK_DISTANCE = 10.0;
+    std::vector<Point> modified_bound{};
+    size_t i = 0;
+    while (i < bound.size() - 2) {
+      BoostPoint p1(bound.at(i).x, bound.at(i).y);
+      BoostPoint p2(bound.at(i + 1).x, bound.at(i + 1).y);
+      LineString p_line{};
+      p_line.push_back(p1);
+      p_line.push_back(p2);
+      bool intersection_found = false;
+      for (size_t j = i + 2; j < bound.size() - 1; j++) {
+        const double distance = autoware::universe_utils::calcDistance2d(bound.at(i), bound.at(j));
+        if (distance > INTERSECTION_CHECK_DISTANCE) {
+          break;
+        }
+        LineString q_line{};
+        BoostPoint q1(bound.at(j).x, bound.at(j).y);
+        BoostPoint q2(bound.at(j + 1).x, bound.at(j + 1).y);
+        q_line.push_back(q1);
+        q_line.push_back(q2);
+        std::vector<BoostPoint> intersection_points;
+        boost::geometry::intersection(p_line, q_line, intersection_points);
+        if (intersection_points.empty()) {
+          continue;
+        }
+        modified_bound.push_back(bound.at(i));
+        Point intersection_point{};
+        intersection_point.x = intersection_points.at(0).x();
+        intersection_point.y = intersection_points.at(0).y();
+        intersection_point.z = bound.at(i).z;
+        modified_bound.push_back(intersection_point);
+        i = j + 1;
+        intersection_found = true;
+        break;
+      }
+      if (!intersection_found) {
+        modified_bound.push_back(bound.at(i));
+        i++;
+      }
+    }
+    modified_bound.push_back(bound.back());
+    return modified_bound;
+  };
+  outer_boundary_points = remove_self_intersection(outer_boundary_points);
+  inner_boundary_points = remove_self_intersection(inner_boundary_points);
+
+  // create clockwise polygon
+  Polygon2d polygon{};
+  const auto & left_boundary_points = left_side ? outer_boundary_points : inner_boundary_points;
+  const auto & right_boundary_points = left_side ? inner_boundary_points : outer_boundary_points;
+  std::vector<Point> reversed_right_boundary_points = right_boundary_points;
+  std::reverse(reversed_right_boundary_points.begin(), reversed_right_boundary_points.end());
+  for (const auto & left_point : left_boundary_points) {
+    autoware::universe_utils::Point2d point{left_point.x, left_point.y};
+    polygon.outer().push_back(point);
+  }
+  for (const auto & right_point : reversed_right_boundary_points) {
+    autoware::universe_utils::Point2d point{right_point.x, right_point.y};
+    polygon.outer().push_back(point);
+  }
+  autoware::universe_utils::Point2d first_point{
+    left_boundary_points.front().x, left_boundary_points.front().y};
+  polygon.outer().push_back(first_point);
+
+  if (polygon.outer().size() < 3) {
+    return std::nullopt;
+  }
+
+  return polygon;
+}
+
 PredictedObjects extractObjectsInExpandedPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance, double bound_offset, const PredictedObjects & objects)
@@ -388,7 +509,7 @@ PathWithLaneId cropForwardPoints(
 
 PathWithLaneId extendPath(
   const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
-  const double extend_length)
+  const double extend_length, const bool remove_connected_zero_velocity)
 {
   const auto & target_terminal_pose = target_path.points.back().point.pose;
 
@@ -408,6 +529,11 @@ PathWithLaneId extendPath(
   }
 
   auto extended_path = target_path;
+  auto & target_terminal_vel = extended_path.points.back().point.longitudinal_velocity_mps;
+  if (remove_connected_zero_velocity && target_terminal_vel < 0.01) {
+    target_terminal_vel = clipped_path.points.front().point.longitudinal_velocity_mps;
+  }
+
   const auto start_point =
     std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) {
       const bool is_forward =
@@ -426,7 +552,7 @@ PathWithLaneId extendPath(
 
 PathWithLaneId extendPath(
   const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
-  const Pose & extend_pose)
+  const Pose & extend_pose, const bool remove_connected_zero_velocity)
 {
   const auto & target_terminal_pose = target_path.points.back().point.pose;
   const size_t target_path_terminal_idx = autoware::motion_utils::findNearestSegmentIndex(
@@ -434,7 +560,7 @@ PathWithLaneId extendPath(
   const double extend_distance = autoware::motion_utils::calcSignedArcLength(
     reference_path.points, target_path_terminal_idx, extend_pose.position);
 
-  return extendPath(target_path, reference_path, extend_distance);
+  return extendPath(target_path, reference_path, extend_distance, remove_connected_zero_velocity);
 }
 
 std::vector<Polygon2d> createPathFootPrints(
@@ -450,4 +576,59 @@ std::vector<Polygon2d> createPathFootPrints(
   return footprints;
 }
 
+lanelet::Points3d combineLanePoints(
+  const lanelet::Points3d & points, const lanelet::Points3d & points_next)
+{
+  lanelet::Points3d combined_points;
+  std::unordered_set<lanelet::Id> point_ids;
+  for (const auto & point : points) {
+    if (point_ids.insert(point.id()).second) {
+      combined_points.push_back(point);
+    }
+  }
+  for (const auto & point : points_next) {
+    if (point_ids.insert(point.id()).second) {
+      combined_points.push_back(point);
+    }
+  }
+  return combined_points;
+}
+
+lanelet::Lanelet createDepartureCheckLanelet(
+  const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler,
+  const bool left_side_parking)
+{
+  const auto getBoundPoints =
+    [&](const lanelet::ConstLanelet & lane, const bool is_outer) -> lanelet::Points3d {
+    lanelet::Points3d points;
+    const auto & bound = left_side_parking ? (is_outer ? lane.leftBound() : lane.rightBound())
+                                           : (is_outer ? lane.rightBound() : lane.leftBound());
+    for (const auto & pt : bound) {
+      points.push_back(lanelet::Point3d(pt));
+    }
+    return points;
+  };
+
+  const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet {
+    return left_side_parking ? route_handler.getMostRightLanelet(lane, false, true)
+                             : route_handler.getMostLeftLanelet(lane, false, true);
+  };
+
+  lanelet::Points3d outer_bound_points{};
+  lanelet::Points3d inner_bound_points{};
+  for (const auto & lane : pull_over_lanes) {
+    const auto current_outer_bound_points = getBoundPoints(lane, true);
+    const auto most_inner_lane = getMostInnerLane(lane);
+    const auto current_inner_bound_points = getBoundPoints(most_inner_lane, false);
+    outer_bound_points = combineLanePoints(outer_bound_points, current_outer_bound_points);
+    inner_bound_points = combineLanePoints(inner_bound_points, current_inner_bound_points);
+  }
+
+  const auto outer_linestring = lanelet::LineString3d(lanelet::InvalId, outer_bound_points);
+  const auto inner_linestring = lanelet::LineString3d(lanelet::InvalId, inner_bound_points);
+  return lanelet::Lanelet(
+    lanelet::InvalId, left_side_parking ? outer_linestring : inner_linestring,
+    left_side_parking ? inner_linestring : outer_linestring);
+}
+
 }  // namespace autoware::behavior_path_planner::goal_planner_utils
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
index a270900b491c3..717e8d322f05a 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -141,17 +141,6 @@ std::vector<DrivableLanes> generateDrivableLanes(
 
 double getLateralShift(const LaneChangePath & path);
 
-bool hasEnoughLengthToLaneChangeAfterAbort(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const Pose & curent_pose, const double abort_return_dist,
-  const LaneChangeParameters & lane_change_parameters, const Direction direction);
-
-double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
-
-double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);
-
-std::string getStrDirection(const std::string & name, const Direction direction);
-
 CandidateOutput assignToCandidate(
   const LaneChangePath & lane_change_path, const Point & ego_position);
 std::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
index 28efae2549d1e..0beec0909c034 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -2034,9 +2034,21 @@ bool NormalLaneChange::calcAbortPath()
     return false;
   }
 
-  if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort(
-        route_handler, reference_lanelets, current_pose, abort_return_dist,
-        *lane_change_parameters_, direction)) {
+  const auto dist_to_terminal_start = std::invoke([&]() {
+    const auto dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end(
+      common_data_ptr_, get_current_lanes(), common_data_ptr_->get_ego_pose());
+    const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength(
+      route_handler, get_current_lanes().back(), *common_data_ptr_->lc_param_ptr,
+      common_data_ptr_->direction);
+    return dist_to_terminal_end - min_lc_length;
+  });
+
+  const auto enough_abort_dist =
+    abort_start_dist + abort_return_dist +
+      calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <=
+    dist_to_terminal_start;
+
+  if (!enough_abort_dist) {
     RCLCPP_ERROR(logger_, "insufficient distance to abort.");
     return false;
   }
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
index 71e6b0bf6530e..8bcd6a97d19f5 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
@@ -644,47 +644,6 @@ double getLateralShift(const LaneChangePath & path)
   return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx);
 }
 
-bool hasEnoughLengthToLaneChangeAfterAbort(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const Pose & current_pose, const double abort_return_dist,
-  const LaneChangeParameters & lane_change_parameters, const Direction direction)
-{
-  if (current_lanes.empty()) {
-    return false;
-  }
-
-  const auto minimum_lane_change_length = calcMinimumLaneChangeLength(
-    route_handler, current_lanes.back(), lane_change_parameters, direction);
-  const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length;
-  if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) {
-    return false;
-  }
-
-  if (
-    abort_plus_lane_change_length >
-    utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) {
-    return false;
-  }
-
-  return true;
-}
-
-double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer)
-{
-  return lateral_buffer + 0.5 * vehicle_width;
-}
-
-std::string getStrDirection(const std::string & name, const Direction direction)
-{
-  if (direction == Direction::LEFT) {
-    return name + "_left";
-  }
-  if (direction == Direction::RIGHT) {
-    return name + "_right";
-  }
-  return "";
-}
-
 std::vector<std::vector<int64_t>> getSortedLaneIds(
   const RouteHandler & route_handler, const Pose & current_pose,
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes)
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp
index 518d1108f0ee1..9cc9bec8e963a 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp
@@ -70,11 +70,11 @@ class GeometricParallelParking
   bool isParking() const;
   bool planPullOver(
     const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward,
+    const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward,
     const bool left_side_parking);
   bool planPullOut(
     const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
+    const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start,
     const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
       autoware_lane_departure_checker);
   void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
@@ -117,7 +117,7 @@ class GeometricParallelParking
   void clearPaths();
   std::vector<PathWithLaneId> planOneTrial(
     const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
-    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
     const bool is_forward, const bool left_side_parking, const double end_pose_offset,
     const double lane_departure_margin, const double arc_path_interval,
     const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
@@ -134,7 +134,7 @@ class GeometricParallelParking
     const bool left_side_parking);
   std::vector<PathWithLaneId> generatePullOverPaths(
     const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
-    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
     const bool is_forward, const bool left_side_parking, const double end_pose_offset,
     const double velocity);
   PathWithLaneId generateStraightPath(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
index 104d0a345be3a..a878b578842f4 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
@@ -107,7 +107,7 @@ void GeometricParallelParking::setVelocityToArcPaths(
 
 std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
-  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
   const bool is_forward, const bool left_side_parking, const double end_pose_offset,
   const double velocity)
 {
@@ -117,7 +117,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval
                                               : parameters_.backward_parking_path_interval;
   auto arc_paths = planOneTrial(
-    start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking,
+    start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking,
     end_pose_offset, lane_departure_margin, arc_path_interval, {});
   if (arc_paths.empty()) {
     return std::vector<PathWithLaneId>{};
@@ -158,7 +158,7 @@ void GeometricParallelParking::clearPaths()
 
 bool GeometricParallelParking::planPullOver(
   const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward,
+  const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward,
   const bool left_side_parking)
 {
   const auto & common_params = planner_data_->parameters;
@@ -188,7 +188,7 @@ bool GeometricParallelParking::planPullOver(
       }
 
       const auto paths = generatePullOverPaths(
-        *start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking,
+        *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking,
         end_pose_offset, parameters_.forward_parking_velocity);
       if (!paths.empty()) {
         paths_ = paths;
@@ -210,8 +210,8 @@ bool GeometricParallelParking::planPullOver(
       }
 
       const auto paths = generatePullOverPaths(
-        *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_parking,
-        end_pose_offset, parameters_.backward_parking_velocity);
+        *start_pose, goal_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward,
+        left_side_parking, end_pose_offset, parameters_.backward_parking_velocity);
       if (!paths.empty()) {
         paths_ = paths;
         return true;
@@ -224,7 +224,7 @@ bool GeometricParallelParking::planPullOver(
 
 bool GeometricParallelParking::planPullOut(
   const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
+  const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start,
   const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
     lane_departure_checker)
 {
@@ -244,7 +244,7 @@ bool GeometricParallelParking::planPullOut(
 
     // plan reverse path of parking. end_pose <-> start_pose
     auto arc_paths = planOneTrial(
-      *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start,
+      *end_pose, start_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward, left_side_start,
       start_pose_offset, parameters_.pull_out_lane_departure_margin,
       parameters_.pull_out_arc_path_interval, lane_departure_checker);
     if (arc_paths.empty()) {
@@ -376,7 +376,7 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(
 
 std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
-  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
   const bool is_forward, const bool left_side_parking, const double end_pose_offset,
   const double lane_departure_margin, const double arc_path_interval,
   const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
@@ -421,7 +421,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
     }
     lanes.push_back(lane);
   }
-  lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end());
+  lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
 
   // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle,
   // and detect lane departure.
@@ -429,7 +429,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
     const double R_front_near =
       std::hypot(R_E_far + common_params.vehicle_width / 2, common_params.base_link2front);
     const double distance_to_near_bound =
-      utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, left_side_parking);
+      utils::getSignedDistanceFromBoundary(pull_over_lanes, arc_end_pose, left_side_parking);
     const double near_deviation = R_front_near - R_E_far;
     if (std::abs(distance_to_near_bound) - near_deviation < lane_departure_margin) {
       return std::vector<PathWithLaneId>{};