diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp
index ac24591a0a283..d308f55799ce0 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp
@@ -59,8 +59,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons
 
 MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns);
 
-MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);
-
 MarkerArray makeOverhangToRoadShoulderMarkerArray(
   const behavior_path_planner::ObjectDataArray & objects, std::string && ns);
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp
index 808e76a140761..d337bae17ca6c 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp
@@ -26,12 +26,8 @@ namespace marker_utils::lane_change_markers
 {
 using behavior_path_planner::LaneChangePath;
 using visualization_msgs::msg::MarkerArray;
-MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
-MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
-MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
-MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
index 657437615f809..ba8ddd9b19c46 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
@@ -104,6 +104,11 @@ MarkerArray createPredictedPathMarkerArray(
   const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info,
   std::string && ns, const int32_t & id, const float & r, const float & g, const float & b);
 
+MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
index 0278b73b0acb4..3264ec1e9ddfd 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -556,9 +556,6 @@ struct DebugData
   // shift path
   std::vector<double> proposed_spline_shift;
 
-  // future pose
-  PathWithLaneId path_with_planned_velocity;
-
   // avoidance require objects
   ObjectDataArray unavoidable_objects;
 
@@ -568,6 +565,10 @@ struct DebugData
   // tmp for plot
   PathWithLaneId center_line;
 
+  // collision check debug map
+  utils::path_safety_checker::CollisionCheckDebugMap collision_check;
+
+  // debug msg array
   AvoidanceDebugMsgArray avoidance_debug_msg_array;
 };
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
index 87c93f49901f0..93eb0457d7cf4 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
@@ -179,10 +179,10 @@ struct SafetyCheckParams
 struct CollisionCheckDebug
 {
   std::string unsafe_reason;                ///< Reason indicating unsafe situation.
-  Pose current_pose{};                      ///< Ego vehicle's current pose.
   Twist current_twist{};                    ///< Ego vehicle's current velocity and rotation.
-  Twist object_twist{};                     ///< Detected object's velocity and rotation.
   Pose expected_ego_pose{};                 ///< Predicted future pose of ego vehicle.
+  Pose current_obj_pose{};                  ///< Detected object's current pose.
+  Twist object_twist{};                     ///< Detected object's velocity and rotation.
   Pose expected_obj_pose{};                 ///< Predicted future pose of object.
   double rss_longitudinal{0.0};             ///< Longitudinal RSS measure.
   double inter_vehicle_distance{0.0};       ///< Distance between ego vehicle and object.
@@ -190,9 +190,10 @@ struct CollisionCheckDebug
   double extended_polygon_lat_offset{0.0};  ///< Lateral offset for extended polygon.
   bool is_front{false};                     ///< True if object is in front of ego vehicle.
   bool is_safe{false};                      ///< True if situation is deemed safe.
-  std::vector<Pose> lerped_path;            ///< Interpolated ego vehicle path.
-  Polygon2d extended_ego_polygon{};         ///< Ego vehicle's extended collision polygon.
-  Polygon2d extended_obj_polygon{};         ///< Detected object's extended collision polygon.
+  std::vector<PoseWithVelocityStamped> ego_predicted_path;  ///< ego vehicle's predicted path.
+  std::vector<PoseWithVelocityAndPolygonStamped> obj_predicted_path;  ///< object's predicted path.
+  Polygon2d extended_ego_polygon{};  ///< Ego vehicle's extended collision polygon.
+  Polygon2d extended_obj_polygon{};  ///< Detected object's extended collision polygon.
 };
 using CollisionCheckDebugPair = std::pair<std::string, CollisionCheckDebug>;
 using CollisionCheckDebugMap =
diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp
index 94073ad467326..bea1447b0d95a 100644
--- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp
+++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp
@@ -416,12 +416,6 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
   return msg;
 }
 
-MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
-{
-  return createObjectsCubeMarkerArray(
-    objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8));
-}
-
 MarkerArray makeOverhangToRoadShoulderMarkerArray(
   const ObjectDataArray & objects, std::string && ns)
 {
diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
index aab2be6f3054e..7493982db78b1 100644
--- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
+++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
@@ -36,40 +36,6 @@ using geometry_msgs::msg::Point;
 using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createMarkerScale;
 
-MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  Marker obj_marker = createDefaultMarker(
-    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
-    createMarkerScale(0.5, 0.5, 0.5), colors::aqua());
-
-  MarkerArray marker_array;
-  int32_t id{0};
-
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  int idx{0};
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    obj_marker.id = ++id;
-    obj_marker.pose = info.current_pose;
-
-    std::ostringstream ss;
-
-    ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason
-       << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
-       << "\nEgo to obj: " << info.inter_vehicle_distance
-       << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
-       << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
-       << "\nPosition: " << (info.is_front ? "front" : "back")
-       << "\nSafe: " << (info.is_safe ? "Yes" : "No");
-
-    obj_marker.text = ss.str();
-
-    marker_array.markers.push_back(obj_marker);
-  }
-  return marker_array;
-}
-
 MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes, std::string && ns)
 {
   if (lanes.empty()) {
@@ -103,121 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes
   return marker_array;
 }
 
-MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  MarkerArray marker_array;
-  int32_t id{0};
-  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    Marker marker = createDefaultMarker(
-      "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3),
-      colors::magenta());
-    marker.points.reserve(info.lerped_path.size());
-
-    for (const auto & point : info.lerped_path) {
-      marker.points.push_back(point.position);
-    }
-
-    marker_array.markers.push_back(marker);
-  }
-  return marker_array;
-}
-
-MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  if (obj_debug_vec.empty()) {
-    return MarkerArray{};
-  }
-
-  constexpr float scale_val{0.2};
-  int32_t id{0};
-  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
-  Marker ego_marker = createDefaultMarker(
-    "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val),
-    colors::green());
-  Marker obj_marker = ego_marker;
-
-  auto text_marker = createDefaultMarker(
-    "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
-    createMarkerScale(1.5, 1.5, 1.5), colors::white());
-
-  MarkerArray marker_array;
-
-  const auto reserve_size = obj_debug_vec.size();
-
-  marker_array.markers.reserve(reserve_size * 4);
-
-  int32_t idx = {0};
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    const auto color = info.is_safe ? colors::green() : colors::red();
-    const auto & ego_polygon = info.extended_ego_polygon.outer();
-    const auto poly_z = info.current_pose.position.z;  // temporally
-    ego_marker.id = ++id;
-    ego_marker.color = color;
-    ego_marker.points.reserve(ego_polygon.size());
-    for (const auto & p : ego_polygon) {
-      ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
-    }
-    marker_array.markers.push_back(ego_marker);
-
-    std::ostringstream ss;
-    text_marker.id = ego_marker.id;
-    ss << ++idx;
-    text_marker.text = ss.str();
-    text_marker.pose = info.expected_ego_pose;
-
-    marker_array.markers.push_back(text_marker);
-
-    const auto & obj_polygon = info.extended_obj_polygon.outer();
-    obj_marker.id = ++id;
-    obj_marker.color = color;
-    obj_marker.points.reserve(obj_polygon.size());
-    for (const auto & p : obj_polygon) {
-      obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
-    }
-    marker_array.markers.push_back(obj_marker);
-
-    text_marker.id = obj_marker.id;
-    text_marker.pose = info.expected_obj_pose;
-    marker_array.markers.push_back(text_marker);
-
-    ego_marker.points.clear();
-    obj_marker.points.clear();
-  }
-
-  return marker_array;
-}
-
-MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  const auto colors = colors::colors_list();
-  const auto loop_size = std::min(colors.size(), obj_debug_vec.size());
-  MarkerArray marker_array;
-  int32_t id{0};
-  size_t idx{0};
-  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    const auto & color = colors.at(idx);
-    Marker marker = createDefaultMarker(
-      "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color);
-    marker.points.reserve(2);
-    marker.points.push_back(info.expected_ego_pose.position);
-    marker.points.push_back(info.expected_obj_pose.position);
-    marker_array.markers.push_back(marker);
-    ++idx;
-    if (idx >= loop_size) {
-      break;
-    }
-  }
-
-  return marker_array;
-}
-
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns)
diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp
index f24e772d198c1..d6a237079448f 100644
--- a/planning/behavior_path_planner/src/marker_utils/utils.cpp
+++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp
@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
 
+#include "behavior_path_planner/marker_utils/colors.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
@@ -37,7 +38,7 @@ using visualization_msgs::msg::Marker;
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
 {
   CollisionCheckDebug debug;
-  debug.current_pose = obj.initial_pose.pose;
+  debug.current_obj_pose = obj.initial_pose.pose;
   debug.current_twist = obj.initial_twist.twist;
   return {tier4_autoware_utils::toHexString(obj.uuid), debug};
 }
@@ -486,4 +487,162 @@ MarkerArray createPredictedPathMarkerArray(
   return msg;
 }
 
+MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
+{
+  if (obj_debug_vec.empty()) {
+    return MarkerArray{};
+  }
+
+  int32_t id{0};
+  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
+
+  constexpr float line_scale_val{0.2};
+  const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val);
+
+  auto default_line_marker = [&](const auto & color = colors::green()) {
+    return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color);
+  };
+
+  constexpr float text_scale_val{1.5};
+  const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val);
+
+  auto default_text_marker = [&]() {
+    return createDefaultMarker(
+      "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      text_marker_scale, colors::white());
+  };
+
+  auto default_cube_marker =
+    [&](const auto & width, const auto & depth, const auto & color = colors::green()) {
+      return createDefaultMarker(
+        "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE,
+        createMarkerScale(width, depth, 1.0), color);
+    };
+
+  MarkerArray marker_array;
+  marker_array.markers.reserve(
+    obj_debug_vec.size() * 5);  // poly ego, text ego, poly obj, text obj, cube obj
+
+  int32_t idx = {0};
+  for (const auto & [uuid, info] : obj_debug_vec) {
+    const auto color = info.is_safe ? colors::green() : colors::red();
+    const auto poly_z = info.current_obj_pose.position.z;  // temporally
+
+    const auto insert_polygon_marker = [&](const auto & polygon) {
+      marker_array.markers.emplace_back();
+      auto & polygon_marker = marker_array.markers.back();
+      polygon_marker = default_line_marker(color);
+      polygon_marker.points.reserve(polygon.outer().size());
+      for (const auto & p : polygon.outer()) {
+        polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z));
+      }
+    };
+
+    insert_polygon_marker(info.extended_ego_polygon);
+    insert_polygon_marker(info.extended_obj_polygon);
+
+    const auto str_idx = std::to_string(++idx);
+    const auto insert_text_marker = [&](const auto & pose) {
+      marker_array.markers.emplace_back();
+      auto & text_marker = marker_array.markers.back();
+      text_marker = default_text_marker();
+      text_marker.text = str_idx;
+      text_marker.pose = pose;
+    };
+
+    insert_text_marker(info.expected_ego_pose);
+    insert_text_marker(info.expected_obj_pose);
+
+    const auto insert_cube_marker = [&](const auto & pose) {
+      marker_array.markers.emplace_back();
+      auto & cube_marker = marker_array.markers.back();
+      cube_marker = default_cube_marker(1.0, 1.0, color);
+      cube_marker.pose = pose;
+    };
+    insert_cube_marker(info.current_obj_pose);
+  }
+  return marker_array;
+}
+
+MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
+{
+  int32_t id{0};
+  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
+  const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3);
+  const auto default_arrow_marker = [&](const auto & color) {
+    return createDefaultMarker(
+      "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color);
+  };
+
+  MarkerArray marker_array;
+  marker_array.markers.reserve(std::accumulate(
+    obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL,
+    [&](const auto current_sum, const auto & obj_debug) {
+      const auto & [uuid, info] = obj_debug;
+      return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2;
+    }));
+
+  for (const auto & [uuid, info] : obj_debug_vec) {
+    const auto insert_marker = [&](const auto & path, const auto & color) {
+      for (const auto & pose : path) {
+        marker_array.markers.emplace_back();
+        auto & marker = marker_array.markers.back();
+        marker = default_arrow_marker(color);
+        marker.pose = pose.pose;
+      }
+    };
+
+    insert_marker(info.ego_predicted_path, colors::aqua());
+    insert_marker(info.obj_predicted_path, colors::yellow());
+    const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) {
+      // instead of checking for distance, inserting a new marker might be more efficient
+      marker_array.markers.emplace_back();
+      auto & marker = marker_array.markers.back();
+      marker = default_arrow_marker(color);
+      marker.pose = pose;
+      marker.pose.position.z += 0.05;
+    };
+
+    insert_expected_pose_marker(info.expected_ego_pose, colors::red());
+    insert_expected_pose_marker(info.expected_obj_pose, colors::red());
+  }
+  return marker_array;
+}
+
+MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
+{
+  int32_t id{0};
+  auto default_text_marker = [&]() {
+    return createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.5, 0.5, 0.5), colors::aqua());
+  };
+
+  MarkerArray marker_array;
+
+  marker_array.markers.reserve(obj_debug_vec.size());
+
+  int idx{0};
+
+  for (const auto & [uuid, info] : obj_debug_vec) {
+    auto safety_check_info_text = default_text_marker();
+    safety_check_info_text.pose = info.current_obj_pose;
+
+    std::ostringstream ss;
+
+    ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason
+       << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
+       << "\nEgo to obj: " << info.inter_vehicle_distance
+       << "\nExtended polygon: " << (info.is_front ? "ego" : "object")
+       << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
+       << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
+       << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego")
+       << "\nSafe: " << (info.is_safe ? "Yes" : "No");
+
+    safety_check_info_text.text = ss.str();
+    marker_array.markers.push_back(safety_check_info_text);
+  }
+  return marker_array;
+}
+
 }  // namespace marker_utils
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index ad11dad090ff5..36a33e5bee088 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -1855,25 +1855,35 @@ bool AvoidanceModule::isSafePath(
     avoid_data_, planner_data_, parameters_, is_right_shift.value());
 
   for (const auto & object : safety_check_target_objects) {
+    auto current_debug_data = marker_utils::createObjectDebug(object);
+
     const auto obj_polygon =
       tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape);
 
     const auto is_object_front =
       utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info);
 
+    const auto is_object_incoming =
+      std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2;
+
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, parameters_->check_all_predicted_path);
 
-    const auto & ego_predicted_path =
-      is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object;
+    const auto & ego_predicted_path = is_object_front && !is_object_incoming
+                                        ? ego_predicted_path_for_front_object
+                                        : ego_predicted_path_for_rear_object;
 
     for (const auto & obj_path : obj_predicted_paths) {
-      CollisionCheckDebug collision{};
       if (!utils::path_safety_checker::checkCollision(
             shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
-            hysteresis_factor, collision)) {
+            hysteresis_factor, current_debug_data.second)) {
+        marker_utils::updateCollisionCheckDebugMap(
+          debug.collision_check, current_debug_data, false);
+
         safe_count_ = 0;
         return false;
+      } else {
+        marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true);
       }
     }
   }
@@ -2507,6 +2517,7 @@ void AvoidanceModule::updateData()
 void AvoidanceModule::processOnEntry()
 {
   initVariables();
+  removeRTCStatus();
 }
 
 void AvoidanceModule::processOnExit()
@@ -2685,12 +2696,14 @@ void AvoidanceModule::updateDebugMarker(
   using marker_utils::createShiftGradMarkerArray;
   using marker_utils::createShiftLengthMarkerArray;
   using marker_utils::createShiftLineMarkerArray;
+  using marker_utils::showPolygon;
+  using marker_utils::showPredictedPath;
+  using marker_utils::showSafetyCheckInfo;
   using marker_utils::avoidance_marker::createAvoidLineMarkerArray;
   using marker_utils::avoidance_marker::createEgoStatusMarkerArray;
   using marker_utils::avoidance_marker::createOtherObjectsMarkerArray;
   using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray;
   using marker_utils::avoidance_marker::createPredictedVehiclePositions;
-  using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray;
   using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray;
   using tier4_autoware_utils::appendMarkerArray;
 
@@ -2714,9 +2727,11 @@ void AvoidanceModule::updateDebugMarker(
       add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w));
     };
 
+  const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) {
+    add(createOtherObjectsMarkerArray(objects, ns));
+  };
+
   add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status"));
-  add(createPredictedVehiclePositions(
-    debug.path_with_planned_velocity, "predicted_vehicle_positions"));
 
   const auto & path = data.reference_path;
   add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9));
@@ -2728,31 +2743,37 @@ void AvoidanceModule::updateDebugMarker(
   add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
   add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));
 
-  add(createOtherObjectsMarkerArray(
-    data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD));
-  add(createOtherObjectsMarkerArray(
-    data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD));
-  add(createOtherObjectsMarkerArray(
-    data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL));
-  add(createOtherObjectsMarkerArray(
-    data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE));
-  add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE));
-  add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT));
-  add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject")));
-  add(createOtherObjectsMarkerArray(data.other_objects, std::string("CrosswalkUser")));
-  add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea")));
-  add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance")));
-  add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold")));
-
   add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang"));
   add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0));
 
-  add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects"));
+  // ignore objects
+  {
+    addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD);
+    addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD);
+    addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE);
+    addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL);
+    addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE);
+    addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT);
+    addObjects(data.other_objects, std::string("MovingObject"));
+    addObjects(data.other_objects, std::string("CrosswalkUser"));
+    addObjects(data.other_objects, std::string("OutOfTargetArea"));
+    addObjects(data.other_objects, std::string("NotNeedAvoidance"));
+    addObjects(data.other_objects, std::string("LessThanExecutionThreshold"));
+  }
 
   // parent object info
-  addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0);
-  addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2);
-  addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8);
+  {
+    addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0);
+    addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2);
+    addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8);
+  }
+
+  // safety check
+  {
+    add(showSafetyCheckInfo(debug.collision_check, "object_debug_info"));
+    add(showPredictedPath(debug.collision_check, "ego_predicted_path"));
+    add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation"));
+  }
 
   // shift length
   {
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
index 072c1c62ae3d2..0ceab21573316 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -295,11 +295,10 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   if (!parameters_->publish_debug_marker) {
     return;
   }
+  using marker_utils::showPolygon;
+  using marker_utils::showPredictedPath;
+  using marker_utils::showSafetyCheckInfo;
   using marker_utils::lane_change_markers::showAllValidLaneChangePath;
-  using marker_utils::lane_change_markers::showLerpedPose;
-  using marker_utils::lane_change_markers::showObjectInfo;
-  using marker_utils::lane_change_markers::showPolygon;
-  using marker_utils::lane_change_markers::showPolygonPose;
 
   const auto debug_data = module_type_->getDebugData();
   const auto debug_after_approval = module_type_->getAfterApprovalDebugData();
@@ -312,14 +311,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const
 
   add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths"));
   if (!debug_data.empty()) {
-    add(showObjectInfo(debug_data, "object_debug_info"));
-    add(showLerpedPose(debug_data, "ego_predicted_path"));
+    add(showSafetyCheckInfo(debug_data, "object_debug_info"));
+    add(showPredictedPath(debug_data, "ego_predicted_path"));
     add(showPolygon(debug_data, "ego_and_target_polygon_relation"));
   }
 
   if (!debug_after_approval.empty()) {
-    add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval"));
-    add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval"));
+    add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval"));
+    add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval"));
     add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval"));
   }
 }
diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
index 2939ca29c0a40..1838dc6bdc07b 100644
--- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
+++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
@@ -253,7 +253,11 @@ bool checkCollision(
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
   double hysteresis_factor, CollisionCheckDebug & debug)
 {
-  debug.lerped_path.reserve(target_object_path.path.size());
+  {
+    debug.ego_predicted_path = predicted_ego_path;
+    debug.obj_predicted_path = target_object_path.path;
+    debug.current_obj_pose = target_object.initial_pose.pose;
+  }
 
   for (const auto & obj_pose_with_poly : target_object_path.path) {
     const auto & current_time = obj_pose_with_poly.time;
@@ -277,7 +281,6 @@ bool checkCollision(
     const auto & ego_velocity = interpolated_data->velocity;
 
     {
-      debug.lerped_path.push_back(ego_pose);
       debug.expected_ego_pose = ego_pose;
       debug.expected_obj_pose = obj_pose;
       debug.extended_ego_polygon = ego_polygon;