diff --git a/.github/CODEOWNERS b/.github/_CODEOWNERS
similarity index 100%
rename from .github/CODEOWNERS
rename to .github/_CODEOWNERS
diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml
index 5a3139e4c5..5f95afb271 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
@@ -93,6 +93,15 @@ launch:
       #         none
 
   # motion velocity planner modules
+  - arg:
+      name: launch_obstacle_stop_module
+      default: "true"
+  - arg:
+      name: launch_obstacle_slow_down_module
+      default: "true"
+  - arg:
+      name: launch_obstacle_cruise_module
+      default: "true"
   - arg:
       name: launch_dynamic_obstacle_stop_module
       default: "true"
@@ -120,7 +129,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_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml
index 9bba8c52dc..aa1fdb3048 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml
@@ -56,10 +56,10 @@
         object_path_base:
           min_longitudinal_polygon_margin: 3.0 # [m]
 
-        lat_offset_from_obstacle: 0.3 # [m]
-        margin_distance_around_pedestrian: 0.8 # [m]
+        lat_offset_from_obstacle: 1.0 # [m]
+        margin_distance_around_pedestrian: 2.0 # [m]
         predicted_path:
-          end_time_to_consider: 1.0 # [s]
+          end_time_to_consider: 2.0 # [s]
           threshold_confidence: 0.0 # [] not probability
         max_lat_offset_to_avoid: 0.5 # [m]
         max_time_for_object_lat_shift: 0.0 # [s]
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml
index 3480bb66e5..31e35dcad5 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml
@@ -23,9 +23,9 @@
           th_moving_time: 2.0                           # [s]
           longitudinal_margin: 0.0                      # [m]
           lateral_margin:
-            soft_margin: 0.5                            # [m]
+            soft_margin: 0.7                            # [m]
             hard_margin: 0.2                            # [m]
-            hard_margin_for_parked_vehicle: 0.2         # [m]
+            hard_margin_for_parked_vehicle: 0.7         # [m]
           max_expand_ratio: 0.0                         # [-] FOR DEVELOPER
           envelope_buffer_margin: 0.1                   # [m] FOR DEVELOPER
           th_error_eclipse_long_radius : 0.6            # [m]
@@ -34,9 +34,9 @@
           th_moving_time: 2.0
           longitudinal_margin: 0.0
           lateral_margin:
-            soft_margin: 0.5
+            soft_margin: 0.7
             hard_margin: 0.2
-            hard_margin_for_parked_vehicle: 0.2
+            hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.1
           th_error_eclipse_long_radius : 0.6
@@ -45,9 +45,9 @@
           th_moving_time: 2.0
           longitudinal_margin: 0.0
           lateral_margin:
-            soft_margin: 0.5
+            soft_margin: 0.7
             hard_margin: 0.2
-            hard_margin_for_parked_vehicle: 0.2
+            hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.1
           th_error_eclipse_long_radius : 0.6
@@ -56,9 +56,9 @@
           th_moving_time: 2.0
           longitudinal_margin: 0.0
           lateral_margin:
-            soft_margin: 0.5
+            soft_margin: 0.7
             hard_margin: 0.2
-            hard_margin_for_parked_vehicle: 0.2
+            hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.1
           th_error_eclipse_long_radius : 0.6
@@ -67,7 +67,7 @@
           th_moving_time: 1.0
           longitudinal_margin: 0.0
           lateral_margin:
-            soft_margin: 0.5
+            soft_margin: 0.7
             hard_margin: -0.2
             hard_margin_for_parked_vehicle: -0.2
           max_expand_ratio: 0.0
@@ -76,33 +76,33 @@
         bicycle:
           th_moving_speed: 0.28
           th_moving_time: 1.0
-          longitudinal_margin: 0.6
+          longitudinal_margin: 1.0
           lateral_margin:
             soft_margin: 0.7
             hard_margin: 0.5
-            hard_margin_for_parked_vehicle: 0.2
+            hard_margin_for_parked_vehicle: 0.5
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
           th_error_eclipse_long_radius : 0.6
         motorcycle:
           th_moving_speed: 1.0
           th_moving_time: 1.0
-          longitudinal_margin: 0.6
+          longitudinal_margin: 1.0
           lateral_margin:
-            soft_margin: 0.5
+            soft_margin: 0.7
             hard_margin: 0.3
-            hard_margin_for_parked_vehicle: 0.2
+            hard_margin_for_parked_vehicle: 0.3
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
           th_error_eclipse_long_radius : 0.6
         pedestrian:
           th_moving_speed: 0.28
           th_moving_time: 1.0
-          longitudinal_margin: 0.6
+          longitudinal_margin: 1.0
           lateral_margin:
             soft_margin: 0.7
             hard_margin: 0.5
-            hard_margin_for_parked_vehicle: 0.2
+            hard_margin_for_parked_vehicle: 0.5
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
           th_error_eclipse_long_radius : 0.6
@@ -232,7 +232,7 @@
         # avoidance distance parameters
         longitudinal:
           min_prepare_time: 1.0                         # [s]
-          max_prepare_time: 2.0                         # [s]
+          max_prepare_time: 3.0                         # [s]
           min_prepare_distance: 1.0                     # [m]
           min_slow_down_speed: 1.38                     # [m/s]
           buf_slow_down_speed: 0.56                     # [m/s] FOR DEVELOPER
@@ -294,7 +294,7 @@
           velocity: [1.39, 4.17, 11.1]                  # [m/s]
           max_accel_values: [0.5, 0.5, 0.5]             # [m/ss]
           min_jerk_values: [0.2, 0.2, 0.2]              # [m/sss]
-          max_jerk_values: [3.0, 3.0, 3.0]              # [m/sss]
+          max_jerk_values: [1.0, 1.0, 1.0]              # [m/sss]
 
         # longitudinal constraints
         longitudinal:
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/tier4_diagnostics/autoware-awsim.yaml b/autoware_launch/config/system/tier4_diagnostics/autoware-awsim.yaml
new file mode 100644
index 0000000000..5456b20c7a
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/autoware-awsim.yaml
@@ -0,0 +1,2 @@
+files:
+  - { path: $(dirname)/autoware-main.yaml }
diff --git a/autoware_launch/config/system/tier4_diagnostics/autoware-main.yaml b/autoware_launch/config/system/tier4_diagnostics/autoware-main.yaml
new file mode 100644
index 0000000000..fed7797849
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/autoware-main.yaml
@@ -0,0 +1,67 @@
+files:
+  - { path: $(dirname)/control.yaml }
+  # - { path: $(dirname)/hardware.yaml }
+  - { path: $(dirname)/localization.yaml }
+  - { path: $(dirname)/map.yaml }
+  - { path: $(dirname)/perception.yaml }
+  - { path: $(dirname)/planning.yaml }
+  # - { path: $(dirname)/others.yaml }
+  # - { path: $(dirname)/sensing.yaml }
+  - { path: $(dirname)/system.yaml }
+  - { path: $(dirname)/vehicle.yaml }
+
+units:
+  - path: /autoware/modes/local
+    type: stale
+
+  - path: /autoware/modes/remote
+    type: stale
+
+  - path: /autoware/modes/stop
+    type: ok
+
+  - path: /autoware/modes/autonomous
+    type: and
+    list:
+      - { type: link, link: /map/autonomous_available }
+      - { type: link, link: /localization/autonomous_available }
+      - { type: link, link: /planning/autonomous_available }
+      - { type: link, link: /perception/autonomous_available }
+      - { type: link, link: /control/autonomous_available }
+      - { type: link, link: /vehicle/autonomous_available }
+      - { type: link, link: /system/autonomous_available }
+
+  - path: /autoware/modes/pull_over
+    type: and
+    list:
+      - { type: link, link: /map/pull_over_available }
+      - { type: link, link: /localization/pull_over_available }
+      - { type: link, link: /planning/pull_over_available }
+      - { type: link, link: /perception/pull_over_available }
+      - { type: link, link: /control/pull_over_available }
+      - { type: link, link: /vehicle/pull_over_available }
+      - { type: link, link: /system/pull_over_available }
+
+  - path: /autoware/modes/comfortable_stop
+    type: and
+    list:
+      - { type: link, link: /map/comfortable_stop_available }
+      - { type: link, link: /localization/comfortable_stop_available }
+      - { type: link, link: /planning/comfortable_stop_available }
+      - { type: link, link: /perception/comfortable_stop_available }
+      - { type: link, link: /control/comfortable_stop_available }
+      - { type: link, link: /vehicle/comfortable_stop_available }
+      - { type: link, link: /system/comfortable_stop_available }
+
+  - path: /autoware/modes/emergency_stop
+    type: ok
+
+  - path: /autoware/debug/tools
+    type: and
+    list:
+      - { type: link, link: /autoware/system/service_log_checker }
+
+  - path: /autoware/system/service_log_checker
+    type: diag
+    node: service_log_checker
+    name: response_status
diff --git a/autoware_launch/config/system/tier4_diagnostics/control.yaml b/autoware_launch/config/system/tier4_diagnostics/control.yaml
new file mode 100644
index 0000000000..3904d210d5
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/control.yaml
@@ -0,0 +1,141 @@
+units:
+  - path: /control/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /control/emergency_stop }
+      - { type: link, link: /control/pull_over }
+      - { type: link, link: /control/comfortable_stop }
+
+  - path: /control/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /control/emergency_stop }
+      - { type: link, link: /control/comfortable_stop }
+
+  - path: /control/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /control/emergency_stop }
+
+  # *******************************************************************************
+  # NOTE: Please modify this section according to your environment and requirements.
+  # *******************************************************************************
+  - path: /control/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /control/001-topic_status/control_command-error }
+      - { type: link, link: /control/004-lane_departure-error }
+      - { type: link, link: /control/005-trajectory_deviation-error }
+      - { type: link, link: /control/009-aeb_emergency_stop }
+      # - { type: link, link: /control/010-max_distance_deviation-error }
+      # - { type: link, link: /control/011-slip_detection }
+      - { type: link, link: /control/collision_detector }
+      - { type: link, link: /control/rolling_back }
+      - { type: link, link: /control/over_velocity }
+
+  - path: /control/comfortable_stop
+    type: and
+
+  - path: /control/pull_over
+    type: and
+
+  - path: /control/none
+    type: and
+    list:
+      - { type: link, link: /control/003-gate_heartbeat }
+      - { type: link, link: /control/007-external_command_converter_heartbeat }
+      - { type: link, link: /control/008-external_command_selector_heartbeat }
+
+  # Intermediate paths
+  - path: /control/001-topic_status/control_command-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /control/001-topic_status/control_command
+
+  - path: /control/004-lane_departure-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /control/004-lane_departure
+
+  - path: /control/005-trajectory_deviation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /control/005-trajectory_deviation
+
+  - path: /control/010-max_distance_deviation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /control/010-max_distance_deviation
+
+  - path: /control/001-topic_status/control_command
+    type: diag
+    node: topic_state_monitor_control_command_control_cmd
+    name: control_topic_status
+    timeout: 1.0
+
+  - path: /control/003-gate_heartbeat
+    type: diag
+    node: vehicle_cmd_gate
+    name: heartbeat
+    timeout: 1.0
+
+  - path: /control/004-lane_departure
+    type: diag
+    node: lane_departure_checker_node
+    name: lane_departure
+    timeout: 1.0
+
+  - path: /control/005-trajectory_deviation
+    type: diag
+    node: lane_departure_checker_node
+    name: trajectory_deviation
+    timeout: 1.0
+
+  - path: /control/007-external_command_converter_heartbeat
+    type: diag
+    node: external_cmd_converter
+    name: remote_control_topic_status
+    timeout: 1.0
+
+  - path: /control/008-external_command_selector_heartbeat
+    type: diag
+    node: external_cmd_selector
+    name: heartbeat
+    timeout: 1.0
+
+  - path: /control/009-aeb_emergency_stop
+    type: diag
+    node: autonomous_emergency_braking
+    name: aeb_emergency_stop
+    timeout: 1.0
+
+  - path: /control/010-max_distance_deviation
+    type: diag
+    node: control_validator
+    name: control_validation_max_distance_deviation
+    timeout: 1.0
+
+  - path: /control/011-slip_detection
+    type: diag
+    node: slip_detector
+    name: slip_status
+    timeout: 3.0
+
+  - path: /control/collision_detector
+    type: diag
+    node: collision_detector
+    name: collision_detect
+
+  - path: /control/rolling_back
+    type: diag
+    node: control_validator
+    name: control_validation_rolling_back
+
+  - path: /control/over_velocity
+    type: diag
+    node: control_validator
+    name: control_validation_over_velocity
diff --git a/autoware_launch/config/system/tier4_diagnostics/dummy_diag_publisher.param.yaml b/autoware_launch/config/system/tier4_diagnostics/dummy_diag_publisher.param.yaml
new file mode 100644
index 0000000000..8a171c01f6
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/dummy_diag_publisher.param.yaml
@@ -0,0 +1,48 @@
+# Description:
+#   required_diags:
+#     <name>: {is_active: <is_active>, status: <status>}
+#   name: diag name
+#   is_active: Force update or not
+#   status: diag status set by dummy diag publisher "OK, Warn, Error, Stale"
+#
+# Note:
+#
+# default values are:
+#   is_active: "true"
+#   status: "OK"
+---
+/**:
+  ros__parameters:
+    required_diags:
+      # localization
+      ## /localization/001-topic_status/initialpose
+      "topic_state_monitor_initialpose3d: localization_topic_status": default
+
+      ## /localization/001-topic_status/pose_twist_fusion_filter
+      "topic_state_monitor_pose_twist_fusion_filter_pose: localization_topic_status": default
+
+      ## /localization/002-tf
+      "topic_state_monitor_transform_map_to_base_link: localization_topic_status": default
+
+      ## /localization/003-matching_score
+      "ndt_scan_matcher: scan_matching_status": default
+
+      ## /localization/004-accuracy
+      "localization_error_monitor: ellipse_error_status": default
+
+      # map
+      ## /map/001-topic_status/pointcloud_map
+      "topic_state_monitor_pointcloud_map: map_topic_status": default
+
+      # perception
+      ## /perception/001-topic_status/traffic_signals
+      "topic_state_monitor_traffic_light_recognition_traffic_signals: perception_topic_status": default
+
+      ## /perception/001-topic_status/objects
+      "topic_state_monitor_object_recognition_objects: perception_topic_status": default
+
+      ## /perception/001-topic_status/pointcloud
+      "topic_state_monitor_obstacle_segmentation_pointcloud: perception_topic_status": default
+
+      ## /perception/002-detection_delay
+      "multi_object_tracker: Perception delay check from original header stamp": default
diff --git a/autoware_launch/config/system/tier4_diagnostics/hardware.yaml b/autoware_launch/config/system/tier4_diagnostics/hardware.yaml
new file mode 100644
index 0000000000..d73f723670
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/hardware.yaml
@@ -0,0 +1,121 @@
+# TODO(Takagi, Isamu): This file is under construction.
+units:
+  - path: /autoware/system/resources/clock/offset
+    diag: ": NTP Offset"
+    timeout: 10.0
+
+  - path: /autoware/system/resources/cpu/offset
+    diag: ": CPU Temperature"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/cpu/usage
+    diag: ": CPU Usage"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/cpu/thermal_throttling
+    diag: ": CPU Thermal Throttling"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/cpu/frequency
+    diag: ": CPU Frequency"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/cpu/load_average
+    diag: ": CPU Load Average"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/gpu/temperature
+    diag: ": GPU Temperature"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/gpu/usage
+    diag: ": GPU Usage"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/gpu/memory_usage
+    diag: ": GPU Memory Usage"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/gpu/thermal_throttling
+    diag: ": GPU Thermal Throttling"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/gpu/frequency
+    diag: ": GPU Frequency"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/memory/usage
+    diag: ": Memory Usage"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/network/usage
+    diag: ": Network Usage"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/network/traffic
+    diag: ": Network Traffic"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/network/crc
+    diag: ": Network CRC Error"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/network/packet_reassembles
+    diag: ": IP Packet Reassembles Failed"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/temperature
+    diag: ": HDD Temperature"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/recovered_error
+    diag: ": HDD RecoveredError"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/read_data_rate
+    diag: ": HDD ReadDataRate"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/write_data_rate
+    diag: ": HDD WriteDataRate"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/read_iops
+    diag: ": HDD ReadIOPS"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/write_iops
+    diag: ": HDD WriteIOPS"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/usage
+    diag: ": HDD Usage"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/power_on_hours
+    diag: ": HDD PowerOnHours"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/total_data_written
+    diag: ": HDD TotalDataWritten"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/storage/connection
+    diag: ": HDD Connection"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/process/high_load
+    diag: ": High-load"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/process/high_mem
+    diag: ": High-mem"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/process/tasks_summary
+    diag: ": Tasks Summary"
+    timeout: 3.0
+
+  - path: /autoware/system/resources/voltage/battery
+    diag: ": CMOS Battery Status"
+    timeout: 3.0
diff --git a/autoware_launch/config/system/tier4_diagnostics/localization.yaml b/autoware_launch/config/system/tier4_diagnostics/localization.yaml
new file mode 100644
index 0000000000..22bfa26ae0
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/localization.yaml
@@ -0,0 +1,99 @@
+units:
+  - path: /localization/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /localization/emergency_stop }
+      - { type: link, link: /localization/pull_over }
+      - { type: link, link: /localization/comfortable_stop }
+
+  - path: /localization/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /localization/emergency_stop }
+      - { type: link, link: /localization/comfortable_stop }
+
+  - path: /localization/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /localization/emergency_stop }
+
+  # *******************************************************************************
+  # NOTE: Please modify this section according to your environment and requirements.
+  # *******************************************************************************
+  - path: /localization/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /localization/001-topic_status/initialpose-error }
+      - { type: link, link: /localization/001-topic_status/pose_twist_fusion_filter-error }
+      - { type: link, link: /localization/002-tf-error }
+      - { type: link, link: /localization/003-matching_score-error }
+      - { type: link, link: /localization/004-accuracy-error }
+
+  - path: /localization/comfortable_stop
+    type: and
+
+  - path: /localization/pull_over
+    type: and
+
+  - path: /localization/none
+    type: and
+
+  - path: /localization/001-topic_status/initialpose-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /localization/001-topic_status/initialpose
+
+  - path: /localization/001-topic_status/pose_twist_fusion_filter-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /localization/001-topic_status/pose_twist_fusion_filter
+
+  - path: /localization/002-tf-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /localization/002-tf
+
+  - path: /localization/003-matching_score-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /localization/003-matching_score
+
+  - path: /localization/004-accuracy-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /localization/004-accuracy
+
+  - path: /localization/001-topic_status/initialpose
+    type: diag
+    node: topic_state_monitor_initialpose3d
+    name: localization_topic_status
+    timeout: 1.0
+
+  - path: /localization/001-topic_status/pose_twist_fusion_filter
+    type: diag
+    node: topic_state_monitor_pose_twist_fusion_filter_pose
+    name: localization_topic_status
+    timeout: 1.0
+
+  - path: /localization/002-tf
+    type: diag
+    node: topic_state_monitor_transform_map_to_base_link
+    name: localization_topic_status
+    timeout: 1.0
+
+  - path: /localization/003-matching_score
+    type: diag
+    node: ndt_scan_matcher
+    name: scan_matching_status
+    timeout: 1.0
+
+  - path: /localization/004-accuracy
+    type: diag
+    node: localization_error_monitor
+    name: ellipse_error_status
+    timeout: 1.0
diff --git a/autoware_launch/config/system/tier4_diagnostics/map.yaml b/autoware_launch/config/system/tier4_diagnostics/map.yaml
new file mode 100644
index 0000000000..685956f160
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/map.yaml
@@ -0,0 +1,60 @@
+units:
+  - path: /map/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /map/emergency_stop }
+      - { type: link, link: /map/pull_over }
+      - { type: link, link: /map/comfortable_stop }
+
+  - path: /map/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /map/emergency_stop }
+      - { type: link, link: /map/comfortable_stop }
+
+  - path: /map/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /map/emergency_stop }
+
+  # *******************************************************************************
+  # NOTE: Please modify this section according to your environment and requirements.
+  # *******************************************************************************
+  - path: /map/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /map/001-topic_status/vector_map-error }
+      - { type: link, link: /map/001-topic_status/pointcloud_map-error }
+
+  - path: /map/comfortable_stop
+    type: and
+
+  - path: /map/pull_over
+    type: and
+
+  - path: /map/none
+    type: and
+
+  - path: /map/001-topic_status/vector_map-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /map/001-topic_status/vector_map
+
+  - path: /map/001-topic_status/pointcloud_map-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /map/001-topic_status/pointcloud_map
+
+  - path: /map/001-topic_status/vector_map
+    type: diag
+    node: topic_state_monitor_vector_map
+    name: map_topic_status
+    timeout: 1.0
+
+  - path: /map/001-topic_status/pointcloud_map
+    type: diag
+    node: topic_state_monitor_pointcloud_map
+    name: map_topic_status
+    timeout: 1.0
diff --git a/autoware_launch/config/system/tier4_diagnostics/perception.yaml b/autoware_launch/config/system/tier4_diagnostics/perception.yaml
new file mode 100644
index 0000000000..273e7ada84
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/perception.yaml
@@ -0,0 +1,86 @@
+units:
+  - path: /perception/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /perception/emergency_stop }
+      - { type: link, link: /perception/pull_over }
+      - { type: link, link: /perception/comfortable_stop }
+
+  - path: /perception/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /perception/emergency_stop }
+      - { type: link, link: /perception/comfortable_stop }
+
+  - path: /perception/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /perception/emergency_stop }
+
+  # *******************************************************************************
+  # NOTE: Please modify this section according to your environment and requirements.
+  # *******************************************************************************
+  - path: /perception/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /perception/001-topic_status/traffic_signals-error }
+      - { type: link, link: /perception/001-topic_status/objects-error }
+      - { type: link, link: /perception/001-topic_status/pointcloud-error }
+      - { type: link, link: /perception/002-detection_delay-error }
+
+  - path: /perception/comfortable_stop
+    type: and
+
+  - path: /perception/pull_over
+    type: and
+
+  - path: /perception/none
+    type: and
+
+  - path: /perception/001-topic_status/traffic_signals-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /perception/001-topic_status/traffic_signals
+
+  - path: /perception/001-topic_status/objects-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /perception/001-topic_status/objects
+
+  - path: /perception/001-topic_status/pointcloud-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /perception/001-topic_status/pointcloud
+
+  - path: /perception/002-detection_delay-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /perception/002-detection_delay
+
+  - path: /perception/001-topic_status/traffic_signals
+    type: diag
+    node: topic_state_monitor_traffic_light_recognition_traffic_signals
+    name: perception_topic_status
+    timeout: 1.0
+
+  - path: /perception/001-topic_status/objects
+    type: diag
+    node: topic_state_monitor_object_recognition_objects
+    name: perception_topic_status
+    timeout: 1.0
+
+  - path: /perception/001-topic_status/pointcloud
+    type: diag
+    node: topic_state_monitor_obstacle_segmentation_pointcloud
+    name: perception_topic_status
+    timeout: 1.0
+
+  - path: /perception/002-detection_delay
+    type: diag
+    node: multi_object_tracker
+    name: Perception delay check from original header stamp
+    timeout: 1.0
diff --git a/autoware_launch/config/system/tier4_diagnostics/planning.yaml b/autoware_launch/config/system/tier4_diagnostics/planning.yaml
new file mode 100644
index 0000000000..3a8240aedd
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/planning.yaml
@@ -0,0 +1,203 @@
+units:
+  - path: /planning/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /planning/emergency_stop }
+      - { type: link, link: /planning/pull_over }
+      - { type: link, link: /planning/comfortable_stop }
+
+  - path: /planning/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /planning/emergency_stop }
+      - { type: link, link: /planning/comfortable_stop }
+
+  - path: /planning/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /planning/emergency_stop }
+
+  # *******************************************************************************
+  # NOTE: Please modify this section according to your environment and requirements.
+  # *******************************************************************************
+  - path: /planning/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /planning/001-topic_status/route-error }
+      - { type: link, link: /planning/001-topic_status/trajectory-error }
+      - { type: link, link: /planning/003-trajectory_finite_validation-error }
+      - { type: link, link: /planning/004-trajectory_interval_validation-error }
+      - { type: link, link: /planning/005-trajectory_curvature_validation-error }
+      - { type: link, link: /planning/006-trajectory_relative_angle_validation-error }
+      - { type: link, link: /planning/007-trajectory_lateral_acceleration_validation-error }
+      - { type: link, link: /planning/008-trajectory_acceleration_validation-error }
+      - { type: link, link: /planning/009-trajectory_deceleration_validation-error }
+      - { type: link, link: /planning/010-trajectory_steering_validation-error }
+      - { type: link, link: /planning/011-trajectory_steering_rate_validation-error }
+      - { type: link, link: /planning/012-trajectory_velocity_deviation_validation-error }
+      # - { type: link, link: /planning/013-collision_checker-error }
+
+  - path: /planning/comfortable_stop
+    type: and
+
+  - path: /planning/pull_over
+    type: and
+
+  - path: /planning/none
+    type: and
+
+  - path: /planning/001-topic_status/route-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/001-topic_status/route
+
+  - path: /planning/001-topic_status/trajectory-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/001-topic_status/trajectory
+
+  - path: /planning/003-trajectory_finite_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/003-trajectory_finite_validation
+
+  - path: /planning/004-trajectory_interval_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/004-trajectory_interval_validation
+
+  - path: /planning/005-trajectory_curvature_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/005-trajectory_curvature_validation
+
+  - path: /planning/006-trajectory_relative_angle_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/006-trajectory_relative_angle_validation
+
+  - path: /planning/007-trajectory_lateral_acceleration_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/007-trajectory_lateral_acceleration_validation
+
+  - path: /planning/008-trajectory_acceleration_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/008-trajectory_acceleration_validation
+
+  - path: /planning/009-trajectory_deceleration_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/009-trajectory_deceleration_validation
+
+  - path: /planning/010-trajectory_steering_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/010-trajectory_steering_validation
+
+  - path: /planning/011-trajectory_steering_rate_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/011-trajectory_steering_rate_validation
+
+  - path: /planning/012-trajectory_velocity_deviation_validation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/012-trajectory_velocity_deviation_validation
+
+  - path: /planning/013-collision_checker-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/013-collision_checker
+
+  - path: /planning/001-topic_status/route
+    type: diag
+    node: topic_state_monitor_mission_planning_route
+    name: planning_topic_status
+    timeout: 1.0
+
+  - path: /planning/001-topic_status/trajectory
+    type: diag
+    node: topic_state_monitor_scenario_planning_trajectory
+    name: planning_topic_status
+    timeout: 1.0
+
+  - path: /planning/003-trajectory_finite_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_finite
+    timeout: 1.0
+
+  - path: /planning/004-trajectory_interval_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_interval
+    timeout: 1.0
+
+  - path: /planning/005-trajectory_curvature_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_curvature
+    timeout: 1.0
+
+  - path: /planning/006-trajectory_relative_angle_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_relative_angle
+    timeout: 1.0
+
+  - path: /planning/007-trajectory_lateral_acceleration_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_lateral_acceleration
+    timeout: 1.0
+
+  - path: /planning/008-trajectory_acceleration_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_acceleration
+    timeout: 1.0
+
+  - path: /planning/009-trajectory_deceleration_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_deceleration
+    timeout: 1.0
+
+  - path: /planning/010-trajectory_steering_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_steering
+    timeout: 1.0
+
+  - path: /planning/011-trajectory_steering_rate_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_steering_rate
+    timeout: 1.0
+
+  - path: /planning/012-trajectory_velocity_deviation_validation
+    type: diag
+    node: planning_validator
+    name: trajectory_validation_velocity_deviation
+    timeout: 1.0
+
+  - path: /planning/013-collision_checker
+    type: diag
+    node: collision_checker
+    name: collision_check
+    timeout: 1.0
diff --git a/autoware_launch/config/system/tier4_diagnostics/sensing.yaml b/autoware_launch/config/system/tier4_diagnostics/sensing.yaml
new file mode 100644
index 0000000000..a253858f7f
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/sensing.yaml
@@ -0,0 +1,35 @@
+units:
+  - path: /sensing/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /sensing/emergency_stop }
+      - { type: link, link: /sensing/pull_over }
+      - { type: link, link: /sensing/comfortable_stop }
+
+  - path: /sensing/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /sensing/emergency_stop }
+      - { type: link, link: /sensing/comfortable_stop }
+
+  - path: /sensing/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /sensing/emergency_stop }
+
+  # *******************************************************************************
+  # NOTE: Please modify this section according to your environment and requirements.
+  # *******************************************************************************
+  - path: /sensing/emergency_stop
+    type:
+      and
+      # - { type: link, link: /sensing/radar/001-connection }
+
+  - path: /sensing/pull_over
+    type: and
+
+  - path: /sensing/comfortable_stop
+    type: and
+
+  - path: /sensing/none
+    type: and
diff --git a/autoware_launch/config/system/tier4_diagnostics/system.yaml b/autoware_launch/config/system/tier4_diagnostics/system.yaml
new file mode 100644
index 0000000000..8735ba5a98
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/system.yaml
@@ -0,0 +1,60 @@
+units:
+  - path: /system/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /system/emergency_stop }
+      - { type: link, link: /system/pull_over }
+      - { type: link, link: /system/comfortable_stop }
+
+  - path: /system/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /system/emergency_stop }
+      - { type: link, link: /system/comfortable_stop }
+
+  - path: /system/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /system/emergency_stop }
+
+  # *******************************************************************************
+  # NOTE: Please modify this section according to your environment and requirements.
+  # *******************************************************************************
+  - path: /system/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /system/001-topic_status-error }
+      - { type: link, link: /system/002-emergency_stop_operation-error }
+
+  - path: /system/comfortable_stop
+    type: and
+
+  - path: /system/pull_over
+    type: and
+
+  - path: /system/none
+    type: and
+
+  - path: /system/001-topic_status-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /system/001-topic_status
+
+  - path: /system/002-emergency_stop_operation-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /system/002-emergency_stop_operation
+
+  - path: /system/001-topic_status
+    type: diag
+    node: topic_state_monitor_system_emergency_control_cmd
+    name: system_topic_status
+    timeout: 1.0
+
+  - path: /system/002-emergency_stop_operation
+    type: diag
+    node: vehicle_cmd_gate
+    name: emergency_stop_operation
+    timeout: 1.0
diff --git a/autoware_launch/config/system/tier4_diagnostics/vehicle.yaml b/autoware_launch/config/system/tier4_diagnostics/vehicle.yaml
new file mode 100644
index 0000000000..0034709dbc
--- /dev/null
+++ b/autoware_launch/config/system/tier4_diagnostics/vehicle.yaml
@@ -0,0 +1,88 @@
+units:
+  - path: /vehicle/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /vehicle/emergency_stop }
+      - { type: link, link: /vehicle/pull_over }
+      - { type: link, link: /vehicle/comfortable_stop }
+
+  - path: /vehicle/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /vehicle/emergency_stop }
+      - { type: link, link: /vehicle/comfortable_stop }
+
+  - path: /vehicle/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /vehicle/emergency_stop }
+
+  # *******************************************************************************
+  # NOTE: Please modify this section according to your environment and requirements.
+  # *******************************************************************************
+  - path: /vehicle/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /vehicle/001-topic_status/velocity-error }
+      - { type: link, link: /vehicle/001-topic_status/steering-error }
+      # - { type: link, link: /vehicle/005-vehicle_heartbeat-error }
+      # - { type: link, link: /vehicle/006-vehicle_errors-error }
+
+  - path: /vehicle/comfortable_stop
+    type: and
+    # list:
+    #   - { type: link, link: /vehicle/006-vehicle_errors }
+
+  - path: /vehicle/pull_over
+    type: and
+
+  - path: /vehicle/none
+    type: and
+
+  - path: /vehicle/001-topic_status/velocity-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /vehicle/001-topic_status/velocity
+
+  - path: /vehicle/001-topic_status/steering-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /vehicle/001-topic_status/steering
+
+  # - path: /vehicle/005-vehicle_heartbeat-error
+  #   type: warn-to-ok
+  #   item:
+  #     type: link
+  #     link: /vehicle/005-vehicle_heartbeat
+
+  # - path: /vehicle/006-vehicle_errors-error
+  #   type: warn-to-ok
+  #   item:
+  #     type: link
+  #     link: /vehicle/006-vehicle_errors
+
+  - path: /vehicle/001-topic_status/velocity
+    type: diag
+    node: topic_state_monitor_vehicle_status_velocity_status
+    name: vehicle_topic_status
+    timeout: 1.0
+
+  - path: /vehicle/001-topic_status/steering
+    type: diag
+    node: topic_state_monitor_vehicle_status_steering_status
+    name: vehicle_topic_status
+    timeout: 1.0
+
+  # - path: /vehicle/005-vehicle_heartbeat
+  #   type: diag
+  #   node:
+  #   name: vehicle_heartbeat_errors
+  #   timeout: 1.0
+
+  # - path: /vehicle/006-vehicle_errors
+  #   type: diag
+  #   node:
+  #   name: vehicle_errors
+  #   timeout: 1.0
diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
index b125d23186..b29819a1aa 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/diagnostics/autoware-main.yaml" description="diagnostic graph config"/>
+  <arg name="diagnostic_graph_aggregator_graph_path" default="$(find-pkg-share autoware_launch)/config/system/tier4_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_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml
index a01a5fca17..bf2ed9459d 100644
--- a/autoware_launch/launch/components/tier4_planning_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml
@@ -74,6 +74,12 @@
     <arg name="obstacle_stop_planner_acc_param_path" value="$(var motion_config_path)/obstacle_stop_planner/adaptive_cruise_control.param.yaml"/>
     <arg name="obstacle_cruise_planner_param_path" value="$(var motion_config_path)/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml"/>
     <arg name="motion_velocity_planner_param_path" value="$(var motion_config_path)/motion_velocity_planner/motion_velocity_planner.param.yaml"/>
+    <!-- <arg name="motion_velocity_planner_obstacle_stop_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_stop.param.yaml"/> -->
+    <!-- <arg name="motion_velocity_planner_obstacle_slow_down_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_slow_down.param.yaml"/> -->
+    <!-- <arg name="motion_velocity_planner_obstacle_cruise_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_cruise.param.yaml"/> -->
+    <arg name="motion_velocity_planner_obstacle_stop_module_param_path" value="$(find-pkg-share autoware_motion_velocity_obstacle_stop_module)/config/obstacle_stop.param.yaml"/>
+    <arg name="motion_velocity_planner_obstacle_slow_down_module_param_path" value="$(find-pkg-share autoware_motion_velocity_obstacle_slow_down_module)/config/obstacle_slow_down.param.yaml"/>
+    <arg name="motion_velocity_planner_obstacle_cruise_module_param_path" value="$(find-pkg-share autoware_motion_velocity_obstacle_cruise_module)/config/obstacle_cruise.param.yaml"/>
     <arg name="motion_velocity_planner_dynamic_obstacle_stop_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/dynamic_obstacle_stop.param.yaml"/>
     <arg name="motion_velocity_planner_out_of_lane_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/out_of_lane.param.yaml"/>
     <arg name="motion_velocity_planner_obstacle_velocity_limiter_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_velocity_limiter.param.yaml"/>
diff --git a/autoware_launch/launch/components/tier4_system_component.launch.xml b/autoware_launch/launch/components/tier4_system_component.launch.xml
index 31cc739683..6e0f334383 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/diagnostics/autoware-main.yaml"/>
+  <arg name="diagnostic_graph_aggregator_graph_path" default="$(find-pkg-share autoware_launch)/config/system/tier4_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/diagnostics/dummy_diag_publisher.param.yaml"/>
+    <arg name="dummy_diag_publisher_param_path" value="$(find-pkg-share autoware_launch)/config/system/tier4_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,12 @@
     <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="10.0"/>
+    <arg name="enable_terminal_log" value="false"/>
+  </include>
 </launch>
diff --git a/autoware_launch/launch/e2e_simulator.launch.xml b/autoware_launch/launch/e2e_simulator.launch.xml
index c4fd15b41c..03044cf090 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/diagnostics/autoware-awsim.yaml"/>
+      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/tier4_diagnostics/autoware-awsim.yaml"/>
       <!-- Sensing -->
       <arg name="launch_sensing_driver" value="$(var launch_sensing_driver)"/>
       <!-- Perception-->
diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
index 123bd01ed3..ad11e90bdf 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/diagnostics/autoware-main.yaml"/>
+      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/tier4_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 3c56e11412..3e0ed8d20c 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/diagnostics/autoware-main.yaml"/>
+      <arg name="diagnostic_graph_aggregator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/tier4_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 3f029ff75d..a1fe107c86 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -268,21 +268,21 @@ Visualization Manager:
               Primary Color: 174; 174; 174
           Enabled: true
           Name: Vehicle
-        - Class: rviz_plugins/MrmSummaryOverlayDisplay
-          Enabled: false
-          Font Size: 10
-          Left: 512
-          Max Letter Num: 100
-          Name: MRM Summary
-          Text Color: 25; 255; 240
-          Top: 64
+        - Class: autoware_string_stamped_rviz_plugin/StringStampedOverlayDisplay
+          Enabled: true
+          Font Size: 12
+          Left: 10
+          Max Letter Num: 70
+          Name: Error Diag Graph
+          Text Color: 51; 201; 220
+          Top: 10
           Topic:
-            Depth: 5
+            Depth: 1
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /system/emergency/hazard_status
-          Value: false
+            Value: /logging_diag_graph/debug/error_graph_text
+          Value: true
           Value height offset: 0
       Enabled: true
       Name: System