diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt
index bf48e8fcaca3f..454e0e7ef044f 100644
--- a/common/autoware_auto_geometry/CMakeLists.txt
+++ b/common/autoware_auto_geometry/CMakeLists.txt
@@ -12,12 +12,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/bounding_box.cpp
 )
 
-if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
-  target_compile_definitions(${PROJECT_NAME} PRIVATE
-    DEFINE_LEGACY_FUNCTION
-  )
-endif()
-
 if(BUILD_TESTING)
   set(GEOMETRY_GTEST geometry_gtest)
   set(GEOMETRY_SRC test/src/test_geometry.cpp
diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt
index fe4cec1c0fc89..a8ae9ec2d962e 100755
--- a/common/autoware_auto_tf2/CMakeLists.txt
+++ b/common/autoware_auto_tf2/CMakeLists.txt
@@ -17,11 +17,6 @@ if(BUILD_TESTING)
     "tf2_geometry_msgs"
     "tf2_ros"
   )
-  if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
-    target_compile_definitions(test_tf2_autoware_auto_msgs PRIVATE
-      DEFINE_LEGACY_FUNCTION
-    )
-  endif()
 endif()
 
 ament_auto_package()
diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
index ee8d52fa402c0..b35eb6e93ce6e 100644
--- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
+++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
@@ -45,57 +45,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
 
 namespace tf2
 {
-#ifdef DEFINE_LEGACY_FUNCTION
-/*************/
-/** Point32 **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The point to transform, as a Point32 message.
- * \param t_out The transformed point, as a Point32 message.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
-  const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
-  t_out.x = static_cast<float>(v_out[0]);
-  t_out.y = static_cast<float>(v_out[1]);
-  t_out.z = static_cast<float>(v_out[2]);
-}
-
-/*************/
-/** Polygon **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The polygon to transform.
- * \param t_out The transformed polygon.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
-  const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  // Don't call the Point32 doTransform to avoid doing this conversion every time
-  const auto kdl_frame = gmTransformToKDL(transform);
-  // We don't use std::back_inserter to allow aliasing between t_in and t_out
-  t_out.points.resize(t_in.points.size());
-  for (size_t i = 0; i < t_in.points.size(); ++i) {
-    const KDL::Vector v_out =
-      kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
-    t_out.points[i].x = static_cast<float>(v_out[0]);
-    t_out.points[i].y = static_cast<float>(v_out[1]);
-    t_out.points[i].z = static_cast<float>(v_out[2]);
-  }
-}
-#endif
-
 /******************/
 /** Quaternion32 **/
 /******************/
diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
index efae65ae48813..0352096d02b2b 100644
--- a/planning/behavior_path_planner/CMakeLists.txt
+++ b/planning/behavior_path_planner/CMakeLists.txt
@@ -57,12 +57,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/marker_utils/lane_change/debug.cpp
 )
 
-if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
-  target_compile_definitions(behavior_path_planner_node PRIVATE
-    DEFINE_LEGACY_FUNCTION
-  )
-endif()
-
 target_include_directories(behavior_path_planner_node SYSTEM PUBLIC
   ${EIGEN3_INCLUDE_DIR}
 )
diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
index 102ed5b8c5fd1..81bcd383f0a49 100644
--- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
@@ -26,6 +26,15 @@
 
 #include <boost/geometry.hpp>
 
+// for writing the svg file
+#include <fstream>
+#include <iostream>
+// for the geometry types
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+// for the svg mapper
+#include <boost/geometry/io/svg/svg_mapper.hpp>
+#include <boost/geometry/io/svg/write.hpp>
+
 namespace drivable_area_expansion
 {
 
@@ -57,6 +66,22 @@ void expandDrivableArea(
 {
   if (path.points.empty() || path.left_bound.empty() || path.right_bound.empty()) return;
 
+  // Declare a stream and an SVG mapper
+  std::ofstream svg1("/home/mclement/Pictures/DA_before.svg");  // /!\ CHANGE PATH
+  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> before(svg1, 400, 400);
+  // Declare a stream and an SVG mapper
+  std::ofstream svg2("/home/mclement/Pictures/DA_after.svg");  // /!\ CHANGE PATH
+  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> after(svg2, 400, 400);
+
+  linestring_t left_ls;
+  linestring_t right_ls;
+  for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
+  for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
+  before.add(left_ls);
+  before.add(right_ls);
+  before.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+  before.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stopwatch;
   const auto & params = planner_data->drivable_area_expansion_parameters;
   const auto & dynamic_objects = *planner_data->dynamic_object;
@@ -126,6 +151,15 @@ void expandDrivableArea(
     std::printf("\tupdate: %2.2fms\n", update_duration);
     std::cout << std::endl;
   }
+
+  left_ls.clear();
+  right_ls.clear();
+  for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
+  for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
+  after.add(left_ls);
+  after.add(right_ls);
+  after.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
+  after.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
 }
 
 point_t convert_point(const Point & p)
diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml
index c74c5b3d87c1d..092f1d32b27b3 100644
--- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml
+++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml
@@ -17,6 +17,7 @@
         use_predicted_paths: true  # if true, use the predicted paths to estimate future positions.
                                    # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in.
         predicted_path_min_confidence : 0.1  # when using predicted paths, ignore the ones whose confidence is lower than this value.
+        distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane
 
       overlap:
         minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered
@@ -26,6 +27,7 @@
         skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
         precision: 0.1  # [m] precision when inserting a stop pose in the path
         distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        min_duration: 1.0  # [s] minimum duration needed before a decision can be canceled
         slowdown:
           distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
           velocity: 2.0  # [m/s] slowdown velocity
diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp
index 6f3094aea9682..b9d9f6f98d53a 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp
@@ -26,6 +26,11 @@
 namespace behavior_velocity_planner::out_of_lane
 {
 
+/// @brief estimate whether ego can decelerate without breaking the deceleration limit
+/// @details assume ego wants to reach the target point at the target velocity
+/// @param [in] ego_data ego data
+/// @param [in] point target point
+/// @param [in] target_vel target_velocity
 bool can_decelerate(
   const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel)
 {
@@ -37,9 +42,16 @@ bool can_decelerate(
   return acc_to_target_vel < std::abs(ego_data.max_decel);
 }
 
+/// @brief calculate the last pose along the path where ego does not overlap the lane to avoid
+/// @param [in] ego_data ego data
+/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed)
+/// @param [in] footprint the ego footprint
+/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits
+/// @param [in] params parameters
+/// @return the last pose that is not out of lane (if found)
 std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
   const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint,
-  const PlannerParam & params)
+  const std::optional<SlowdownToInsert> & prev_slowdown_point, const PlannerParam & params)
 {
   const auto from_arc_length =
     motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx);
@@ -50,12 +62,17 @@ std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
     // TODO(Maxime): binary search
     interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l);
     const auto respect_decel_limit =
-      !params.skip_if_over_max_decel ||
+      !params.skip_if_over_max_decel || prev_slowdown_point ||
       can_decelerate(ego_data, interpolated_point, decision.velocity);
-    if (
-      respect_decel_limit && !boost::geometry::overlaps(
-                               project_to_pose(footprint, interpolated_point.point.pose),
-                               decision.lane_to_avoid.polygon2d().basicPolygon()))
+    const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.point.pose);
+    const auto is_overlap_lane = boost::geometry::overlaps(
+      interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon());
+    const auto is_overlap_extra_lane =
+      prev_slowdown_point &&
+      boost::geometry::overlaps(
+        interpolated_footprint,
+        prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon());
+    if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane)
       return interpolated_point;
   }
   return std::nullopt;
@@ -64,10 +81,12 @@ std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
 /// @brief calculate the slowdown point to insert in the path
 /// @param ego_data ego data (path, velocity, etc)
 /// @param decisions decision (before which point to stop, what lane to avoid entering, etc)
+/// @param prev_slowdown_point previously calculated slowdown point
 /// @param params parameters
 /// @return optional slowdown point to insert in the path
 std::optional<SlowdownToInsert> calculate_slowdown_point(
-  const EgoData & ego_data, const std::vector<Slowdown> & decisions, PlannerParam params)
+  const EgoData & ego_data, const std::vector<Slowdown> & decisions,
+  const std::optional<SlowdownToInsert> prev_slowdown_point, PlannerParam params)
 {
   params.extra_front_offset += params.dist_buffer;
   const auto base_footprint = make_base_footprint(params);
@@ -75,7 +94,7 @@ std::optional<SlowdownToInsert> calculate_slowdown_point(
   // search for the first slowdown decision for which a stop point can be inserted
   for (const auto & decision : decisions) {
     const auto last_in_lane_pose =
-      calculate_last_in_lane_pose(ego_data, decision, base_footprint, params);
+      calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params);
     if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose};
   }
   return std::nullopt;
diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp
index 475e341d76528..43377ec744eb5 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp
@@ -32,13 +32,12 @@ visualization_msgs::msg::Marker get_base_marker()
   base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
   base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
   base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5);
-  base_marker.lifetime = rclcpp::Duration::from_seconds(0.5);
   return base_marker;
 }
 }  // namespace
 void add_footprint_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
-  const lanelet::BasicPolygons2d & footprints, const double z)
+  const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = "footprints";
@@ -52,12 +51,14 @@ void add_footprint_markers(
     debug_marker.id++;
     debug_marker.points.clear();
   }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 void add_current_overlap_marker(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::BasicPolygon2d & current_footprint,
-  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z)
+  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = "current_overlap";
@@ -80,12 +81,14 @@ void add_current_overlap_marker(
     debug_marker_array.markers.push_back(debug_marker);
     debug_marker.id++;
   }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 void add_lanelet_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::ConstLanelets & lanelets, const std::string & ns,
-  const std_msgs::msg::ColorRGBA & color)
+  const std_msgs::msg::ColorRGBA & color, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = ns;
@@ -101,6 +104,82 @@ void add_lanelet_markers(
     debug_marker_array.markers.push_back(debug_marker);
     debug_marker.id++;
   }
+  debug_marker.action = debug_marker.DELETE;
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
+}
+
+void add_range_markers(
+  visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx,
+  const double z, const size_t prev_nb)
+{
+  auto debug_marker = get_base_marker();
+  debug_marker.ns = "ranges";
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5);
+  for (const auto & range : ranges) {
+    debug_marker.points.clear();
+    debug_marker.points.push_back(
+      path.points[first_ego_idx + range.entering_path_idx].point.pose.position);
+    debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+      range.entering_point.x(), range.entering_point.y(), z));
+    for (const auto & overlap : range.debug.overlaps) {
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z));
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z));
+    }
+    debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+      range.exiting_point.x(), range.exiting_point.y(), z));
+    debug_marker.points.push_back(
+      path.points[first_ego_idx + range.exiting_path_idx].point.pose.position);
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.id++;
+  }
+  debug_marker.action = debug_marker.DELETE;
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
+  debug_marker.action = debug_marker.ADD;
+  debug_marker.id = 0;
+  debug_marker.ns = "decisions";
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0);
+  debug_marker.points.clear();
+  for (const auto & range : ranges) {
+    debug_marker.type = debug_marker.LINE_STRIP;
+    if (range.debug.decision) {
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        range.entering_point.x(), range.entering_point.y(), z));
+      debug_marker.points.push_back(
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position);
+    }
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.points.clear();
+    debug_marker.id++;
+
+    debug_marker.type = debug_marker.TEXT_VIEW_FACING;
+    debug_marker.pose.position.x = range.entering_point.x();
+    debug_marker.pose.position.y = range.entering_point.y();
+    debug_marker.pose.position.z = z;
+    std::stringstream ss;
+    ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time
+       << "\n";
+    if (range.debug.object) {
+      debug_marker.pose.position.x +=
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x;
+      debug_marker.pose.position.y +=
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y;
+      debug_marker.pose.position.x /= 2;
+      debug_marker.pose.position.y /= 2;
+      ss << "Obj: " << range.debug.times.object.enter_time << " - "
+         << range.debug.times.object.exit_time << "\n";
+    }
+    debug_marker.scale.z = 1.0;
+    debug_marker.text = ss.str();
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.id++;
+  }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 }  // namespace behavior_velocity_planner::out_of_lane::debug
diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp
index 06bc6d935f310..2b6b65638ec40 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp
@@ -29,27 +29,41 @@ namespace behavior_velocity_planner::out_of_lane::debug
 /// @param [inout] debug_marker_array marker array
 /// @param [in] footprints footprints to turn into markers
 /// @param [in] z z value to use for the markers
+/// @param [in] prev_nb previous number of markers (used to delete the extra ones)
 void add_footprint_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
-  const lanelet::BasicPolygons2d & footprints, const double z);
+  const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb);
 /// @brief add footprint markers to the given marker array
 /// @param [inout] debug_marker_array marker array
 /// @param [in] current_footprint footprint to turn into a marker
 /// @param [in] current_overlapped_lanelets lanelets to turn into markers
 /// @param [in] z z value to use for the markers
+/// @param [in] prev_nb previous number of markers (used to delete the extra ones)
 void add_current_overlap_marker(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::BasicPolygon2d & current_footprint,
-  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z);
+  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb);
 /// @brief add footprint markers to the given marker array
 /// @param [inout] debug_marker_array marker array
 /// @param [in] lanelets lanelets to turn into markers
 /// @param [in] ns namespace of the markers
 /// @param [in] color namespace of the markers
+/// @param [in] prev_nb previous number of markers (used to delete the extra ones)
 void add_lanelet_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::ConstLanelets & lanelets, const std::string & ns,
-  const std_msgs::msg::ColorRGBA & color);
+  const std_msgs::msg::ColorRGBA & color, const size_t prev_nb);
+/// @brief add ranges markers to the given marker array
+/// @param [inout] debug_marker_array marker array
+/// @param [in] ranges ranges to turn into markers
+/// @param [in] path modified ego path that was used to calculate the ranges
+/// @param [in] first_path_idx first idx of ego on the path
+/// @param [in] z z value to use for the markers
+/// @param [in] prev_nb previous number of markers (used to delete the extra ones)
+void add_range_markers(
+  visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx,
+  const double z, const size_t prev_nb);
 }  // namespace behavior_velocity_planner::out_of_lane::debug
 
 #endif  // DEBUG_HPP_
diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
index 3d8c6b7eb663b..80ac33849b4f2 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
@@ -17,7 +17,6 @@
 #include <lanelet2_extension/utility/utilities.hpp>
 
 #include <algorithm>
-#include <limits>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -55,7 +54,8 @@ bool object_is_incoming(
 
 std::optional<std::pair<double, double>> object_time_to_range(
   const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
-  const std::shared_ptr<route_handler::RouteHandler> route_handler, const rclcpp::Logger & logger)
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, const double dist_buffer,
+  const rclcpp::Logger & logger)
 {
   // skip the dynamic object if it is not in a lane preceding the overlapped lane
   // lane changes are intentionally not considered
@@ -64,23 +64,23 @@ std::optional<std::pair<double, double>> object_time_to_range(
     object.kinematics.initial_pose_with_covariance.pose.position.y);
   if (!object_is_incoming(object_point, route_handler, range.lane)) return {};
 
-  const auto max_deviation = object.shape.dimensions.y + range.inside_distance;
-
+  const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer;
   auto worst_enter_time = std::optional<double>();
   auto worst_exit_time = std::optional<double>();
 
   for (const auto & predicted_path : object.kinematics.predicted_paths) {
+    const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
     const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds();
     const auto enter_point =
       geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y());
     const auto enter_segment_idx =
-      motion_utils::findNearestSegmentIndex(predicted_path.path, enter_point);
+      motion_utils::findNearestSegmentIndex(unique_path_points, enter_point);
     const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment(
-      predicted_path.path, enter_segment_idx, enter_point);
-    const auto enter_lat_dist = std::abs(
-      motion_utils::calcLateralOffset(predicted_path.path, enter_point, enter_segment_idx));
+      unique_path_points, enter_segment_idx, enter_point);
+    const auto enter_lat_dist =
+      std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx));
     const auto enter_segment_length = tier4_autoware_utils::calcDistance2d(
-      predicted_path.path[enter_segment_idx], predicted_path.path[enter_segment_idx + 1]);
+      unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]);
     const auto enter_offset_ratio = enter_offset / enter_segment_length;
     const auto enter_time =
       static_cast<double>(enter_segment_idx) * time_step + enter_offset_ratio * time_step;
@@ -88,13 +88,13 @@ std::optional<std::pair<double, double>> object_time_to_range(
     const auto exit_point =
       geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y());
     const auto exit_segment_idx =
-      motion_utils::findNearestSegmentIndex(predicted_path.path, exit_point);
+      motion_utils::findNearestSegmentIndex(unique_path_points, exit_point);
     const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment(
-      predicted_path.path, exit_segment_idx, exit_point);
+      unique_path_points, exit_segment_idx, exit_point);
     const auto exit_lat_dist =
-      std::abs(motion_utils::calcLateralOffset(predicted_path.path, exit_point, exit_segment_idx));
+      std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx));
     const auto exit_segment_length = tier4_autoware_utils::calcDistance2d(
-      predicted_path.path[exit_segment_idx], predicted_path.path[exit_segment_idx + 1]);
+      unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]);
     const auto exit_offset_ratio = exit_offset / static_cast<double>(exit_segment_length);
     const auto exit_time =
       static_cast<double>(exit_segment_idx) * time_step + exit_offset_ratio * time_step;
@@ -299,7 +299,8 @@ bool should_not_enter(
     // skip objects that are already on the interval
     const auto enter_exit_time =
       params.objects_use_predicted_paths
-        ? object_time_to_range(object, range, inputs.route_handler, logger)
+        ? object_time_to_range(
+            object, range, inputs.route_handler, params.objects_dist_buffer, logger)
         : object_time_to_range(object, range, inputs, logger);
     if (!enter_exit_time) {
       RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n");
@@ -308,27 +309,14 @@ bool should_not_enter(
 
     range_times.object.enter_time = enter_exit_time->first;
     range_times.object.exit_time = enter_exit_time->second;
-    if (will_collide_on_range(range_times, params, logger)) return true;
-  }
-  return false;
-}
-
-OverlapRange find_most_preceding_range(const OverlapRange & range, const DecisionInputs & inputs)
-{
-  OverlapRange preceding_range = range;
-  bool found_preceding_range = true;
-  while (found_preceding_range) {
-    found_preceding_range = false;
-    for (const auto & other_range : inputs.ranges) {
-      if (
-        other_range.entering_path_idx < preceding_range.entering_path_idx &&
-        other_range.exiting_path_idx >= preceding_range.entering_path_idx) {
-        preceding_range = other_range;
-        found_preceding_range = true;
-      }
+    if (will_collide_on_range(range_times, params, logger)) {
+      range.debug.times = range_times;
+      range.debug.object = object;
+      return true;
     }
   }
-  return preceding_range;
+  range.debug.times = range_times;
+  return false;
 }
 
 void set_decision_velocity(
@@ -366,6 +354,7 @@ std::vector<Slowdown> calculate_decisions(
   for (const auto & range : inputs.ranges) {
     if (range.entering_path_idx == 0UL) continue;  // skip if we already entered the range
     const auto optional_decision = calculate_decision(range, inputs, params, logger);
+    range.debug.decision = optional_decision;
     if (optional_decision) decisions.push_back(*optional_decision);
   }
   return decisions;
diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp
index 1d1fe8bea94a6..9fdc9f333db65 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp
@@ -30,16 +30,6 @@
 
 namespace behavior_velocity_planner::out_of_lane
 {
-struct EnterExitTimes
-{
-  double enter_time;
-  double exit_time;
-};
-struct RangeTimes
-{
-  EnterExitTimes ego;
-  EnterExitTimes object;
-};
 /// @brief calculate the distance along the ego path between ego and some target path index
 /// @param [in] ego_data data related to the ego vehicle
 /// @param [in] target_idx target ego path index
@@ -63,13 +53,15 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx);
 /// the opposite direction, time at enter > time at exit
 std::optional<std::pair<double, double>> object_time_to_range(
   const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
-  const std::shared_ptr<route_handler::RouteHandler> route_handler, const rclcpp::Logger & logger);
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, const double dist_buffer,
+  const rclcpp::Logger & logger);
 /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit
 /// points of an overlapping range
 /// @param [in] object dynamic object
 /// @param [in] range overlapping range
 /// @param [in] inputs information used to take decisions (ranges, ego and objects data, route
 /// handler, lanelets)
+/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range
 /// @param [in] logger ros logger
 /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in
 /// the opposite direction, time at enter > time at exit.
@@ -93,14 +85,6 @@ bool will_collide_on_range(
 bool should_not_enter(
   const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params,
   const rclcpp::Logger & logger);
-/// @brief find the most preceding range
-/// @details the most preceding range is the first range in a succession of ranges with overlapping
-/// enter/exit indexes.
-/// @param [in] range range
-/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route
-/// handler, lanelets)
-/// @return most preceding range
-OverlapRange find_most_preceding_range(const OverlapRange & range, const DecisionInputs & inputs);
 /// @brief set the velocity of a decision (or unset it) based on the distance away from the range
 /// @param [out] decision decision to update (either set its velocity or unset the decision)
 /// @param [in] distance distance between ego and the range corresponding to the decision
diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
index 48869e5e3926d..0d26a29b6fab5 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
@@ -21,6 +21,9 @@
 
 namespace behavior_velocity_planner::out_of_lane
 {
+using motion_utils::calcLateralOffset;
+using tier4_autoware_utils::calcDistance2d;
+
 /// @brief filter predicted objects and their predicted paths
 /// @param [in] objects predicted objects to filter
 /// @param [in] ego_data ego data
@@ -42,8 +45,13 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
     auto filtered_object = object;
     const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
       const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
+      const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
+
+      // Note: lateral offset can not be calculated for path with less than 2 unique points
       const auto lat_offset_to_current_ego =
-        std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position));
+        unique_path_points.size() > 1
+          ? std::abs(calcLateralOffset(unique_path_points, ego_data.pose.position))
+          : calcDistance2d(unique_path_points.front(), ego_data.pose.position);
       const auto is_crossing_ego =
         lat_offset_to_current_ego <=
         object.shape.dimensions.y / 2.0 + std::max(
diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp
index 2422c6fe84e56..23de809e429ec 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp
@@ -50,6 +50,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns + ".objects.use_predicted_paths");
   pp.objects_min_confidence =
     getOrDeclareParameter<double>(node, ns + ".objects.predicted_path_min_confidence");
+  pp.objects_dist_buffer = getOrDeclareParameter<double>(node, ns + ".objects.distance_buffer");
 
   pp.overlap_min_dist = getOrDeclareParameter<double>(node, ns + ".overlap.minimum_distance");
   pp.overlap_extra_length = getOrDeclareParameter<double>(node, ns + ".overlap.extra_length");
@@ -57,6 +58,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
   pp.skip_if_over_max_decel =
     getOrDeclareParameter<bool>(node, ns + ".action.skip_if_over_max_decel");
   pp.precision = getOrDeclareParameter<double>(node, ns + ".action.precision");
+  pp.min_decision_duration = getOrDeclareParameter<double>(node, ns + ".action.min_duration");
   pp.dist_buffer = getOrDeclareParameter<double>(node, ns + ".action.distance_buffer");
   pp.slow_velocity = getOrDeclareParameter<double>(node, ns + ".action.slowdown.velocity");
   pp.slow_dist_threshold =
diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp
index 98a9e4404e013..6f74f42af2056 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp
@@ -23,7 +23,6 @@
 #include <tf2/utils.h>
 
 #include <algorithm>
-#include <limits>
 
 namespace behavior_velocity_planner::out_of_lane
 {
@@ -76,10 +75,12 @@ OverlapRanges calculate_overlapping_ranges(
 {
   OverlapRanges ranges;
   OtherLane other_lane(lanelet);
+  std::vector<Overlap> overlaps;
   for (auto i = 0UL; i < path_footprints.size(); ++i) {
     const auto overlap = calculate_overlap(path_footprints[i], path_lanelets, lanelet);
     const auto has_overlap = overlap.inside_distance > params.overlap_min_dist;
     if (has_overlap) {  // open/update the range
+      overlaps.push_back(overlap);
       if (!other_lane.range_is_open) {
         other_lane.first_range_bound.index = i;
         other_lane.first_range_bound.point = overlap.min_overlap_point;
@@ -94,10 +95,16 @@ OverlapRanges calculate_overlapping_ranges(
       other_lane.last_range_bound.inside_distance = overlap.inside_distance;
     } else if (other_lane.range_is_open) {  // !has_overlap: close the range if it is open
       ranges.push_back(other_lane.close_range());
+      ranges.back().debug.overlaps = overlaps;
+      overlaps.clear();
     }
   }
   // close the range if it is still open
-  if (other_lane.range_is_open) ranges.push_back(other_lane.close_range());
+  if (other_lane.range_is_open) {
+    ranges.push_back(other_lane.close_range());
+    ranges.back().debug.overlaps = overlaps;
+    overlaps.clear();
+  }
   return ranges;
 }
 
diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp
index ab4d31480e1cb..24bd6b3f16eb9 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp
@@ -21,21 +21,11 @@
 
 #include <lanelet2_core/LaneletMap.h>
 
-#include <limits>
 #include <vector>
 
 namespace behavior_velocity_planner::out_of_lane
 {
 
-/// @brief representation of an overlap between the ego footprint and some other lane
-struct Overlap
-{
-  double inside_distance = 0.0;  ///!< distance inside the overlap
-  double min_arc_length = std::numeric_limits<double>::infinity();
-  double max_arc_length = 0.0;
-  lanelet::BasicPoint2d min_overlap_point{};  ///!< point with min arc length
-  lanelet::BasicPoint2d max_overlap_point{};  ///!< point with max arc length
-};
 /// @brief calculate the overlap between the given footprint and lanelet
 /// @param [in] path_footprint footprint used to calculate the overlap
 /// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path
diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
index 74d1f86c51df1..1a73e74a723e2 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
@@ -50,8 +50,7 @@ OutOfLaneModule::OutOfLaneModule(
   velocity_factor_.init(VelocityFactor::UNKNOWN);
 }
 
-bool OutOfLaneModule::modifyPathVelocity(
-  PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
+bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
 {
   debug_data_.reset_data();
   *stop_reason = planning_utils::initializeStopReason(StopReason::OUT_OF_LANE);
@@ -63,6 +62,19 @@ bool OutOfLaneModule::modifyPathVelocity(
   ego_data.path.points = path->points;
   ego_data.first_path_idx =
     motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position);
+  for (const auto & p : prev_overlapping_path_points_) {
+    const auto nearest_idx =
+      motion_utils::findNearestIndex(ego_data.path.points, p.point.pose, 1.0);
+    const auto insert_idx =
+      motion_utils::findNearestSegmentIndex(ego_data.path.points, p.point.pose, 1.0);
+    if (nearest_idx && insert_idx && *insert_idx > ego_data.first_path_idx) {
+      if (
+        tier4_autoware_utils::calcDistance2d(
+          ego_data.path.points[*nearest_idx].point.pose, p.point.pose) > 0.5)
+        ego_data.path.points.insert(std::next(ego_data.path.points.begin(), *insert_idx), p);
+    }
+  }
+  motion_utils::removeOverlapPoints(ego_data.path.points);
   ego_data.velocity = planner_data_->current_velocity->twist.linear.x;
   ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold;
   stopwatch.tic("calculate_path_footprints");
@@ -82,6 +94,8 @@ bool OutOfLaneModule::modifyPathVelocity(
   debug_data_.path_lanelets = path_lanelets;
   debug_data_.ignored_lanelets = ignored_lanelets;
   debug_data_.other_lanelets = other_lanelets;
+  debug_data_.path = ego_data.path;
+  debug_data_.first_path_idx = ego_data.first_path_idx;
 
   if (params_.skip_if_already_overlapping) {
     debug_data_.current_footprint = current_ego_footprint;
@@ -99,6 +113,10 @@ bool OutOfLaneModule::modifyPathVelocity(
   stopwatch.tic("calculate_overlapping_ranges");
   const auto ranges =
     calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_);
+  prev_overlapping_path_points_.clear();
+  for (const auto & range : ranges)
+    prev_overlapping_path_points_.push_back(
+      ego_data.path.points[ego_data.first_path_idx + range.entering_path_idx]);
   const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges");
   // Calculate stop and slowdown points
   stopwatch.tic("calculate_decisions");
@@ -108,14 +126,27 @@ bool OutOfLaneModule::modifyPathVelocity(
   inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_);
   inputs.route_handler = planner_data_->route_handler_;
   inputs.lanelets = other_lanelets;
-  auto decisions = calculate_decisions(inputs, params_, logger_);
+  const auto decisions = calculate_decisions(inputs, params_, logger_);
   const auto calculate_decisions_us = stopwatch.toc("calculate_decisions");
   stopwatch.tic("calc_slowdown_points");
-  const auto point_to_insert = calculate_slowdown_point(ego_data, decisions, params_);
+  if (  // reset the previous inserted point if the timer expired
+    prev_inserted_point_ &&
+    (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration)
+    prev_inserted_point_.reset();
+  auto point_to_insert =
+    calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_);
   const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points");
   stopwatch.tic("insert_slowdown_points");
   debug_data_.slowdowns.clear();
+  if (  // reset the timer if there is no previous inserted point or if we avoid the same lane
+    point_to_insert &&
+    (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() ==
+                                point_to_insert->slowdown.lane_to_avoid.id()))
+    prev_inserted_point_time_ = clock_->now();
+  if (!point_to_insert && prev_inserted_point_) point_to_insert = prev_inserted_point_;
   if (point_to_insert) {
+    prev_inserted_point_ = point_to_insert;
+    RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id());
     debug_data_.slowdowns = {*point_to_insert};
     auto path_idx = motion_utils::findNearestSegmentIndex(
                       path->points, point_to_insert->point.point.pose.position) +
@@ -127,14 +158,17 @@ bool OutOfLaneModule::modifyPathVelocity(
       tier4_planning_msgs::msg::StopFactor stop_factor;
       stop_factor.stop_pose = point_to_insert->point.point.pose;
       stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength(
-        ego_data.path.points, ego_data.pose.position, point_to_insert->point.point.pose.position);
+        path->points, ego_data.pose.position, point_to_insert->point.point.pose.position);
       planning_utils::appendStopReason(stop_factor, stop_reason);
     }
     velocity_factor_.set(
       path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose,
       VelocityFactor::UNKNOWN);
+  } else if (!decisions.empty()) {
+    RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)");
   }
   const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points");
+  debug_data_.ranges = inputs.ranges;
 
   const auto total_time_us = stopwatch.toc();
   RCLCPP_DEBUG(
@@ -155,18 +189,23 @@ MarkerArray OutOfLaneModule::createDebugMarkerArray()
   constexpr auto z = 0.0;
   MarkerArray debug_marker_array;
 
-  debug::add_footprint_markers(debug_marker_array, debug_data_.footprints, z);
+  debug::add_footprint_markers(
+    debug_marker_array, debug_data_.footprints, z, debug_data_.prev_footprints);
   debug::add_current_overlap_marker(
-    debug_marker_array, debug_data_.current_footprint, debug_data_.current_overlapped_lanelets, z);
+    debug_marker_array, debug_data_.current_footprint, debug_data_.current_overlapped_lanelets, z,
+    debug_data_.prev_current_overlapped_lanelets);
   debug::add_lanelet_markers(
     debug_marker_array, debug_data_.path_lanelets, "path_lanelets",
-    tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5));
+    tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), debug_data_.prev_path_lanelets);
   debug::add_lanelet_markers(
     debug_marker_array, debug_data_.ignored_lanelets, "ignored_lanelets",
-    tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5));
+    tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data_.prev_ignored_lanelets);
   debug::add_lanelet_markers(
     debug_marker_array, debug_data_.other_lanelets, "other_lanelets",
-    tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5));
+    tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data_.prev_other_lanelets);
+  debug::add_range_markers(
+    debug_marker_array, debug_data_.ranges, debug_data_.path, debug_data_.first_path_idx, z,
+    debug_data_.prev_ranges);
   return debug_marker_array;
 }
 
diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp
index f2ca2ababfef4..ebadf81bee004 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp
@@ -21,6 +21,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
@@ -47,9 +48,13 @@ class OutOfLaneModule : public SceneModuleInterface
   motion_utils::VirtualWalls createVirtualWalls() override;
 
 private:
-  // Parameter
   PlannerParam params_;
 
+  std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>
+    prev_overlapping_path_points_{};
+  std::optional<SlowdownToInsert> prev_inserted_point_{};
+  rclcpp::Time prev_inserted_point_time_{};
+
 protected:
   int64_t module_id_{};
 
diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp
index 64d88f15e6966..b2453aebc5dc9 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp
@@ -24,6 +24,7 @@
 #include <lanelet2_core/LaneletMap.h>
 
 #include <algorithm>
+#include <limits>
 #include <memory>
 #include <string>
 #include <utility>
@@ -47,6 +48,8 @@ struct PlannerParam
   bool objects_use_predicted_paths;  // whether to use the objects' predicted paths
   double objects_min_vel;            // [m/s] objects lower than this velocity will be ignored
   double objects_min_confidence;     // minimum confidence to consider a predicted path
+  double objects_dist_buffer;  // [m] distance buffer used to determine if a collision will occur in
+                               // the other lane
 
   double overlap_extra_length;  // [m] extra length to add around an overlap range
   double overlap_min_dist;      // [m] min distance inside another lane to consider an overlap
@@ -56,7 +59,8 @@ struct PlannerParam
   double slow_velocity;
   double slow_dist_threshold;
   double stop_dist_threshold;
-  double precision;  // [m] precision when inserting a stop pose in the path
+  double precision;              // [m] precision when inserting a stop pose in the path
+  double min_decision_duration;  // [s] minimum duration needed a decision can be canceled
   // ego dimensions used to create its polygon footprint
   double front_offset;        // [m]  front offset (from vehicle info)
   double rear_offset;         // [m]  rear offset (from vehicle info)
@@ -68,17 +72,16 @@ struct PlannerParam
   double extra_left_offset;   // [m] extra left distance
 };
 
-/// @brief range along the path where ego overlaps another lane
-struct OverlapRange
+struct EnterExitTimes
 {
-  lanelet::ConstLanelet lane;
-  size_t entering_path_idx{};
-  size_t exiting_path_idx{};
-  lanelet::BasicPoint2d entering_point;  // pose of the overlapping point closest along the lane
-  lanelet::BasicPoint2d exiting_point;   // pose of the overlapping point furthest along the lane
-  double inside_distance{};              // [m] how much ego footprint enters the lane
+  double enter_time{};
+  double exit_time{};
+};
+struct RangeTimes
+{
+  EnterExitTimes ego{};
+  EnterExitTimes object{};
 };
-using OverlapRanges = std::vector<OverlapRange>;
 
 /// @brief action taken by the "out of lane" module
 struct Slowdown
@@ -102,6 +105,35 @@ struct RangeBound
   double arc_length;
   double inside_distance;
 };
+
+/// @brief representation of an overlap between the ego footprint and some other lane
+struct Overlap
+{
+  double inside_distance = 0.0;  ///!< distance inside the overlap
+  double min_arc_length = std::numeric_limits<double>::infinity();
+  double max_arc_length = 0.0;
+  lanelet::BasicPoint2d min_overlap_point{};  ///!< point with min arc length
+  lanelet::BasicPoint2d max_overlap_point{};  ///!< point with max arc length
+};
+
+/// @brief range along the path where ego overlaps another lane
+struct OverlapRange
+{
+  lanelet::ConstLanelet lane;
+  size_t entering_path_idx{};
+  size_t exiting_path_idx{};
+  lanelet::BasicPoint2d entering_point;  // pose of the overlapping point closest along the lane
+  lanelet::BasicPoint2d exiting_point;   // pose of the overlapping point furthest along the lane
+  double inside_distance{};              // [m] how much ego footprint enters the lane
+  mutable struct
+  {
+    std::vector<Overlap> overlaps;
+    std::optional<Slowdown> decision;
+    RangeTimes times;
+    std::optional<autoware_auto_perception_msgs::msg::PredictedObject> object{};
+  } debug;
+};
+using OverlapRanges = std::vector<OverlapRange>;
 /// @brief representation of a lane and its current overlap range
 struct OtherLane
 {
@@ -164,12 +196,32 @@ struct DebugData
   lanelet::ConstLanelets path_lanelets;
   lanelet::ConstLanelets ignored_lanelets;
   lanelet::ConstLanelets other_lanelets;
+  autoware_auto_planning_msgs::msg::PathWithLaneId path;
+  size_t first_path_idx;
+
+  size_t prev_footprints = 0;
+  size_t prev_slowdowns = 0;
+  size_t prev_ranges = 0;
+  size_t prev_current_overlapped_lanelets = 0;
+  size_t prev_ignored_lanelets = 0;
+  size_t prev_path_lanelets = 0;
+  size_t prev_other_lanelets = 0;
   void reset_data()
   {
+    prev_footprints = footprints.size();
     footprints.clear();
+    prev_slowdowns = slowdowns.size();
     slowdowns.clear();
+    prev_ranges = ranges.size();
     ranges.clear();
+    prev_current_overlapped_lanelets = current_overlapped_lanelets.size();
     current_overlapped_lanelets.clear();
+    prev_ignored_lanelets = ignored_lanelets.size();
+    ignored_lanelets.clear();
+    prev_path_lanelets = path_lanelets.size();
+    path_lanelets.clear();
+    prev_other_lanelets = other_lanelets.size();
+    other_lanelets.clear();
   }
 };
 
diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt
index 694bddf421486..0d7b636646783 100644
--- a/planning/surround_obstacle_checker/CMakeLists.txt
+++ b/planning/surround_obstacle_checker/CMakeLists.txt
@@ -18,12 +18,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/node.cpp
 )
 
-if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
-  target_compile_definitions(${PROJECT_NAME} PRIVATE
-    DEFINE_LEGACY_FUNCTION
-  )
-endif()
-
 target_link_libraries(${PROJECT_NAME}
   ${PCL_LIBRARIES}
 )