diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml
index 5a3139e4c5..0ffeeeccf0 100644
--- a/autoware_launch/config/planning/preset/default_preset.yaml
+++ b/autoware_launch/config/planning/preset/default_preset.yaml
@@ -8,7 +8,7 @@ launch:
       default: "true"
   - arg:
       name: launch_dynamic_obstacle_avoidance
-      default: "false"
+      default: "true"
   - arg:
       name: launch_sampling_planner_module
       default: "false" # Warning, experimental module, use only in simulations
@@ -120,7 +120,7 @@ launch:
 
   - arg:
       name: launch_surround_obstacle_checker
-      default: "false"
+      default: "true"
 
   # parking modules
   - arg:
diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml
index 4edbbf2d35..5680a99713 100644
--- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    max_vel: 4.17           # max velocity limit [m/s]
+    max_vel: 11.1           # max velocity limit [m/s]
 
     # constraints param for normal driving
     normal:
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 c9ac6aa1a6..069f336959 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
@@ -89,7 +89,7 @@
           object_time_margin_to_collision_point: 4.0
 
       occlusion:
-        enable: false
+        enable: true
         occlusion_attention_area_length: 70.0
         free_space_max: 43
         occupied_min: 58
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml
index db89a81e47..17a044fb67 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml
@@ -3,7 +3,7 @@
     option:
       enable_skip_optimization: false                              # skip elastic band and model predictive trajectory
       enable_reset_prev_optimization: false                        # If true, optimization has no fix constraint to the previous result.
-      enable_outside_drivable_area_stop: true                      # stop if the ego's trajectory footprint is outside the drivable area
+      enable_outside_drivable_area_stop: false                     # stop if the ego's trajectory footprint is outside the drivable area
       use_footprint_polygon_for_outside_drivable_area_check: false # If false, only the footprint's corner points are considered.
 
       debug:
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
index 4b9e96f6bc..2213cc39dc 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
@@ -12,7 +12,7 @@
       idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
       min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
-      safe_distance_margin : 4.0 # This is also used as a stop margin [m]
+      safe_distance_margin : 5.0 # This is also used as a stop margin [m]
       terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]
       hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s]
       hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m]
@@ -99,8 +99,8 @@
       stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle
 
       # hysteresis for cruise and stop
-      obstacle_velocity_threshold_from_cruise_to_stop : 1.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
-      obstacle_velocity_threshold_from_stop_to_cruise : 1.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+      obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+      obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
 
       # if crossing vehicle is determined as target obstacles or not
       crossing_obstacle:
@@ -126,7 +126,7 @@
           ego_obstacle_overlap_time_threshold : 0.2 #  time threshold to decide cut-in obstacle for cruise or stop [s]
           max_prediction_time_for_collision_check : 10.0 # prediction time to check collision between obstacle and ego
           max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s]
-          num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego
+          num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego
         yield:
           enable_yield: true
           lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding
diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-awsim.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-awsim.yaml
deleted file mode 100644
index 937cfdeb65..0000000000
--- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-awsim.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-files:
-  - { path: $(find-pkg-share autoware_launch)/config/system/system_diagnostic_monitor/autoware-main.yaml }
-
-edits:
-  - { type: remove, path: /autoware/system/duplicated_node_checker }
diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml
deleted file mode 100644
index c96dd5ed46..0000000000
--- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-files:
-  - { path: $(find-pkg-share autoware_launch)/config/system/system_diagnostic_monitor/autoware-main.yaml }
diff --git a/autoware_launch/config/system/diagnostics/autoware-awsim.yaml b/autoware_launch/config/system/diagnostics/autoware-awsim.yaml
new file mode 100644
index 0000000000..4abae34a31
--- /dev/null
+++ b/autoware_launch/config/system/diagnostics/autoware-awsim.yaml
@@ -0,0 +1,5 @@
+files:
+  - { path: $(dirname)/autoware-main.yaml }
+
+edits:
+  - { type: remove, path: /autoware/system/duplicated_node_checker }
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml b/autoware_launch/config/system/diagnostics/autoware-main.yaml
similarity index 100%
rename from autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml
rename to autoware_launch/config/system/diagnostics/autoware-main.yaml
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml b/autoware_launch/config/system/diagnostics/control.yaml
similarity index 92%
rename from autoware_launch/config/system/system_diagnostic_monitor/control.yaml
rename to autoware_launch/config/system/diagnostics/control.yaml
index 5551ed4575..6a04ed6714 100644
--- a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml
+++ b/autoware_launch/config/system/diagnostics/control.yaml
@@ -9,6 +9,7 @@ units:
       - { type: link, link: /autoware/control/performance_monitoring/lane_departure }
       - { type: link, link: /autoware/control/performance_monitoring/trajectory_deviation }
       - { type: link, link: /autoware/control/performance_monitoring/control_state }
+      - { type: link, link: /autoware/control/collision_detector }
 
   - path: /autoware/control/local
     type: and
@@ -66,3 +67,8 @@ units:
     type: diag
     node: external_cmd_converter
     name: remote_control_topic_status
+
+  - path: /autoware/control/collision_detector
+    type: diag
+    node: collision_detector
+    name: collision_detect
diff --git a/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml b/autoware_launch/config/system/diagnostics/dummy_diag_publisher.param.yaml
similarity index 100%
rename from autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml
rename to autoware_launch/config/system/diagnostics/dummy_diag_publisher.param.yaml
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml b/autoware_launch/config/system/diagnostics/hardware.yaml
similarity index 100%
rename from autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml
rename to autoware_launch/config/system/diagnostics/hardware.yaml
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml b/autoware_launch/config/system/diagnostics/localization.yaml
similarity index 100%
rename from autoware_launch/config/system/system_diagnostic_monitor/localization.yaml
rename to autoware_launch/config/system/diagnostics/localization.yaml
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/map.yaml b/autoware_launch/config/system/diagnostics/map.yaml
similarity index 100%
rename from autoware_launch/config/system/system_diagnostic_monitor/map.yaml
rename to autoware_launch/config/system/diagnostics/map.yaml
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml b/autoware_launch/config/system/diagnostics/perception.yaml
similarity index 100%
rename from autoware_launch/config/system/system_diagnostic_monitor/perception.yaml
rename to autoware_launch/config/system/diagnostics/perception.yaml
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml b/autoware_launch/config/system/diagnostics/planning.yaml
similarity index 100%
rename from autoware_launch/config/system/system_diagnostic_monitor/planning.yaml
rename to autoware_launch/config/system/diagnostics/planning.yaml
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml b/autoware_launch/config/system/diagnostics/system.yaml
similarity index 91%
rename from autoware_launch/config/system/system_diagnostic_monitor/system.yaml
rename to autoware_launch/config/system/diagnostics/system.yaml
index cb96c2cd7f..097ff367e5 100644
--- a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml
+++ b/autoware_launch/config/system/diagnostics/system.yaml
@@ -2,7 +2,7 @@ units:
   - path: /autoware/system
     type: and
     list:
-      - { type: link, link: /autoware/system/duplicated_node_checker }
+      # - { type: link, link: /autoware/system/duplicated_node_checker }
       - { type: link, link: /autoware/system/topic_rate_check/emergency_control_command }
       - { type: link, link: /autoware/system/emergency_stop_operation }
 
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml b/autoware_launch/config/system/diagnostics/vehicle.yaml
similarity index 100%
rename from autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml
rename to autoware_launch/config/system/diagnostics/vehicle.yaml
diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
index 29378cc0c8..4bf18af889 100644
--- a/autoware_launch/launch/autoware.launch.xml
+++ b/autoware_launch/launch/autoware.launch.xml
@@ -37,7 +37,7 @@
   <arg name="system_run_mode" default="online" description="run mode in system"/>
   <arg name="launch_system_monitor" default="true" description="launch system monitor"/>
   <arg name="launch_dummy_diag_publisher" default="false" description="launch dummy diag publisher"/>
-  <arg name="diagnostic_graph_aggregator_graph_path" default="$(find-pkg-share autoware_launch)/config/system/diagnostic_graph_aggregator/autoware-main.yaml" description="diagnostic graph config"/>
+  <arg name="diagnostic_graph_aggregator_graph_path" default="$(find-pkg-share autoware_launch)/config/system/diagnostics/autoware-main.yaml" description="diagnostic graph config"/>
   <!-- Tools -->
   <arg name="rviz" default="true" description="launch rviz"/>
   <arg name="rviz_config_name" default="autoware.rviz" description="rviz config name"/>
diff --git a/autoware_launch/launch/components/tier4_autoware_api_component.launch.xml b/autoware_launch/launch/components/tier4_autoware_api_component.launch.xml
index 1c7b520108..bc582fbb9a 100644
--- a/autoware_launch/launch/components/tier4_autoware_api_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_autoware_api_component.launch.xml
@@ -1,7 +1,8 @@
 <?xml version="1.0"?>
 <launch>
   <!-- Fork the repository and add the parameters here if needed. -->
-  <arg name="scenario_simulation" default="false"/>
-  <arg name="launch_deprecated_api" default="$(var scenario_simulation)"/>
+  <arg name="launch_deprecated_api" default="true"/>
+  <arg name="rosbridge_enabled" default="true"/>
+  <arg name="rosbridge_max_message_size" default="1000000000"/>
   <include file="$(find-pkg-share tier4_autoware_api_launch)/launch/autoware_api.launch.xml"/>
 </launch>
diff --git a/autoware_launch/launch/components/tier4_system_component.launch.xml b/autoware_launch/launch/components/tier4_system_component.launch.xml
index 5bb8403dc2..e729fef464 100644
--- a/autoware_launch/launch/components/tier4_system_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_system_component.launch.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="diagnostic_graph_aggregator_param_path" default="$(find-pkg-share diagnostic_graph_aggregator)/config/default.param.yaml"/>
-  <arg name="diagnostic_graph_aggregator_graph_path" default="$(find-pkg-share autoware_launch)/config/system/diagnostic_graph_aggregator/autoware-main.yaml"/>
+  <arg name="diagnostic_graph_aggregator_graph_path" default="$(find-pkg-share autoware_launch)/config/system/diagnostics/autoware-main.yaml"/>
 
   <include file="$(find-pkg-share tier4_system_launch)/launch/system.launch.xml">
     <arg name="run_mode" value="$(var system_run_mode)"/>
@@ -14,7 +14,7 @@
     <arg name="processing_time_checker_param_path" value="$(find-pkg-share autoware_launch)/config/system/processing_time_checker/processing_time_checker.param.yaml"/>
     <arg name="mrm_comfortable_stop_operator_param_path" value="$(find-pkg-share autoware_launch)/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml"/>
     <arg name="mrm_emergency_stop_operator_param_path" value="$(find-pkg-share autoware_launch)/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml"/>
-    <arg name="dummy_diag_publisher_param_path" value="$(find-pkg-share autoware_launch)/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml"/>
+    <arg name="dummy_diag_publisher_param_path" value="$(find-pkg-share autoware_launch)/config/system/diagnostics/dummy_diag_publisher.param.yaml"/>
     <arg name="system_monitor_cpu_monitor_param_path" value="$(find-pkg-share autoware_launch)/config/system/system_monitor/cpu_monitor.param.yaml"/>
     <arg name="system_monitor_gpu_monitor_param_path" value="$(find-pkg-share autoware_launch)/config/system/system_monitor/gpu_monitor.param.yaml"/>
     <arg name="system_monitor_hdd_monitor_param_path" value="$(find-pkg-share autoware_launch)/config/system/system_monitor/hdd_monitor.param.yaml"/>
@@ -28,4 +28,11 @@
     <arg name="diagnostic_graph_aggregator_param_path" value="$(var diagnostic_graph_aggregator_param_path)"/>
     <arg name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path)"/>
   </include>
+
+  <!-- For logging of diagnostics_graph error -->
+  <include file="$(find-pkg-share diagnostic_graph_utils)/launch/logging.launch.xml">
+    <arg name="root_path" value="/autoware/modes/autonomous"/>
+    <arg name="max_depth" value="3"/>
+    <arg name="show_rate" value="0.2"/>
+  </include>
 </launch>
diff --git a/autoware_launch/launch/e2e_simulator.launch.xml b/autoware_launch/launch/e2e_simulator.launch.xml
index 3237754c48..eaa9dcead4 100644
--- a/autoware_launch/launch/e2e_simulator.launch.xml
+++ b/autoware_launch/launch/e2e_simulator.launch.xml
@@ -71,7 +71,7 @@
       <!-- System -->
       <arg name="launch_system_monitor" value="$(var launch_system_monitor)"/>
       <arg name="launch_dummy_diag_publisher" value="$(var launch_dummy_diag_publisher)"/>
-      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/diagnostic_graph_aggregator/autoware-awsim.yaml"/>
+      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/diagnostics/autoware-awsim.yaml"/>
       <!-- Sensing -->
       <arg name="launch_sensing_driver" value="$(var launch_sensing_driver)"/>
       <!-- Perception-->
@@ -83,3 +83,4 @@
     </include>
   </group>
 </launch>
+
diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
index f1a9ad5fab..123bd01ed3 100644
--- a/autoware_launch/launch/logging_simulator.launch.xml
+++ b/autoware_launch/launch/logging_simulator.launch.xml
@@ -65,7 +65,7 @@
       <arg name="system_run_mode" value="logging_simulation"/>
       <arg name="launch_system_monitor" value="$(var launch_system_monitor)"/>
       <arg name="launch_dummy_diag_publisher" value="$(var launch_dummy_diag_publisher)"/>
-      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/diagnostic_graph_aggregator/autoware-main.yaml"/>
+      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/diagnostics/autoware-main.yaml"/>
       <!-- Map -->
       <arg name="lanelet2_map_file" value="$(var lanelet2_map_file)"/>
       <arg name="pointcloud_map_file" value="$(var pointcloud_map_file)"/>
diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml
index be175553b7..3c56e11412 100644
--- a/autoware_launch/launch/planning_simulator.launch.xml
+++ b/autoware_launch/launch/planning_simulator.launch.xml
@@ -70,7 +70,7 @@
       <arg name="system_run_mode" value="planning_simulation"/>
       <arg name="launch_system_monitor" value="$(var launch_system_monitor)"/>
       <arg name="launch_dummy_diag_publisher" value="$(var launch_dummy_diag_publisher)"/>
-      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/diagnostic_graph_aggregator/autoware-main.yaml"/>
+      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/diagnostics/autoware-main.yaml"/>
       <!-- Map -->
       <arg name="lanelet2_map_file" value="$(var lanelet2_map_file)"/>
       <arg name="pointcloud_map_file" value="$(var pointcloud_map_file)"/>
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index c53d5d62e6..589c9cbfd5 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -269,13 +269,13 @@ Visualization Manager:
           Enabled: true
           Name: Vehicle
         - Class: rviz_plugins/MrmSummaryOverlayDisplay
-          Enabled: false
+          Enabled: true
           Font Size: 10
-          Left: 512
+          Left: 10
           Max Letter Num: 100
           Name: MRM Summary
           Text Color: 25; 255; 240
-          Top: 64
+          Top: 10
           Topic:
             Depth: 5
             Durability Policy: Volatile