diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
index 48f7d18ce67a5..b20a835f80e39 100644
--- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
+++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
@@ -346,9 +346,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   {
     std_msgs::msg::ColorRGBA sample_color;
     sample_color.r = 1.0;
-    sample_color.g = 0.0;
-    sample_color.b = 1.0;
-    colors.push_back(sample_color);  // magenta
+    sample_color.g = 0.65;
+    sample_color.b = 0.0;
+    colors.push_back(sample_color);  // orange
+    sample_color.r = 1.0;
+    sample_color.g = 1.0;
+    sample_color.b = 0.0;
+    colors.push_back(sample_color);  // yellow
     sample_color.r = 0.69;
     sample_color.g = 1.0;
     sample_color.b = 0.18;
@@ -361,22 +365,18 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     sample_color.g = 1.0;
     sample_color.b = 0.0;
     colors.push_back(sample_color);  // chartreuse green
-    sample_color.r = 0.12;
-    sample_color.g = 0.56;
-    sample_color.b = 1.0;
-    colors.push_back(sample_color);  // dodger blue
     sample_color.r = 0.0;
     sample_color.g = 1.0;
     sample_color.b = 1.0;
     colors.push_back(sample_color);  // cyan
-    sample_color.r = 0.54;
-    sample_color.g = 0.168;
-    sample_color.b = 0.886;
-    colors.push_back(sample_color);  // blueviolet
-    sample_color.r = 0.0;
-    sample_color.g = 1.0;
-    sample_color.b = 0.5;
-    colors.push_back(sample_color);  // spring green
+    sample_color.r = 0.53;
+    sample_color.g = 0.81;
+    sample_color.b = 0.98;
+    colors.push_back(sample_color);  // light skyblue
+    sample_color.r = 1.0;
+    sample_color.g = 0.41;
+    sample_color.b = 0.71;
+    colors.push_back(sample_color);  // hot pink
   }
 
   double get_line_width() { return m_line_width_property.getFloat(); }
diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
index 73a524e130776..894e377a6f851 100644
--- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
+++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
@@ -64,8 +64,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr(
   marker_ptr->color = path_confidence_color;
   marker_ptr->pose.position = predicted_path.path.back().position;
   marker_ptr->text = std::to_string(predicted_path.confidence);
-  marker_ptr->color.a = std::max(
-    static_cast<double>(std::min(static_cast<double>(predicted_path.confidence), 0.999)), 0.5);
+  marker_ptr->color.a = 0.5;
   return marker_ptr;
 }
 
@@ -81,9 +80,8 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr(
   marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
   marker_ptr->pose = initPose();
   marker_ptr->color = predicted_path_color;
-  marker_ptr->color.a = std::max(
-    static_cast<double>(std::min(static_cast<double>(predicted_path.confidence), 0.999)), 0.5);
-  marker_ptr->scale.x = 0.03 * marker_ptr->color.a;
+  marker_ptr->color.a = 0.6;
+  marker_ptr->scale.x = 0.015;
   calc_path_line_list(predicted_path, marker_ptr->points, is_simple);
   for (size_t k = 0; k < marker_ptr->points.size(); ++k) {
     marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0;
diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp
index f13e971b4d6d6..5d3684d0351c6 100644
--- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp
+++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp
@@ -150,7 +150,11 @@ void ConsoleMeterDisplay::update(float wall_dt, float ros_dt)
   font.setBold(true);
   painter.setFont(font);
   std::ostringstream velocity_ss;
-  velocity_ss << std::fixed << std::setprecision(2) << linear_x * 3.6 << "km/h";
+  if (last_msg_ptr_) {
+    velocity_ss << std::fixed << std::setprecision(2) << linear_x * 3.6 << "km/h";
+  } else {
+    velocity_ss << "Not received";
+  }
   painter.drawText(
     0, std::min(property_value_height_offset_->getInt(), h - 1), w,
     std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter,
diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp
index 18977367afbce..60a81e45c9c29 100644
--- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp
+++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp
@@ -153,7 +153,11 @@ void SteeringAngleDisplay::update(float wall_dt, float ros_dt)
   font.setBold(true);
   painter.setFont(font);
   std::ostringstream steering_angle_ss;
-  steering_angle_ss << std::fixed << std::setprecision(1) << steering * 180.0 / M_PI << "deg";
+  if (last_msg_ptr_) {
+    steering_angle_ss << std::fixed << std::setprecision(1) << steering * 180.0 / M_PI << "deg";
+  } else {
+    steering_angle_ss << "Not received";
+  }
   painter.drawText(
     0, std::min(property_value_height_offset_->getInt(), h - 1), w,
     std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter,
diff --git a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml
index c1ace97b9788f..ec7545956e774 100644
--- a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml
@@ -1,7 +1,18 @@
 <launch>
-  <group>
-    <include file="$(find-pkg-share mission_planner)/launch/mission_planner.launch.xml"/>
-  </group>
+  <arg name="modified_goal_topic_name" default="/planning/scenario_planning/modified_goal"/>
+  <arg name="map_topic_name" default="/map/vector_map"/>
+  <arg name="visualization_topic_name" default="/planning/mission_planning/route_marker"/>
+  <arg name="mission_planner_param_path" default="$(find-pkg-share mission_planner)/config/mission_planner.param.yaml"/>
+
+  <node_container pkg="rclcpp_components" exec="component_container" name="mission_planner_container" namespace="">
+    <composable_node pkg="mission_planner" plugin="mission_planner::MissionPlanner" name="mission_planner" namespace="">
+      <param from="$(var mission_planner_param_path)"/>
+      <remap from="input/modified_goal" to="$(var modified_goal_topic_name)"/>
+      <remap from="input/vector_map" to="$(var map_topic_name)"/>
+      <remap from="debug/route_marker" to="$(var visualization_topic_name)"/>
+    </composable_node>
+    <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+  </node_container>
 
   <group>
     <include file="$(find-pkg-share mission_planner)/launch/goal_pose_visualizer.launch.xml"/>
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
index 665bd82423ea6..b293c5836817d 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -24,15 +24,25 @@
     </group>
     <!-- motion velocity smoother -->
     <group>
-      <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
-      <set_remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
-      <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
-        <arg name="velocity_smoother_type" value="$(var velocity_smoother_type)"/>
-        <arg name="common_param_path" value="$(var common_param_path)"/>
-        <arg name="nearest_search_param_path" value="$(var nearest_search_param_path)"/>
-        <arg name="param_path" value="$(var motion_velocity_smoother_param_path)"/>
-        <arg name="velocity_smoother_param_path" value="$(var velocity_smoother_type_param_path)"/>
-      </include>
+      <node_container pkg="rclcpp_components" exec="component_container" name="motion_velocity_smoother_container" namespace="">
+        <composable_node pkg="motion_velocity_smoother" plugin="motion_velocity_smoother::MotionVelocitySmootherNode" name="motion_velocity_smoother" namespace="">
+          <param name="algorithm_type" value="$(var velocity_smoother_type)"/>
+          <param from="$(var common_param_path)"/>
+          <param from="$(var nearest_search_param_path)"/>
+          <param from="$(var motion_velocity_smoother_param_path)"/>
+          <param from="$(var velocity_smoother_type_param_path)"/>
+
+          <param name="publish_debug_trajs" value="false"/>
+          <remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
+          <remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
+
+          <remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity"/>
+          <remap from="~/input/acceleration" to="/localization/acceleration"/>
+          <remap from="~/input/operation_mode_state" to="/system/operation_mode/state"/>
+          <remap from="~/output/current_velocity_limit_mps" to="/planning/scenario_planning/current_max_velocity"/>
+        </composable_node>
+        <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+      </node_container>
     </group>
   </group>
 
diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md
index 63095a72e835e..e31db7890cf47 100644
--- a/map/map_projection_loader/README.md
+++ b/map/map_projection_loader/README.md
@@ -25,7 +25,7 @@ sample-map-rosbag
 
 ```yaml
 # map_projector_info.yaml
-type: "Local"
+type: "local"
 ```
 
 ### Using MGRS
diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml
index cb9a61b600e43..1f6def24912a2 100644
--- a/perception/map_based_prediction/config/map_based_prediction.param.yaml
+++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml
@@ -11,7 +11,10 @@
     sigma_yaw_angle_deg: 5.0 #[angle degree]
     object_buffer_time_length: 2.0 #[s]
     history_time_length: 1.0 #[s]
+    # parameter for shoulder lane prediction
+    prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
 
+    # parameters for lc prediction
     lane_change_detection:
       method: "lat_diff_distance" # choose from "lat_diff_distance" or "time_to_change_lane"
       time_to_change_lane:
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index be6327cf5c01a..8d633dbd5af64 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -639,33 +639,31 @@ lanelet::Lanelets getLeftOppositeLanelets(
 MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options)
 : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0)
 {
-  enable_delay_compensation_ = declare_parameter("enable_delay_compensation", true);
-  prediction_time_horizon_ = declare_parameter("prediction_time_horizon", 10.0);
-  prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time", 0.5);
+  enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
+  prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
+  prediction_sampling_time_interval_ = declare_parameter<double>("prediction_sampling_delta_time");
   min_velocity_for_map_based_prediction_ =
-    declare_parameter("min_velocity_for_map_based_prediction", 1.0);
-  min_crosswalk_user_velocity_ = declare_parameter("min_crosswalk_user_velocity", 1.0);
+    declare_parameter<double>("min_velocity_for_map_based_prediction");
+  min_crosswalk_user_velocity_ = declare_parameter<double>("min_crosswalk_user_velocity");
   dist_threshold_for_searching_lanelet_ =
-    declare_parameter("dist_threshold_for_searching_lanelet", 3.0);
+    declare_parameter<double>("dist_threshold_for_searching_lanelet");
   delta_yaw_threshold_for_searching_lanelet_ =
-    declare_parameter("delta_yaw_threshold_for_searching_lanelet", 0.785);
-  sigma_lateral_offset_ = declare_parameter("sigma_lateral_offset", 0.5);
-  sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg", 5.0);
-  object_buffer_time_length_ = declare_parameter("object_buffer_time_length", 2.0);
-  history_time_length_ = declare_parameter("history_time_length", 1.0);
+    declare_parameter<double>("delta_yaw_threshold_for_searching_lanelet");
+  sigma_lateral_offset_ = declare_parameter<double>("sigma_lateral_offset");
+  sigma_yaw_angle_deg_ = declare_parameter<double>("sigma_yaw_angle_deg");
+  object_buffer_time_length_ = declare_parameter<double>("object_buffer_time_length");
+  history_time_length_ = declare_parameter<double>("history_time_length");
   {  // lane change detection
     lane_change_detection_method_ = declare_parameter<std::string>("lane_change_detection.method");
 
     // lane change detection by time_to_change_lane
-    dist_threshold_to_bound_ = declare_parameter(
-      "lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection",
-      1.0);  // 1m
-    time_threshold_to_bound_ = declare_parameter(
-      "lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection",
-      5.0);  // 5 sec
-    cutoff_freq_of_velocity_lpf_ = declare_parameter(
-      "lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_detection",
-      0.1);  // 0.1Hz
+    dist_threshold_to_bound_ = declare_parameter<double>(
+      "lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection");  // 1m
+    time_threshold_to_bound_ = declare_parameter<double>(
+      "lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection");
+    cutoff_freq_of_velocity_lpf_ = declare_parameter<double>(
+      "lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_"
+      "detection");
 
     // lane change detection by lat_diff_distance
     dist_ratio_threshold_to_left_bound_ = declare_parameter<double>(
@@ -680,11 +678,11 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     num_continuous_state_transition_ =
       declare_parameter<int>("lane_change_detection.num_continuous_state_transition");
   }
-  reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5);
+  reference_path_resolution_ = declare_parameter<double>("reference_path_resolution");
   /* prediction path will disabled when the estimated path length exceeds lanelet length. This
    * parameter control the estimated path length = vx * th * (rate)  */
   prediction_time_horizon_rate_for_validate_lane_length_ =
-    declare_parameter("prediction_time_horizon_rate_for_validate_lane_length", 0.8);
+    declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");
 
   path_generator_ = std::make_shared<PathGenerator>(
     prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_);
diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp
index 4cd5569129a00..bdfd07ae54b28 100644
--- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp
+++ b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp
@@ -113,7 +113,7 @@ class MultiCameraFusion : public rclcpp::Node
   /*
   the mapping from traffic light id (instance id) to regulatory element id (group id)
   */
-  std::map<lanelet::Id, lanelet::Id> traffic_light_id_to_regulatory_ele_id_;
+  std::map<lanelet::Id, std::vector<lanelet::Id>> traffic_light_id_to_regulatory_ele_id_;
   /*
   save record arrays by increasing timestamp order.
   use multiset in case there are multiple cameras publishing images at exactly the same time
diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp
index 387b3ef1f6758..6c29be9858a9a 100644
--- a/perception/traffic_light_multi_camera_fusion/src/node.cpp
+++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp
@@ -216,7 +216,7 @@ void MultiCameraFusion::mapCallback(
 
     auto lights = tl->trafficLights();
     for (const auto & light : lights) {
-      traffic_light_id_to_regulatory_ele_id_[light.id()] = tl->id();
+      traffic_light_id_to_regulatory_ele_id_[light.id()].emplace_back(tl->id());
     }
   }
 }
@@ -302,11 +302,15 @@ void MultiCameraFusion::groupFusion(
     if (traffic_light_id_to_regulatory_ele_id_.count(roi_id) == 0) {
       RCLCPP_WARN_STREAM(
         get_logger(), "Found Traffic Light Id = " << roi_id << " which is not defined in Map");
-    } else {
-      /*
-      keep the best record for every regulatory element id
-      */
-      IdType reg_ele_id = traffic_light_id_to_regulatory_ele_id_[p.second.roi.traffic_light_id];
+      continue;
+    }
+
+    /*
+    keep the best record for every regulatory element id
+    */
+    const auto reg_ele_id_vec =
+      traffic_light_id_to_regulatory_ele_id_[p.second.roi.traffic_light_id];
+    for (const auto & reg_ele_id : reg_ele_id_vec) {
       if (
         grouped_record_map.count(reg_ele_id) == 0 ||
         ::compareRecord(p.second, grouped_record_map[reg_ele_id]) >= 0) {
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
index 9ecc1a63b55b0..26f53cc91c4cf 100644
--- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
+++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -142,7 +142,8 @@
         time_resolution: 0.5                             # [s]
         time_horizon: 10.0                               # [s]
         safety_check_backward_distance: 100.0            # [m]
-        safety_check_hysteresis_factor: 2.0              # [-]
+        hysteresis_factor_expand_rate: 2.0               # [-]
+        hysteresis_factor_safe_count: 10                 # [-]
         # rss parameters
         expected_front_deceleration: -1.0                # [m/ss]
         expected_rear_deceleration: -1.0                 # [m/ss]
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 b3cb53ec18349..0fc10109240f9 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
@@ -131,6 +131,10 @@ MarkerArray createObjectsMarkerArray(
 MarkerArray createDrivableLanesMarkerArray(
   const std::vector<DrivableLanes> & drivable_lanes, std::string && ns);
 
+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);
+
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
index 7889e91767155..096ae61a9432e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -556,6 +556,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   bool arrived_path_end_{false};
 
+  bool safe_{true};
+
   std::shared_ptr<AvoidanceParameters> parameters_;
 
   helper::avoidance::AvoidanceHelper helper_;
@@ -576,6 +578,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   ObjectDataArray registered_objects_;
 
+  mutable size_t safe_count_{0};
+
   mutable ObjectDataArray ego_stopped_objects_;
 
   mutable ObjectDataArray stopped_objects_;
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 1f47b804e185b..5484ef834a177 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
@@ -194,7 +194,8 @@ struct AvoidanceParameters
   double safety_check_backward_distance;
 
   // transit hysteresis (unsafe to safe)
-  double safety_check_hysteresis_factor;
+  size_t hysteresis_factor_safe_count;
+  double hysteresis_factor_expand_rate;
 
   // keep target velocity in yield maneuver
   double yield_velocity;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp
index b4575eb4d3b7e..8a3b7094c69d1 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp
@@ -55,24 +55,37 @@ PredictedObjects filterObjects(
   const std::shared_ptr<ObjectsFilteringParams> & params);
 
 /**
- * @brief Filter objects based on their velocity.
+ * @brief Filters objects based on their velocity.
  *
- * @param objects The predicted objects to filter.
- * @param lim_v Velocity limit for filtering.
- * @return PredictedObjects The filtered objects.
+ * Depending on the remove_above_threshold parameter, this function either removes objects with
+ * velocities above the given threshold or only keeps those objects. It uses the helper function
+ * filterObjectsByVelocity() to do the actual filtering.
+ *
+ * @param objects The objects to be filtered.
+ * @param velocity_threshold The velocity threshold for the filtering.
+ * @param remove_above_threshold If true, objects with velocities above the threshold are removed.
+ *                               If false, only objects with velocities above the threshold are
+ * kept.
+ * @return A new collection of objects that have been filtered according to the rules.
  */
-PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v);
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, const double velocity_threshold,
+  const bool remove_above_threshold = true);
 
 /**
- * @brief Filter objects based on a velocity range.
+ * @brief Helper function to filter objects based on their velocity.
  *
- * @param objects The predicted objects to filter.
- * @param min_v Minimum velocity for filtering.
- * @param max_v Maximum velocity for filtering.
- * @return PredictedObjects The filtered objects.
+ * This function iterates over all objects and calculates their velocity norm. If the velocity norm
+ * is within the velocity_threshold and max_velocity range, the object is added to a new collection.
+ * This new collection is then returned.
+ *
+ * @param objects The objects to be filtered.
+ * @param velocity_threshold The minimum velocity for an object to be included in the output.
+ * @param max_velocity The maximum velocity for an object to be included in the output.
+ * @return A new collection of objects that have been filtered according to the rules.
  */
 PredictedObjects filterObjectsByVelocity(
-  const PredictedObjects & objects, double min_v, double max_v);
+  const PredictedObjects & objects, double velocity_threshold, double max_velocity);
 
 /**
  * @brief Filter objects based on their position relative to a current_pose.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp
index 6d9df7677ead5..79947d826ed10 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp
@@ -63,6 +63,9 @@ Polygon2d createExtendedPolygon(
   const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
   CollisionCheckDebug & debug);
 
+PredictedPath convertToPredictedPath(
+  const std::vector<PoseWithVelocityStamped> & path, const double time_resolution);
+
 double calcRssDistance(
   const double front_object_velocity, const double rear_object_velocity,
   const RSSparams & rss_params);
@@ -98,7 +101,7 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  CollisionCheckDebug & debug);
+  const double hysteresis_factor, CollisionCheckDebug & debug);
 
 /**
  * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp
index d8c87b6d1b291..40163f4f93d8a 100644
--- a/planning/behavior_path_planner/src/marker_utils/utils.cpp
+++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp
@@ -408,4 +408,59 @@ MarkerArray createDrivableLanesMarkerArray(
 
   return msg;
 }
+MarkerArray createPredictedPathMarkerArray(
+  const PredictedPath & 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)
+{
+  if (predicted_path.path.empty()) {
+    return MarkerArray{};
+  }
+
+  const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
+  MarkerArray msg;
+
+  const auto & path = predicted_path.path;
+
+  Marker marker = createDefaultMarker(
+    "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1),
+
+    createMarkerColor(r, g, b, 0.999));
+  marker.lifetime = rclcpp::Duration::from_seconds(1.5);
+
+  MarkerArray marker_array;
+  for (size_t i = 0; i < path.size(); ++i) {
+    marker.id = i + id;
+    marker.points.clear();
+
+    const auto & predicted_path_pose = path.at(i);
+    const double half_width = vehicle_info.vehicle_width_m / 2.0;
+    const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m;
+    const double base_to_rear = vehicle_info.rear_overhang_m;
+
+    marker.points.push_back(
+      tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, -half_width, 0.0)
+        .position);
+    marker.points.push_back(
+      tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, half_width, 0.0)
+        .position);
+    marker.points.push_back(
+      tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, half_width, 0.0)
+        .position);
+    marker.points.push_back(
+      tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, -half_width, 0.0)
+        .position);
+    marker.points.push_back(marker.points.front());
+
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+
+  marker.points.reserve(path.size());
+  for (const auto & point : path) {
+    marker.points.push_back(point.position);
+  }
+  msg.markers.push_back(marker);
+  return msg;
+}
+
 }  // 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 70999f1504fee..e0c79a46d731e 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
@@ -461,7 +461,7 @@ void AvoidanceModule::fillEgoStatus(
   if (isOutputPathLocked()) {
     data.safe_new_sl.clear();
     data.candidate_path = helper_.getPreviousSplineShiftPath();
-    RCLCPP_WARN_THROTTLE(
+    RCLCPP_DEBUG_THROTTLE(
       getLogger(), *clock_, 500, "this module is locked now. keep current path.");
     return;
   }
@@ -1869,6 +1869,8 @@ bool AvoidanceModule::isSafePath(
     return true;
   }
 
+  const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate;
+
   const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects(
     avoid_data_, planner_data_, parameters_, is_right_shift.value());
 
@@ -1879,13 +1881,16 @@ bool AvoidanceModule::isSafePath(
       CollisionCheckDebug collision{};
       if (!utils::path_safety_checker::checkCollision(
             shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
-            collision)) {
+            hysteresis_factor, collision)) {
+        safe_count_ = 0;
         return false;
       }
     }
   }
 
-  return true;
+  safe_count_++;
+
+  return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count;
 }
 
 void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const
@@ -2455,7 +2460,7 @@ bool AvoidanceModule::isValidShiftLine(
     constexpr double THRESHOLD = 0.1;
     const auto offset = std::abs(new_shift_length - helper_.getEgoShift());
     if (offset > THRESHOLD) {
-      RCLCPP_WARN_THROTTLE(
+      RCLCPP_DEBUG_THROTTLE(
         getLogger(), *clock_, 1000, "new shift line is invalid. [HUGE OFFSET (%.2f)]", offset);
       return false;
     }
@@ -2505,6 +2510,8 @@ void AvoidanceModule::updateData()
 
   // update rtc status.
   updateRTCData();
+
+  safe_ = avoid_data_.safe;
 }
 
 void AvoidanceModule::processOnEntry()
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
index c13c237673625..2d4a6b9c42b98 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
@@ -145,8 +145,9 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.safety_check_time_resolution = get_parameter<double>(node, ns + "time_resolution");
     p.safety_check_backward_distance =
       get_parameter<double>(node, ns + "safety_check_backward_distance");
-    p.safety_check_hysteresis_factor =
-      get_parameter<double>(node, ns + "safety_check_hysteresis_factor");
+    p.hysteresis_factor_expand_rate =
+      get_parameter<double>(node, ns + "hysteresis_factor_expand_rate");
+    p.hysteresis_factor_safe_count = get_parameter<int>(node, ns + "hysteresis_factor_safe_count");
   }
 
   // safety check rss params
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
index f392ef6a26e22..31aab9da87e2a 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
@@ -288,8 +288,8 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
   // safety check
   {
     std::string ns = "avoidance.safety_check.";
-    p.safety_check_hysteresis_factor =
-      get_parameter<double>(node, ns + "safety_check_hysteresis_factor");
+    p.hysteresis_factor_expand_rate =
+      get_parameter<double>(node, ns + "hysteresis_factor_expand_rate");
   }
 
   avoidance_parameters_ = std::make_shared<AvoidanceByLCParameters>(p);
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
index 3db3e049c3c81..251720f4202d5 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -1380,7 +1380,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
       obj, lane_change_parameters_->use_all_predicted_path);
     for (const auto & obj_path : obj_predicted_paths) {
       if (!utils::path_safety_checker::checkCollision(
-            path, ego_predicted_path, obj, obj_path, common_parameters, rss_params,
+            path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
             current_debug_data.second)) {
         path_safety_status.is_safe = false;
         updateDebugInfo(current_debug_data, path_safety_status.is_safe);
diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
index 4cbfebef07a39..c1a883d7a9b82 100644
--- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -771,7 +771,7 @@ void fillAvoidanceNecessity(
   }
 
   // TRUE -> ? (check with hysteresis factor)
-  object_data.avoid_required = check_necessity(parameters->safety_check_hysteresis_factor);
+  object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate);
 }
 
 void fillObjectStoppableJudge(
diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
index 33afe0fe6642f..a775e7c16efed 100644
--- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
+++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
@@ -37,7 +37,7 @@ PredictedObjects filterObjects(
 
   PredictedObjects filtered_objects;
 
-  filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold);
+  filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false);
 
   filterObjectsByClass(filtered_objects, target_object_types);
 
@@ -51,13 +51,19 @@ PredictedObjects filterObjects(
   return filtered_objects;
 }
 
-PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v)
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, const double velocity_threshold,
+  const bool remove_above_threshold)
 {
-  return filterObjectsByVelocity(objects, -lim_v, lim_v);
+  if (remove_above_threshold) {
+    return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold);
+  } else {
+    return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits<double>::max());
+  }
 }
 
 PredictedObjects filterObjectsByVelocity(
-  const PredictedObjects & objects, double min_v, double max_v)
+  const PredictedObjects & objects, double velocity_threshold, double max_velocity)
 {
   PredictedObjects filtered;
   filtered.header = objects.header;
@@ -65,7 +71,7 @@ PredictedObjects filterObjectsByVelocity(
     const auto v_norm = std::hypot(
       obj.kinematics.initial_twist_with_covariance.twist.linear.x,
       obj.kinematics.initial_twist_with_covariance.twist.linear.y);
-    if (min_v < v_norm && v_norm < max_v) {
+    if (velocity_threshold < v_norm && v_norm < max_velocity) {
       filtered.objects.push_back(obj);
     }
   }
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 c6a68e108e7e1..579862de93865 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
@@ -133,6 +133,19 @@ Polygon2d createExtendedPolygon(
            : tier4_autoware_utils::inverseClockwise(polygon);
 }
 
+PredictedPath convertToPredictedPath(
+  const std::vector<PoseWithVelocityStamped> & path, const double time_resolution)
+{
+  PredictedPath predicted_path;
+  predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution);
+  predicted_path.path.resize(path.size());
+
+  for (size_t i = 0; i < path.size(); ++i) {
+    predicted_path.path.at(i) = path.at(i).pose;
+  }
+  return predicted_path;
+}
+
 double calcRssDistance(
   const double front_object_velocity, const double rear_object_velocity,
   const RSSparams & rss_params)
@@ -219,7 +232,7 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  CollisionCheckDebug & debug)
+  double hysteresis_factor, CollisionCheckDebug & debug)
 {
   debug.lerped_path.reserve(target_object_path.path.size());
 
@@ -273,8 +286,8 @@ bool checkCollision(
     const auto min_lon_length =
       calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters);
 
-    const auto & lon_offset = std::max(rss_dist, min_lon_length);
-    const auto & lat_margin = rss_parameters.lateral_distance_max_threshold;
+    const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor;
+    const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor;
     const auto & extended_ego_polygon =
       is_object_front
         ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug)
diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
index 653efff7058d2..9ca083af35c92 100644
--- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
+++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
@@ -321,7 +321,7 @@ std::pair<std::vector<double>, std::vector<double>> PathShifter::calcBaseLengths
 
   if (lateral_a_max < lateral_acc_limit_) {
     // no need to consider acceleration limit
-    RCLCPP_WARN_THROTTLE(
+    RCLCPP_DEBUG_THROTTLE(
       logger_, clock_, 3000, "No need to consider lateral acc limit. max: %f, limit: %f",
       lateral_a_max, lateral_acc_limit_);
     return getBaseLengthsWithoutAccelLimit(S, shift_length, v0, a, T, offset_back);
diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt
index 118931780a92c..3217b3828b8d7 100644
--- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt
+++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt
@@ -13,3 +13,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 
 ament_auto_package(INSTALL_TO_SHARE config)
+
+install(PROGRAMS
+  scripts/time_to_collision_plotter.py
+  DESTINATION lib/${PROJECT_NAME}
+)
diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md
index f85c472c6d856..5f82fe839fda6 100644
--- a/planning/behavior_velocity_crosswalk_module/README.md
+++ b/planning/behavior_velocity_crosswalk_module/README.md
@@ -218,6 +218,16 @@ When multiple crosswalks are nearby (such as intersection), this module may make
 
 ### Known Issues
 
+### Debugging
+
+By `ros2 run behavior_velocity_crosswalk_module time_to_collision_plotter.py`, you can visualize the following figure of the ego and pedestrian's time to collision.
+The label of each plot is `<crosswalk module id>-<pedestrian uuid>`.
+
+<figure markdown>
+  ![limitation](docs/time_to_collision_plot.png){width=1000}
+  <figcaption>Plot of time to collision</figcaption>
+</figure>
+
 ### References/External links
 
 [1] 佐藤 みなみ, 早坂 祥一, 清水 政行, 村野 隆彦, 横断歩行者に対するドライバのリスク回避行動のモデル化, 自動車技術会論文集, 2013, 44 巻, 3 号, p. 931-936.
diff --git a/planning/behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png b/planning/behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png
new file mode 100644
index 0000000000000..71ce642f382bc
Binary files /dev/null and b/planning/behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png differ
diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp
index 578ae66e40006..dfe682d0aa0c9 100644
--- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp
+++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp
@@ -26,6 +26,7 @@
 #include <optional>
 #include <set>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -73,7 +74,7 @@ struct DebugData
   std::optional<geometry_msgs::msg::Point> range_near_point{std::nullopt};
   std::optional<geometry_msgs::msg::Point> range_far_point{std::nullopt};
 
-  std::vector<std::pair<CollisionPoint, CollisionState>> collision_points;
+  std::vector<std::tuple<std::string, CollisionPoint, CollisionState>> collision_points;
 
   std::vector<geometry_msgs::msg::Pose> stop_poses;
   std::vector<geometry_msgs::msg::Pose> slow_poses;
diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml
index d5ef92a4c097d..75f96c9194aef 100644
--- a/planning/behavior_velocity_crosswalk_module/package.xml
+++ b/planning/behavior_velocity_crosswalk_module/package.xml
@@ -34,6 +34,7 @@
   <depend>sensor_msgs</depend>
   <depend>tier4_api_msgs</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>vehicle_info_util</depend>
   <depend>visualization_msgs</depend>
 
diff --git a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py
new file mode 100755
index 0000000000000..5093683e17c22
--- /dev/null
+++ b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py
@@ -0,0 +1,212 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import argparse
+from collections import deque
+from copy import deepcopy
+
+from ament_index_python.packages import get_package_share_directory
+import matplotlib.pyplot as plt
+import rclpy
+from rclpy.node import Node
+from tier4_debug_msgs.msg import StringStamped
+import yaml
+
+
+def expand_margin(x_vec, y_vec, min_x=0.0, max_x=10.0):
+    expanded_x_vec = deepcopy(x_vec)
+    expanded_y_vec = deepcopy(y_vec)
+
+    if min_x < expanded_x_vec[0]:
+        expanded_x_vec = [min_x] + expanded_x_vec
+        min_y = expanded_y_vec[0] + min_x - expanded_x_vec[0]
+        expanded_y_vec = [min_y] + expanded_y_vec
+
+    if expanded_x_vec[-1] < max_x:
+        expanded_x_vec.append(max_x)
+        max_y = expanded_y_vec[-1] + max_x - expanded_x_vec[-2]
+        expanded_y_vec.append(max_y)
+
+    return expanded_x_vec, expanded_y_vec
+
+
+class CollisionInfo:
+    def __init__(self, module_id, obj_id, ttc, ttv, state):
+        self.module_id = module_id
+        self.obj_id = obj_id
+        self.ttc = ttc
+        self.ttv = ttv
+        self.state = state
+
+
+class TimeToCollisionPlotter(Node):
+    def __init__(self, args):
+        super().__init__("time_to_collision_plotter")
+
+        self.depth = args.depth
+
+        self.sub_calculation_cost = self.create_subscription(
+            StringStamped,
+            "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/collision_info",
+            self.on_collision_info,
+            1,
+        )
+
+        self.time_to_collision_hist = {}
+        self.max_hist_size = args.depth
+
+        crosswalk_config = (
+            get_package_share_directory("autoware_launch")
+            + "/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml"
+        )
+        with open(crosswalk_config) as f:
+            crosswalk_config_obj = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+            ego_pass_first_x = crosswalk_config_obj["crosswalk"]["pass_judge"][
+                "ego_pass_first_margin_x"
+            ]
+            ego_pass_first_y = crosswalk_config_obj["crosswalk"]["pass_judge"][
+                "ego_pass_first_margin_y"
+            ]
+            self.ego_pass_first_margin = [ego_pass_first_x, ego_pass_first_y]
+
+            ego_pass_later_x = crosswalk_config_obj["crosswalk"]["pass_judge"][
+                "ego_pass_later_margin_x"
+            ]
+            ego_pass_later_y = crosswalk_config_obj["crosswalk"]["pass_judge"][
+                "ego_pass_later_margin_y"
+            ]
+            self.ego_pass_later_margin = [ego_pass_later_x, ego_pass_later_y]
+
+        plt.ion()
+        _, self.ax = plt.subplots()
+        plt.show()
+        plt.connect("key_press_event", self.on_key)
+
+        self.timer = self.create_timer(0.3, self.on_timer)
+
+    def on_key(self, event):
+        if event.key == "c":
+            self.time_to_collision_hist = {}
+
+    def on_collision_info(self, msg):
+        collision_info_data_vec = []
+        collision_info_str_vec = msg.data[:-1].split(",")
+        if len(collision_info_str_vec) < 5:
+            return
+
+        for i in range(int(len(collision_info_str_vec) / 5)):
+            collision_info_data_vec.append(CollisionInfo(*collision_info_str_vec[i : i + 5]))
+
+        # memorize data in the history dictionary
+        for collision_info_data in collision_info_data_vec:
+            uuid = collision_info_data.module_id + collision_info_data.obj_id
+            if uuid not in self.time_to_collision_hist:
+                self.time_to_collision_hist[uuid] = deque([])
+
+            self.time_to_collision_hist[uuid].append(
+                [float(collision_info_data.ttv), float(collision_info_data.ttc)]
+            )
+
+    def on_timer(self):
+        # make the size of history not exceed the maximum value
+        for uuid in self.time_to_collision_hist:
+            while self.max_hist_size < len(self.time_to_collision_hist[uuid]):
+                self.time_to_collision_hist[uuid].popleft()
+
+        plt.cla()
+
+        # plot "ego_pass_later" border
+        ego_pass_later_x = []
+        ego_pass_later_y = []
+        for i in range(len(self.ego_pass_later_margin[0])):
+            ego_pass_later_x.append(self.ego_pass_later_margin[0][i])
+            ego_pass_later_y.append(self.ego_pass_later_margin[1][i])
+        expanded_ego_pass_later_x, expanded_ego_pass_later_y = expand_margin(
+            ego_pass_later_x, ego_pass_later_y
+        )
+        plt.fill_between(
+            expanded_ego_pass_later_x,
+            expanded_ego_pass_later_y,
+            [expanded_ego_pass_later_y[-1] for i in expanded_ego_pass_later_y],
+            alpha=0.2,
+            color="green",
+        )
+
+        # plot "ego_pass_first" border
+        ego_pass_first_x = []
+        ego_pass_first_y = []
+        for i in range(len(self.ego_pass_first_margin[0])):
+            ego_pass_first_x.append(self.ego_pass_first_margin[0][i])
+            ego_pass_first_y.append(self.ego_pass_first_margin[1][i])
+        expanded_ego_pass_first_x, expanded_ego_pass_first_y = expand_margin(
+            ego_pass_first_x, ego_pass_first_y
+        )
+        plt.fill_between(
+            expanded_ego_pass_first_y,
+            [expanded_ego_pass_first_x[0] for i in expanded_ego_pass_first_x],
+            expanded_ego_pass_first_x,
+            alpha=0.2,
+            color="blue",
+        )
+        plt.text(
+            expanded_ego_pass_later_x[0],
+            expanded_ego_pass_later_y[-1],
+            "Ego passes later.",
+            va="top",
+            ha="left",
+            color="green",
+        )
+        plt.text(
+            expanded_ego_pass_first_y[-1],
+            expanded_ego_pass_first_x[0],
+            "Ego passes first.",
+            va="bottom",
+            ha="right",
+            color="blue",
+        )
+        plt.text(0, 0, "Ego yields.", va="bottom", ha="left", color="red")
+
+        # plot history
+        for uuid in self.time_to_collision_hist:
+            hist = self.time_to_collision_hist[uuid]
+            plt.plot(
+                [val[0] for val in hist],
+                [val[1] for val in hist],
+                label=uuid[0:4] + "-" + uuid[4:8],
+            )
+
+        plt.legend()
+        plt.draw()
+        plt.title("Time to collision")
+        plt.xlabel("Pedestrian's time to collision")
+        plt.ylabel("Ego's time to collision")
+        plt.pause(0.01)
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "-d",
+        "--depth",
+        type=float,
+        default=100,
+    )
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = TimeToCollisionPlotter(args)
+    rclpy.spin(node)
diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp
index 409898233cbbe..a5bc3ebf72179 100644
--- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp
+++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp
@@ -44,8 +44,8 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers(
   {
     for (size_t i = 0; i < debug_data.collision_points.size(); ++i) {
       const auto & collision_point = debug_data.collision_points.at(i);
-      const auto & point = collision_point.first;
-      const auto & state = collision_point.second;
+      const auto & point = std::get<1>(collision_point);
+      const auto & state = std::get<2>(collision_point);
 
       auto marker = createDefaultMarker(
         "map", now, "collision point state", uid + i++, Marker::TEXT_VIEW_FACING,
@@ -134,7 +134,7 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers(
       "map", now, "collision point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0),
       createMarkerColor(1.0, 0.0, 1.0, 0.999));
     for (const auto & collision_point : debug_data.collision_points) {
-      marker.points.push_back(collision_point.first.collision_point);
+      marker.points.push_back(std::get<1>(collision_point).collision_point);
     }
     msg.markers.push_back(marker);
   }
diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp
index 992d1c48eb9f4..96e4e873ce111 100644
--- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp
+++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp
@@ -129,7 +129,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
     const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
 
     registerModule(std::make_shared<CrosswalkModule>(
-      id, lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
+      node_, id, lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
     generateUUID(id);
     updateRTCStatus(getUUID(id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
   };
diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 64410c08a6f80..3b69b74c53d80 100644
--- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -25,6 +25,7 @@
 #include <cmath>
 #include <limits>
 #include <set>
+#include <tuple>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -164,10 +165,27 @@ std::optional<geometry_msgs::msg::Pose> toStdOptional(
   }
   return std::nullopt;
 }
+
+tier4_debug_msgs::msg::StringStamped createStringStampedMessage(
+  const rclcpp::Time & now, const int64_t module_id_,
+  const std::vector<std::tuple<std::string, CollisionPoint, CollisionState>> & collision_points)
+{
+  tier4_debug_msgs::msg::StringStamped msg;
+  msg.stamp = now;
+  for (const auto & collision_point : collision_points) {
+    std::stringstream ss;
+    ss << module_id_ << "," << std::get<0>(collision_point).substr(0, 4) << ","
+       << std::get<1>(collision_point).time_to_collision << ","
+       << std::get<1>(collision_point).time_to_vehicle << ","
+       << static_cast<int>(std::get<2>(collision_point)) << ",";
+    msg.data += ss.str();
+  }
+  return msg;
+}
 }  // namespace
 
 CrosswalkModule::CrosswalkModule(
-  const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  rclcpp::Node & node, const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   const PlannerParam & planner_param, const bool use_regulatory_element,
   const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
@@ -190,6 +208,9 @@ CrosswalkModule::CrosswalkModule(
     }
     crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id);
   }
+
+  collision_info_pub_ =
+    node.create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
 }
 
 bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
@@ -264,6 +285,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   }
   recordTime(4);
 
+  const auto collision_info_msg =
+    createStringStampedMessage(clock_->now(), module_id_, debug_data_.collision_points);
+  collision_info_pub_->publish(collision_info_msg);
+
   return true;
 }
 
@@ -882,7 +907,8 @@ void CrosswalkModule::updateObjectState(
 
     if (collision_point) {
       const auto collision_state = object_info_manager_.getCollisionState(obj_uuid);
-      debug_data_.collision_points.push_back(std::make_pair(*collision_point, collision_state));
+      debug_data_.collision_points.push_back(
+        std::make_tuple(obj_uuid, *collision_point, collision_state));
     }
   }
   object_info_manager_.finalize();
@@ -1034,6 +1060,9 @@ void CrosswalkModule::setDistanceToStop(
 void CrosswalkModule::planGo(
   PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor) const
 {
+  if (!stop_factor.has_value()) {
+    return;
+  }
   // Plan slow down
   const auto target_velocity = calcTargetVelocity(stop_factor->stop_pose.position, ego_path);
   insertDecelPointWithDebugInfo(
diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
index 961126bf4bca1..edff62371f6b4 100644
--- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -26,6 +26,7 @@
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_debug_msgs/msg/string_stamped.hpp>
 
 #include <boost/assert.hpp>
 #include <boost/assign/list_of.hpp>
@@ -257,7 +258,7 @@ class CrosswalkModule : public SceneModuleInterface
   };
 
   CrosswalkModule(
-    const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+    rclcpp::Node & node, const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
     const PlannerParam & planner_param, const bool use_regulatory_element,
     const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock);
 
@@ -360,6 +361,8 @@ class CrosswalkModule : public SceneModuleInterface
 
   const int64_t module_id_;
 
+  rclcpp::Publisher<tier4_debug_msgs::msg::StringStamped>::SharedPtr collision_info_pub_;
+
   lanelet::ConstLanelet crosswalk_;
 
   lanelet::ConstLineStrings3d stop_lines_;
diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
index e2012fab43ba4..08ebea4284bf1 100644
--- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
+++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -20,6 +20,7 @@
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]
+      suppress_sudden_obstacle_stop: false
 
       stop_obstacle_type:
         unknown: true
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
index 260a4c6400588..ac6684d163aea 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -100,6 +100,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   bool enable_debug_info_;
   bool enable_calculation_time_info_;
   double min_behavior_stop_margin_;
+  bool suppress_sudden_obstacle_stop_;
 
   std::vector<int> stop_obstacle_types_;
   std::vector<int> inside_cruise_obstacle_types_;
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
index 9be6ec10e952e..c3fa364da269e 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
@@ -53,11 +53,12 @@ class PlannerInterface
 
   void setParam(
     const bool enable_debug_info, const bool enable_calculation_time_info,
-    const double min_behavior_stop_margin)
+    const double min_behavior_stop_margin, const bool suppress_sudden_obstacle_stop)
   {
     enable_debug_info_ = enable_debug_info;
     enable_calculation_time_info_ = enable_calculation_time_info;
     min_behavior_stop_margin_ = min_behavior_stop_margin;
+    suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop;
   }
 
   std::vector<TrajectoryPoint> generateStopTrajectory(
@@ -101,6 +102,7 @@ class PlannerInterface
   bool enable_calculation_time_info_{false};
   LongitudinalInfo longitudinal_info_;
   double min_behavior_stop_margin_;
+  bool suppress_sudden_obstacle_stop_;
 
   // stop watch
   tier4_autoware_utils::StopWatch<
diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml
index 6cfbf9225dd7e..f482be8ebafe7 100644
--- a/planning/obstacle_cruise_planner/package.xml
+++ b/planning/obstacle_cruise_planner/package.xml
@@ -6,6 +6,8 @@
 
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="yutaka.shimizu@tier4.jp">Yutaka Shimizu</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
 
   <license>Apache License 2.0</license>
 
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index 70c0c687b2514..266e184a06a08 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -397,8 +397,11 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
     }
 
     min_behavior_stop_margin_ = declare_parameter<double>("common.min_behavior_stop_margin");
+    suppress_sudden_obstacle_stop_ =
+      declare_parameter<bool>("common.suppress_sudden_obstacle_stop");
     planner_ptr_->setParam(
-      enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_);
+      enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_,
+      suppress_sudden_obstacle_stop_);
   }
 
   {  // stop/cruise/slow down obstacle type
@@ -436,7 +439,8 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
   tier4_autoware_utils::updateParam<bool>(
     parameters, "common.enable_calculation_time_info", enable_calculation_time_info_);
   planner_ptr_->setParam(
-    enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_);
+    enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_,
+    suppress_sudden_obstacle_stop_);
 
   tier4_autoware_utils::updateParam<bool>(
     parameters, "common.enable_slow_down_planning", enable_slow_down_planning_);
diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
index 4cf6bf9e23806..03cae6e6f9d88 100644
--- a/planning/obstacle_cruise_planner/src/planner_interface.cpp
+++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -300,26 +300,31 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     return longitudinal_info_.safe_distance_margin;
   }();
 
-  // Calculate feasible stop margin (Check the feasibility)
-  const double feasible_stop_dist = calcMinimumDistanceToStop(
-                                      planner_data.ego_vel, longitudinal_info_.limit_max_accel,
-                                      longitudinal_info_.limit_min_accel) +
-                                    dist_to_ego;
-  const double closest_obstacle_stop_dist =
-    closest_obstacle_dist - margin_from_obstacle - abs_ego_offset;
-
-  bool will_collide_with_obstacle = false;
-  double feasible_margin_from_obstacle = margin_from_obstacle;
-  if (closest_obstacle_stop_dist < feasible_stop_dist) {
-    feasible_margin_from_obstacle =
-      margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist);
-    will_collide_with_obstacle = true;
-  }
+  const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() {
+    // Check feasibility to stop
+    if (suppress_sudden_obstacle_stop_) {
+      const double closest_obstacle_stop_dist =
+        closest_obstacle_dist - margin_from_obstacle - abs_ego_offset;
+
+      // Calculate feasible stop margin (Check the feasibility)
+      const double feasible_stop_dist = calcMinimumDistanceToStop(
+                                          planner_data.ego_vel, longitudinal_info_.limit_max_accel,
+                                          longitudinal_info_.limit_min_accel) +
+                                        dist_to_ego;
+
+      if (closest_obstacle_stop_dist < feasible_stop_dist) {
+        const auto feasible_margin_from_obstacle =
+          margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist);
+        return std::make_pair(feasible_margin_from_obstacle, true);
+      }
+    }
+    return std::make_pair(margin_from_obstacle, false);
+  }();
 
   // Generate Output Trajectory
   const double zero_vel_dist = [&]() {
     const double current_zero_vel_dist =
-      std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle);
+      std::max(0.0, closest_obstacle_dist - abs_ego_offset - stop_margin_from_obstacle);
 
     // Hold previous stop distance if necessary
     if (
@@ -378,7 +383,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity);
 
   stop_planning_debug_info_.set(
-    StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, feasible_margin_from_obstacle);
+    StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, stop_margin_from_obstacle);
   stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0);
   stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0);