diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml
index e9db0d140d947..3ebf73dab7ba5 100644
--- a/.github/workflows/build-and-test-differential.yaml
+++ b/.github/workflows/build-and-test-differential.yaml
@@ -24,7 +24,6 @@ jobs:
         rosdistro:
           - humble
         container-suffix:
-          - ""
           - -cuda
         include:
           - rosdistro: humble
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp
index 04bcfbacbbc17..d986d0f23fc85 100644
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp
@@ -18,6 +18,7 @@
 #include <boost/geometry/core/cs.hpp>
 #include <boost/geometry/geometries/geometries.hpp>
 #include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/geometries/register/ring.hpp>
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
@@ -98,5 +99,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(                                       // NOLIN
   tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y())       // NOLINT
 BOOST_GEOMETRY_REGISTER_POINT_3D(                                       // NOLINT
   tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z())  // NOLINT
+BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d)        // NOLINT
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_
diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp
index dc55576640133..be2411cd3268b 100644
--- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp
+++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp
@@ -29,14 +29,20 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <boost/geometry/algorithms/envelope.hpp>
+#include <boost/geometry/algorithms/union.hpp>
 #include <boost/geometry/index/rtree.hpp>
-#include <boost/optional.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Polygon.h>
 
 #include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace lane_departure_checker
@@ -112,6 +118,19 @@ class LaneDepartureChecker
   bool checkPathWillLeaveLane(
     const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
 
+  std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  bool checkPathWillLeaveLane(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  PathWithLaneId cropPointsOutsideOfLanes(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
+    const size_t end_index);
+
   static bool isOutOfLane(
     const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint);
 
diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
index 5bd0fcace9909..811e1652fcb4a 100644
--- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
+++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
@@ -34,6 +34,7 @@ using motion_utils::calcArcLength;
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::LineString2d;
 using tier4_autoware_utils::MultiPoint2d;
+using tier4_autoware_utils::MultiPolygon2d;
 using tier4_autoware_utils::Point2d;
 
 namespace
@@ -92,6 +93,7 @@ lanelet::ConstLanelets getCandidateLanelets(
 
   return candidate_lanelets;
 }
+
 }  // namespace
 
 namespace lane_departure_checker
@@ -298,6 +300,92 @@ bool LaneDepartureChecker::willLeaveLane(
   return false;
 }
 
+std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  // Get Footprint Hull basic polygon
+  std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);
+  auto to_basic_polygon = [](const LinearRing2d & footprint_hull) -> lanelet::BasicPolygon2d {
+    lanelet::BasicPolygon2d basic_polygon;
+    for (const auto & point : footprint_hull) {
+      Eigen::Vector2d p(point.x(), point.y());
+      basic_polygon.push_back(p);
+    }
+    return basic_polygon;
+  };
+  lanelet::BasicPolygon2d footprint_hull_basic_polygon = to_basic_polygon(footprint_hull);
+
+  // Find all lanelets that intersect the footprint hull
+  return lanelet::geometry::findWithin2d(
+    lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
+}
+
+std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
+  // Fuse lanelets into a single BasicPolygon2d
+  auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
+    if (lanelets_distance_pair.empty()) return std::nullopt;
+    if (lanelets_distance_pair.size() == 1)
+      return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+
+    lanelet::BasicPolygon2d merged_polygon =
+      lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+    for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
+      const auto & route_lanelet = lanelets_distance_pair.at(i).second;
+      const auto & poly = route_lanelet.polygon2d().basicPolygon();
+
+      std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
+      boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
+
+      // Update merged_polygon by accumulating all merged results
+      merged_polygon.clear();
+      for (const auto & temp_poly : lanelet_union_temp) {
+        merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
+      }
+    }
+    if (merged_polygon.empty()) return std::nullopt;
+    return merged_polygon;
+  }();
+
+  return fused_lanelets;
+}
+
+bool LaneDepartureChecker::checkPathWillLeaveLane(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  // check if the footprint is not fully contained within the fused lanelets polygon
+  const std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
+  if (!fused_lanelets_polygon) return true;
+  return !std::all_of(
+    vehicle_footprints.begin(), vehicle_footprints.end(),
+    [&fused_lanelets_polygon](const auto & footprint) {
+      return boost::geometry::within(footprint, fused_lanelets_polygon.value());
+    });
+}
+
+PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
+{
+  PathWithLaneId temp_path;
+  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
+  if (path.points.empty() || !fused_lanelets_polygon) return temp_path;
+  const auto vehicle_footprints = createVehicleFootprints(path);
+  size_t idx = 0;
+  std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
+    if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
+      temp_path.points.push_back(path.points.at(idx));
+    }
+    ++idx;
+  });
+  PathWithLaneId cropped_path = path;
+  cropped_path.points = temp_path.points;
+  return cropped_path;
+}
+
 bool LaneDepartureChecker::isOutOfLane(
   const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint)
 {
@@ -364,4 +452,5 @@ bool LaneDepartureChecker::willCrossBoundary(
   }
   return false;
 }
+
 }  // namespace lane_departure_checker
diff --git a/planning/.pages b/planning/.pages
index 4be0dbfeeaae0..1b5608f7ded57 100644
--- a/planning/.pages
+++ b/planning/.pages
@@ -77,8 +77,8 @@ nav:
   - 'Additional Tools':
     - 'RTC Replayer': planning/rtc_replayer
     - 'Planning Debug Tools':
-      - ['About Planning Debug Tools'](https://github.com/autowarefoundation/autoware_tools/tree/main/planning_debug_tools)
-      - ['Stop Reason Visualizer'](https://github.com/autowarefoundation/autoware_tools/tree/main/planning_debug_tools/doc-stop-reason-visualizer)
+      - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools
+      - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md
     - 'Planning Test Utils': planning/planning_test_utils
     - 'Planning Topic Converter': planning/planning_topic_converter
     - 'Planning Validator': planning/planning_validator
diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml
index ab4eda81f93c6..cd2a51851fe5f 100644
--- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml
+++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml
@@ -208,6 +208,11 @@
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
           max_deviation_from_lane: 0.5                  # [m]
+          # approve the next shift line after reaching this percentage of the current shift line length.
+          # this parameter should be within range of 0.0 to 1.0.
+          # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear.
+          # this feature can be disabled by setting this parameter to 1.0. (in that case, avoidance module creates return shift as soon as possible.)
+          ratio_for_return_shift_approval: 0.5          # [-]
         # avoidance distance parameters
         longitudinal:
           min_prepare_time: 1.0                         # [s]
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
index 223db73e72a4c..e4df3a8069b16 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
@@ -276,6 +276,9 @@ struct AvoidanceParameters
   // use for judge if the ego is shifting or not.
   double lateral_avoid_check_threshold{0.0};
 
+  // use for return shift approval.
+  double ratio_for_return_shift_approval{0.0};
+
   // For shift line generation process. The continuous shift length is quantized by this value.
   double quantize_filter_threshold{0.0};
 
@@ -539,6 +542,8 @@ struct AvoidancePlanningData
 
   bool valid{false};
 
+  bool ready{false};
+
   bool success{false};
 
   bool comfortable{false};
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
index cc00bfbb978f5..4f15261f42246 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
@@ -270,6 +270,33 @@ class AvoidanceHelper
     });
   }
 
+  bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const
+  {
+    if (std::abs(current_shift_length) < 1e-3) {
+      return true;
+    }
+
+    if (new_shift_lines.empty()) {
+      return true;
+    }
+
+    const auto front_shift_relative_length = new_shift_lines.front().getRelativeLength();
+
+    // same direction shift.
+    if (current_shift_length > 0.0 && front_shift_relative_length > 0.0) {
+      return true;
+    }
+
+    // same direction shift.
+    if (current_shift_length < 0.0 && front_shift_relative_length < 0.0) {
+      return true;
+    }
+
+    // keep waiting the other side shift approval until the ego reaches shift length threshold.
+    const auto ego_shift_ratio = (current_shift_length - getEgoShift()) / current_shift_length;
+    return ego_shift_ratio < parameters_->ratio_for_return_shift_approval + 1e-3;
+  }
+
   bool isShifted() const
   {
     return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold;
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
index fcaad191b1a33..ce75fa03e96ad 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
@@ -258,6 +258,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.max_left_shift_length = getOrDeclareParameter<double>(*node, ns + "max_left_shift_length");
     p.max_deviation_from_lane =
       getOrDeclareParameter<double>(*node, ns + "max_deviation_from_lane");
+    p.ratio_for_return_shift_approval =
+      getOrDeclareParameter<double>(*node, ns + "ratio_for_return_shift_approval");
+    if (p.ratio_for_return_shift_approval < 0.0 || p.ratio_for_return_shift_approval > 1.0) {
+      throw std::domain_error(
+        "ratio_for_return_shift_approval should be within range of 0.0 to 1.0");
+    }
   }
 
   // avoidance maneuver (longitudinal)
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
index 27cd15cc525df..ed3dc8d34a698 100644
--- a/planning/behavior_path_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -133,7 +133,7 @@ bool AvoidanceModule::isExecutionRequested() const
 bool AvoidanceModule::isExecutionReady() const
 {
   DEBUG_PRINT("AVOIDANCE isExecutionReady");
-  return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid;
+  return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready;
 }
 
 bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const
@@ -513,6 +513,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
    */
   data.comfortable = helper_->isComfortable(data.new_shift_line);
   data.safe = isSafePath(data.candidate_path, debug);
+  data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength());
 }
 
 void AvoidanceModule::fillEgoStatus(
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp
index 26c0f247eb558..1489dd1beff07 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp
@@ -18,6 +18,8 @@
 #include "behavior_path_planner_common/data_manager.hpp"
 #include "behavior_path_planner_common/parameters.hpp"
 
+#include <lane_departure_checker/lane_departure_checker.hpp>
+
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -72,7 +74,8 @@ class GeometricParallelParking
     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 & shoulder_lanes, const bool left_side_start,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
   void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
   void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
   {
@@ -115,7 +118,8 @@ class GeometricParallelParking
     const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_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 double lane_departure_margin, const double arc_path_interval,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
   PathWithLaneId generateArcPath(
     const Pose & center, const double radius, const double start_yaw, double end_yaw,
     const double arc_path_interval, const bool is_left_turn, const bool is_forward);
diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml
index db5dc51150e55..2dd4d12c05487 100644
--- a/planning/behavior_path_planner_common/package.xml
+++ b/planning/behavior_path_planner_common/package.xml
@@ -49,6 +49,7 @@
   <depend>freespace_planning_algorithms</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
+  <depend>lane_departure_checker</depend>
   <depend>lanelet2_extension</depend>
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>
diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
index a1a079e679e72..9400abd20b984 100644
--- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
+++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp
@@ -116,7 +116,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
                                               : 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,
-    end_pose_offset, lane_departure_margin, arc_path_interval);
+    end_pose_offset, lane_departure_margin, arc_path_interval, {});
   if (arc_paths.empty()) {
     return std::vector<PathWithLaneId>{};
   }
@@ -222,7 +222,8 @@ 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 & shoulder_lanes, const bool left_side_start,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 {
   constexpr bool is_forward = false;         // parking backward means pull_out forward
   constexpr double start_pose_offset = 0.0;  // start_pose is current_pose
@@ -242,7 +243,7 @@ bool GeometricParallelParking::planPullOut(
     auto arc_paths = planOneTrial(
       *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start,
       start_pose_offset, parameters_.pull_out_lane_departure_margin,
-      parameters_.pull_out_path_interval);
+      parameters_.pull_out_path_interval, lane_departure_checker);
     if (arc_paths.empty()) {
       // not found path
       continue;
@@ -362,7 +363,8 @@ 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 bool is_forward, const bool left_side_parking, const double end_pose_offset,
-  const double lane_departure_margin, const double arc_path_interval)
+  const double lane_departure_margin, const double arc_path_interval,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 {
   clearPaths();
 
@@ -496,6 +498,24 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   setLaneIdsToPath(path_turn_first);
   setLaneIdsToPath(path_turn_second);
 
+  if (lane_departure_checker) {
+    const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
+
+    const bool is_path_turn_first_outside_lanes =
+      lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_first);
+
+    if (is_path_turn_first_outside_lanes) {
+      return std::vector<PathWithLaneId>{};
+    }
+
+    const bool is_path_turn_second_outside_lanes =
+      lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_second);
+
+    if (is_path_turn_second_outside_lanes) {
+      return std::vector<PathWithLaneId>{};
+    }
+  }
+
   // generate arc path vector
   paths_.push_back(path_turn_first);
   paths_.push_back(path_turn_second);
diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp
index 164868b47535e..68a5d8100a271 100644
--- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp
+++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp
@@ -19,20 +19,27 @@
 #include "behavior_path_start_planner_module/pull_out_path.hpp"
 #include "behavior_path_start_planner_module/pull_out_planner_base.hpp"
 
+#include <lane_departure_checker/lane_departure_checker.hpp>
+
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <memory>
+
 namespace behavior_path_planner
 {
 class GeometricPullOut : public PullOutPlannerBase
 {
 public:
-  explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters);
+  explicit GeometricPullOut(
+    rclcpp::Node & node, const StartPlannerParameters & parameters,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
 
   PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; };
   std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   GeometricParallelParking planner_;
   ParallelParkingParameters parallel_parking_parameters_;
+  std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker_;
 };
 }  // namespace behavior_path_planner
 
diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp
index 6bf85a4679dfd..2d50ae189dc13 100644
--- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp
+++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp
@@ -50,11 +50,6 @@ class ShiftPullOut : public PullOutPlannerBase
     ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose,
     const double longitudinal_acc, const double lateral_acc);
 
-  void setDepartureCheckLanes(const lanelet::ConstLanelets & departure_check_lanes)
-  {
-    departure_check_lanes_ = departure_check_lanes;
-  }
-
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
 
 private:
@@ -63,8 +58,6 @@ class ShiftPullOut : public PullOutPlannerBase
   double calcPullOutLongitudinalDistance(
     const double lon_acc, const double shift_time, const double shift_length,
     const double max_curvature, const double min_distance) const;
-
-  lanelet::ConstLanelets departure_check_lanes_;
 };
 }  // namespace behavior_path_planner
 
diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
index 067bbd5d74aee..aaf711b855e7a 100644
--- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
+++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
@@ -275,7 +275,6 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
   bool isSafePath() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
-  void updateDepartureCheckLanes();
   lanelet::ConstLanelets createDepartureCheckLanes() const;
 
   // check if the goal is located behind the ego in the same route segment.
diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp
index d9ff155f36e6c..a5aeb5b257d39 100644
--- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp
+++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp
@@ -30,9 +30,12 @@ namespace behavior_path_planner
 {
 using start_planner_utils::getPullOutLanes;
 
-GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters)
+GeometricPullOut::GeometricPullOut(
+  rclcpp::Node & node, const StartPlannerParameters & parameters,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 : PullOutPlannerBase{node, parameters},
-  parallel_parking_parameters_{parameters.parallel_parking_parameters}
+  parallel_parking_parameters_{parameters.parallel_parking_parameters},
+  lane_departure_checker_(lane_departure_checker)
 {
   planner_.setParameters(parallel_parking_parameters_);
 }
@@ -55,8 +58,8 @@ std::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, const
   planner_.setTurningRadius(
     planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle);
   planner_.setPlannerData(planner_data_);
-  const bool found_valid_path =
-    planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start);
+  const bool found_valid_path = planner_.planPullOut(
+    start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_);
   if (!found_valid_path) {
     return {};
   }
diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp
index 8eca8479fbd44..d21a68048e34f 100644
--- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp
+++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp
@@ -60,9 +60,8 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
 
   // get safe path
   for (auto & pull_out_path : pull_out_paths) {
-    auto & shift_path =
-      pull_out_path.partial_paths.front();  // shift path is not separate but only one.
-
+    // shift path is not separate but only one.
+    auto & shift_path = pull_out_path.partial_paths.front();
     // check lane_departure with path between pull_out_start to pull_out_end
     PathWithLaneId path_shift_start_to_end{};
     {
@@ -75,44 +74,30 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
         shift_path.points.begin() + pull_out_end_idx + 1);
     }
 
-    // crop backward path
-    // removes points which are out of lanes up to the start pose.
-    // this ensures that the backward_path stays within the drivable area when starting from a
-    // narrow place.
-    const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
-      shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold,
-      common_parameters.ego_nearest_yaw_threshold);
-    PathWithLaneId cropped_path{};
-    for (size_t i = 0; i < shift_path.points.size(); ++i) {
-      const Pose pose = shift_path.points.at(i).point.pose;
-      const auto transformed_vehicle_footprint =
-        transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose));
-      const bool is_out_of_lane =
-        LaneDepartureChecker::isOutOfLane(departure_check_lanes_, transformed_vehicle_footprint);
-      if (i <= start_segment_idx) {
-        if (!is_out_of_lane) {
-          cropped_path.points.push_back(shift_path.points.at(i));
-        }
-      } else {
-        cropped_path.points.push_back(shift_path.points.at(i));
-      }
-    }
-    shift_path.points = cropped_path.points;
-
     // check lane departure
     // The method for lane departure checking verifies if the footprint of each point on the path is
     // contained within a lanelet using `boost::geometry::within`, which incurs a high computational
     // cost.
-    // TODO(someone): improve the method for detecting lane departures without using
-    // lanelet::ConstLanelets, making it unnecessary to retain departure_check_lanes_ as a member
-    // variable.
+    const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
     if (
       parameters_.check_shift_path_lane_departure &&
-      lane_departure_checker_->checkPathWillLeaveLane(
-        departure_check_lanes_, path_shift_start_to_end)) {
+      lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) {
       continue;
     }
 
+    // crop backward path
+    // removes points which are out of lanes up to the start pose.
+    // this ensures that the backward_path stays within the drivable area when starting from a
+    // narrow place.
+
+    const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+      shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold,
+      common_parameters.ego_nearest_yaw_threshold);
+
+    const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes(
+      lanelet_map_ptr, shift_path, start_segment_idx);
+
+    shift_path.points = cropped_path.points;
     shift_path.header = planner_data_->route_handler->getRouteHeader();
 
     return pull_out_path;
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
index 2171373cd85ff..ed81f39135181 100644
--- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -69,7 +69,8 @@ StartPlannerModule::StartPlannerModule(
       std::make_shared<ShiftPullOut>(node, *parameters, lane_departure_checker_));
   }
   if (parameters_->enable_geometric_pull_out) {
-    start_planners_.push_back(std::make_shared<GeometricPullOut>(node, *parameters));
+    start_planners_.push_back(
+      std::make_shared<GeometricPullOut>(node, *parameters, lane_departure_checker_));
   }
   if (start_planners_.empty()) {
     RCLCPP_ERROR(getLogger(), "Not found enabled planner");
@@ -142,7 +143,6 @@ void StartPlannerModule::initVariables()
   info_marker_.markers.clear();
   initializeSafetyCheckParameters();
   initializeCollisionCheckDebugMap(debug_data_.collision_check);
-  updateDepartureCheckLanes();
 }
 
 void StartPlannerModule::updateEgoPredictedPathParams(
@@ -608,7 +608,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
 void StartPlannerModule::resetStatus()
 {
   status_ = PullOutStatus{};
-  updateDepartureCheckLanes();
 }
 
 void StartPlannerModule::incrementPathIndex()
@@ -1409,22 +1408,6 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
   }
 }
 
-void StartPlannerModule::updateDepartureCheckLanes()
-{
-  const auto departure_check_lanes = createDepartureCheckLanes();
-  for (auto & planner : start_planners_) {
-    auto shift_pull_out = std::dynamic_pointer_cast<ShiftPullOut>(planner);
-
-    if (shift_pull_out) {
-      shift_pull_out->setDepartureCheckLanes(departure_check_lanes);
-    }
-  }
-  // debug
-  {
-    debug_data_.departure_check_lanes = departure_check_lanes;
-  }
-}
-
 lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const
 {
   const double backward_path_length =
diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
index 42ca694235a00..9dcc8b1a03715 100644
--- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
+++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
@@ -22,13 +22,14 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 
 ### Core Parameters
 
-| Name                      | Type    | Default Value | Description                                                                         |
-| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- |
-| `distance_ratio`          | double  | 1.03          |                                                                                     |
-| `object_length_threshold` | double  | 0.1           |                                                                                     |
-| `num_points_threshold`    | int     | 4             |                                                                                     |
-| `max_rings_num`           | uint_16 | 128           |                                                                                     |
-| `max_points_num_per_ring` | size_t  | 4000          | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` |
+| Name                      | Type    | Default Value | Description                                                                                                                     |
+| ------------------------- | ------- | ------------- | ------------------------------------------------------------------------------------------------------------------------------- |
+| `distance_ratio`          | double  | 1.03          |                                                                                                                                 |
+| `object_length_threshold` | double  | 0.1           |                                                                                                                                 |
+| `num_points_threshold`    | int     | 4             |                                                                                                                                 |
+| `max_rings_num`           | uint_16 | 128           |                                                                                                                                 |
+| `max_points_num_per_ring` | size_t  | 4000          | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring`                                             |
+| `publish_excluded_points` | bool    | false         | Flag to publish excluded pointcloud for debugging purpose. Due to performance concerns, please set to false during experiments. |
 
 ## Assumptions / Known limits
 
diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
index ab7d4e0012304..57ed7a508ca54 100644
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
+++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
@@ -21,6 +21,7 @@
 
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
+#include <memory>
 #include <utility>
 #include <vector>
 
@@ -42,11 +43,15 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
     const TransformInfo & transform_info);
 
 private:
+  /** \brief publisher of excluded pointcloud for debug reason. **/
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr excluded_points_publisher_;
+
   double distance_ratio_;
   double object_length_threshold_;
   int num_points_threshold_;
   uint16_t max_rings_num_;
   size_t max_points_num_per_ring_;
+  bool publish_excluded_points_;
 
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
@@ -68,6 +73,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
 
     return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_;
   }
+  PointCloud2 extractExcludedPoints(
+    const PointCloud2 & input, const PointCloud2 & output, float epsilon);
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW
diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml
index e1a396c69b3e9..936c52ef81b37 100644
--- a/sensing/pointcloud_preprocessor/package.xml
+++ b/sensing/pointcloud_preprocessor/package.xml
@@ -18,6 +18,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_auto_geometry</depend>
   <depend>autoware_point_types</depend>
   <depend>cgal</depend>
   <depend>cv_bridge</depend>
diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
index d968b06a0dc61..e0c8443cfe57b 100644
--- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
+++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
@@ -14,8 +14,12 @@
 
 #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp"
 
+#include "autoware_auto_geometry/common_3d.hpp"
+
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
+#include <pcl/search/pcl_search.h>
+
 #include <algorithm>
 #include <vector>
 namespace pointcloud_preprocessor
@@ -29,6 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
     using tier4_autoware_utils::StopWatch;
     stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
     debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
+    excluded_points_publisher_ =
+      this->create_publisher<sensor_msgs::msg::PointCloud2>("debug/ring_outlier_filter", 1);
     stop_watch_ptr_->tic("cyclic_time");
     stop_watch_ptr_->tic("processing_time");
   }
@@ -42,6 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
     max_rings_num_ = static_cast<uint16_t>(declare_parameter("max_rings_num", 128));
     max_points_num_per_ring_ =
       static_cast<size_t>(declare_parameter("max_points_num_per_ring", 4000));
+    publish_excluded_points_ =
+      static_cast<bool>(declare_parameter("publish_excluded_points", false));
   }
 
   using std::placeholders::_1;
@@ -196,6 +204,17 @@ void RingOutlierFilterComponent::faster_filter(
     sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
     "intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
 
+  if (publish_excluded_points_) {
+    auto excluded_points = extractExcludedPoints(*input, output, 0.01);
+    // set fields
+    sensor_msgs::PointCloud2Modifier excluded_pcd_modifier(excluded_points);
+    excluded_pcd_modifier.setPointCloud2Fields(
+      num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
+      sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
+      "intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
+    excluded_points_publisher_->publish(excluded_points);
+  }
+
   // add processing time for debug
   if (debug_publisher_) {
     const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
@@ -241,6 +260,10 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
   if (get_param(p, "num_points_threshold", num_points_threshold_)) {
     RCLCPP_DEBUG(get_logger(), "Setting new num_points_threshold to: %d.", num_points_threshold_);
   }
+  if (get_param(p, "publish_excluded_points", publish_excluded_points_)) {
+    RCLCPP_DEBUG(
+      get_logger(), "Setting new publish_excluded_points to: %d.", publish_excluded_points_);
+  }
 
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
@@ -248,6 +271,54 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
 
   return result;
 }
+
+sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints(
+  const sensor_msgs::msg::PointCloud2 & input, const sensor_msgs::msg::PointCloud2 & output,
+  float epsilon)
+{
+  // Convert ROS PointCloud2 message to PCL point cloud for easier manipulation
+  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(input, *input_cloud);
+  pcl::fromROSMsg(output, *output_cloud);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr excluded_points(new pcl::PointCloud<pcl::PointXYZ>);
+
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree;
+  if (output_cloud->isOrganized()) {
+    tree.reset(new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
+  } else {
+    tree.reset(new pcl::search::KdTree<pcl::PointXYZ>(false));
+  }
+  tree->setInputCloud(output_cloud);
+  std::vector<int> nn_indices(1);
+  std::vector<float> nn_distances(1);
+  for (const auto & point : input_cloud->points) {
+    if (!tree->nearestKSearch(point, 1, nn_indices, nn_distances)) {
+      continue;
+    }
+    if (nn_distances[0] > epsilon) {
+      excluded_points->points.push_back(point);
+    }
+  }
+
+  sensor_msgs::msg::PointCloud2 excluded_points_msg;
+  pcl::toROSMsg(*excluded_points, excluded_points_msg);
+
+  // Set the metadata for the excluded points message based on the input cloud
+  excluded_points_msg.height = 1;
+  excluded_points_msg.width =
+    static_cast<uint32_t>(output.data.size() / output.height / output.point_step);
+  excluded_points_msg.row_step = static_cast<uint32_t>(output.data.size() / output.height);
+  excluded_points_msg.is_bigendian = input.is_bigendian;
+  excluded_points_msg.is_dense = input.is_dense;
+  excluded_points_msg.header = input.header;
+  excluded_points_msg.header.frame_id =
+    !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_;
+
+  return excluded_points_msg;
+}
+
 }  // namespace pointcloud_preprocessor
 
 #include <rclcpp_components/register_node_macro.hpp>