diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
index 766cce6bc4..2fa01e19ba 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
@@ -13,7 +13,6 @@
       enable_bound_clipping: false
       enable_update_path_when_object_is_gone: false
       enable_force_avoidance_for_stopped_vehicle: true
-      enable_safety_check: true
       enable_yield_maneuver: true
       enable_yield_maneuver_during_shifting: false
       disable_path_update: false
@@ -133,10 +132,18 @@
 
       # For safety check
       safety_check:
+        # safety check configuration
+        enable: true                                     # [-]
+        check_current_lane: false                        # [-]
+        check_shift_side_lane: true                      # [-]
+        check_other_side_lane: false                     # [-]
+        check_unavoidable_object: false                  # [-]
+        check_other_object: true                         # [-]
+        # collision check parameters
+        check_all_predicted_path: false                  # [-]
+        time_resolution: 0.5                             # [s]
+        time_horizon: 5.0                               # [s]
         safety_check_backward_distance: 100.0            # [m]
-        safety_check_time_horizon: 5.0                   # [s]
-        safety_check_idling_time: 1.5                    # [s]
-        safety_check_accel_for_rss: 2.5                  # [m/ss]
         safety_check_hysteresis_factor: 2.0              # [-]
         safety_check_ego_offset: 1.0                     # [m]
 
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml
index 845f9c38e2..336879185f 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml
@@ -1,6 +1,9 @@
 /**:
   ros__parameters:
     dynamic_avoidance:
+      common:
+        enable_debug_info: true
+
       # avoidance is performed for the object type with true
       target_object:
         car: true
@@ -19,6 +22,10 @@
         min_obj_lat_offset_to_ego_path: 0.0 # [m]
         max_obj_lat_offset_to_ego_path: 1.0 # [m]
 
+        cut_in_object:
+          min_time_to_start_cut_in: 1.0 # [s]
+          min_lon_offset_ego_to_object: 0.0 # [m]
+
         crossing_object:
           min_object_vel: 1.0
           max_object_angle: 1.05
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml
index fb70a8a1a0..a51052f0c4 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml
@@ -46,7 +46,7 @@
         min_vehicle_brake_for_rss: -2.5 # [m/s^2]
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
-        pub_debug_grid: false
+        possible_object_bbox: [1.0, 2.0] # [m x m]
 
       enable_rtc:
         intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml
index 2283477e40..af876b4f1b 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml
@@ -2,10 +2,9 @@
   ros__parameters:
     stop_line:
       stop_margin: 1.4
-      stop_check_dist: 2.0
       stop_duration_sec: 1.0
       use_initialization_stop_line_state: true
       hold_stop_margin_distance: 2.0
 
       debug:
-        show_stopline_collision_check: false
+        show_stop_line_collision_check: false
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml
index 730bc3053e..8e77420dd4 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml
@@ -35,3 +35,13 @@
     # nearest search
     ego_nearest_dist_threshold: 3.0 # [m]
     ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg]
+    # replanning & trimming trajectory param outside algorithm
+    replan:
+      enable: true  # if true, only perform smoothing when the input changes significantly
+      max_path_shape_around_ego_lat_dist: 2.0  # threshold of path shape change around ego [m]
+      max_path_shape_forward_lon_dist: 100.0   # forward point to check lateral distance difference [m]
+      max_path_shape_forward_lat_dist: 0.1     # threshold of path shape change around forward point [m]
+      max_ego_moving_dist: 5.0                 # threshold of ego's moving distance for replan [m]
+      # make max_goal_moving_dist long to keep start point fixed for pull over
+      max_goal_moving_dist: 15.0               # threshold of goal's moving distance for replan [m]
+      max_delta_time_sec: 1.0                  # threshold of delta time for replan [second]
diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml
index 7acfa30e6d..1fd99502e3 100644
--- a/autoware_launch/launch/components/tier4_localization_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml
@@ -3,6 +3,7 @@
   <let name="loc_config_path" value="$(find-pkg-share autoware_launch)/config/localization"/>
   <arg name="pose_source" default="ndt" description="select pose_estimator: ndt, yabloc, eagleye"/>
   <arg name="twist_source" default="gyro_odom" description="select twist_estimator. gyro_odom, eagleye"/>
+  <arg name="input_pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud" description="The topic will be used in the localization util module"/>
 
   <group>
     <include file="$(find-pkg-share tier4_localization_launch)/launch/localization.launch.xml">
@@ -11,6 +12,7 @@
       <arg name="pose_source" value="$(var pose_source)"/>
       <arg name="twist_source" value="$(var twist_source)"/>
       <arg name="system_run_mode" value="$(var system_run_mode)"/>
+      <arg name="input_pointcloud" value="$(var input_pointcloud)"/>
 
       <!-- parameter paths for common -->
       <arg name="crop_box_filter_measurement_range_param_path" value="$(var loc_config_path)/crop_box_filter_measurement_range.param.yaml"/>
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index 97f411924c..297c9acf2f 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -1694,32 +1694,6 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection
                           Value: false
-                        - Alpha: 0.5
-                          Autocompute Intensity Bounds: true
-                          Class: grid_map_rviz_plugin/GridMap
-                          Color: 200; 200; 200
-                          Color Layer: color
-                          Color Transformer: IntensityLayer
-                          Enabled: false
-                          Height Layer: elevation
-                          Height Transformer: Layer
-                          History Length: 1
-                          Invert Rainbow: false
-                          Max Color: 255; 255; 255
-                          Max Intensity: 10
-                          Min Color: 0; 0; 0
-                          Min Intensity: 0
-                          Name: IntersectionOcclusion
-                          Show Grid Lines: false
-                          Topic:
-                            Depth: 5
-                            Durability Policy: Volatile
-                            Filter size: 10
-                            History Policy: Keep Last
-                            Reliability Policy: Reliable
-                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/occlusion_grid
-                          Use Rainbow: true
-                          Value: false
                         - Class: rviz_default_plugins/MarkerArray
                           Enabled: false
                           Name: Blind Spot