diff --git a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml
index 9a32c7d329..7f318a5309 100644
--- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml
+++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml
@@ -75,4 +75,4 @@
       average_num: 1000
       steering_offset_limit: 0.02
 
-    debug_publish_predicted_trajectory: false  # publish debug predicted trajectory in Frenet coordinate
+    debug_publish_predicted_trajectory: true  # publish debug predicted trajectory in Frenet coordinate
diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml
index 87d9bf0868..1ad35e4aa5 100644
--- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml
+++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml
@@ -62,11 +62,11 @@
 
     # acceleration limit
     max_acc: 1.86
-    min_acc: -6.00
+    min_acc: -3.36
 
     # jerk limit
     max_jerk: 2.0
-    min_jerk: -30.0
+    min_jerk: -5.0
     max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1]
 
     # slope compensation
diff --git a/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml
index b3600f1477..b8c21cb885 100644
--- a/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml
+++ b/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml
@@ -10,7 +10,7 @@
     filter_activated_velocity_threshold: 1.0
     external_emergency_stop_heartbeat_timeout: 0.0
     stop_hold_acceleration: -1.5
-    emergency_acceleration: -4.0
+    emergency_acceleration: -2.5
     moderate_stop_service_acceleration: -1.5
     stopped_state_entry_duration_time: 0.1
     stop_check_duration: 1.0
@@ -19,8 +19,8 @@
       reference_speed_points: [0.1, 0.2, 20.0, 30.0]
       steer_lim: [1.0, 1.0, 1.0, 0.8]
       steer_rate_lim: [1.0, 1.0, 1.0, 0.8]
-      lon_acc_lim: [6.0, 6.0, 6.0, 6.0]
-      lon_jerk_lim: [80.0, 30.0, 30.0, 30.0]
+      lon_acc_lim: [5.0, 5.0, 5.0, 4.0]
+      lon_jerk_lim: [5.0, 5.0, 5.0, 4.0]
       lat_acc_lim: [5.0, 5.0, 5.0, 4.0]
       lat_jerk_lim: [7.0, 7.0, 7.0, 6.0]
       actual_steer_diff_lim: [1.0, 1.0, 1.0, 0.8]
diff --git a/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml
index e123b0dda6..5d68825f4d 100644
--- a/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml
+++ b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml
@@ -50,3 +50,8 @@
       consider_only_routable_neighbours: false
 
     reference_path_resolution: 0.5 #[m]
+
+    # debug parameters
+    publish_processing_time: true
+    publish_processing_time_detail: true
+    publish_debug_markers: true
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml
index 5e381a2bd3..2214e9d318 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml
@@ -55,6 +55,7 @@
         maximum_jerk: 1.0
         path_priority: "efficient_path" # "efficient_path" or "close_goal"
         efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking)
+        lane_departure_check_expansion_margin: 0.0
 
         # shift parking
         shift_parking:
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml
index 3dbc3b01dd..94595c1397 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml
@@ -4,9 +4,8 @@
       backward_lane_length: 200.0 #[m]
       prepare_duration: 3.0         # [s]
 
-      backward_length_buffer_for_end_of_lane: 7.0 # [m]
-      backward_length_buffer_for_blocking_object: 0.5 # [m]
-      lane_change_finish_judge_buffer: 0.0      # [m]
+      backward_length_buffer_for_end_of_lane: 3.0 # [m]
+      backward_length_buffer_for_blocking_object: 3.0 # [m]
 
       lane_changing_lateral_jerk: 0.3             # [m/s3]
 
@@ -27,18 +26,31 @@
       min_longitudinal_acc: -0.5
       max_longitudinal_acc: 0.2
 
+      skip_process:
+        longitudinal_distance_diff_threshold:
+          prepare: 1.0
+          lane_changing: 1.0
+
       # safety check
       safety_check:
         allow_loose_check_for_cancel: true
         collision_check_yaw_diff_threshold: 3.1416
         execution:
           expected_front_deceleration: -1.0
-          expected_rear_deceleration: -1.0
-          rear_vehicle_reaction_time: 2.0
-          rear_vehicle_safety_time_margin: 1.0
+          expected_rear_deceleration: -1.5
+          rear_vehicle_reaction_time: 2.5
+          rear_vehicle_safety_time_margin: 0.8
           lateral_distance_max_threshold: 2.0
-          longitudinal_distance_min_threshold: 3.0
-          longitudinal_velocity_delta_time: 0.8
+          longitudinal_distance_min_threshold: 6.0
+          longitudinal_velocity_delta_time: 0.0
+        parked:
+          expected_front_deceleration: -1.0
+          expected_rear_deceleration: -2.0
+          rear_vehicle_reaction_time: 1.0
+          rear_vehicle_safety_time_margin: 0.8
+          lateral_distance_max_threshold: 1.0
+          longitudinal_distance_min_threshold: 6.0
+          longitudinal_velocity_delta_time: 0.0
         cancel:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -2.5
@@ -46,15 +58,15 @@
           rear_vehicle_safety_time_margin: 0.5
           lateral_distance_max_threshold: 0.75
           longitudinal_distance_min_threshold: 2.0
-          longitudinal_velocity_delta_time: 0.3
+          longitudinal_velocity_delta_time: 0.0
         stuck:
           expected_front_deceleration: -1.0
-          expected_rear_deceleration: -2.0
-          rear_vehicle_reaction_time: 1.5
-          rear_vehicle_safety_time_margin: 0.8
-          lateral_distance_max_threshold: 1.0
-          longitudinal_distance_min_threshold: 2.5
-          longitudinal_velocity_delta_time: 0.6
+          expected_rear_deceleration: -1.0
+          rear_vehicle_reaction_time: 2.0
+          rear_vehicle_safety_time_margin: 1.0
+          lateral_distance_max_threshold: 2.0
+          longitudinal_distance_min_threshold: 6.0
+          longitudinal_velocity_delta_time: 0.0
 
         # lane expansion for object filtering
         lane_expansion:
@@ -109,7 +121,9 @@
         overhang_tolerance: 0.0             # [m]
         unsafe_hysteresis_threshold: 5      # [/]
 
-      finish_judge_lateral_threshold: 0.2        # [m]
+      lane_change_finish_judge_buffer: 2.0      # [m]
+      finish_judge_lateral_threshold: 0.1        # [m]
+      finish_judge_lateral_angle_deviation: 1.0 # [deg]
 
       # debug
       publish_debug_marker: true
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 768a11de4b..73fbc12b65 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
@@ -70,15 +70,15 @@
           collision_start_margin_time: 2.0
           collision_end_margin_time: 0.0
         partially_prioritized:
-          collision_start_margin_time: 4.0
-          collision_end_margin_time: 6.0
+          collision_start_margin_time: 3.0
+          collision_end_margin_time: 2.0
         not_prioritized:
-          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
-          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
+          collision_start_margin_time: 3.0
+          collision_end_margin_time: 2.0
         yield_on_green_traffic_light:
           distance_to_assigned_lanelet_start: 10.0
           duration: 8.0
-          object_dist_to_stopline: 10.0 # [m]
+          object_dist_to_stopline: 10.0
         ignore_on_amber_traffic_light:
           object_expected_deceleration:
             car: 2.0
diff --git a/autoware_launch/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml b/autoware_launch/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml
index 7868246909..81bc9b9c0b 100644
--- a/autoware_launch/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml
+++ b/autoware_launch/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml
@@ -2,5 +2,5 @@
   ros__parameters:
     update_rate: 10
     min_acceleration: -1.0
-    max_jerk: 0.6
-    min_jerk: -0.6
+    max_jerk: 0.3
+    min_jerk: -0.3
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml
index 371b866497..2e224e6573 100644
--- a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml
+++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml
@@ -6,7 +6,7 @@ files:
   - { path: $(dirname)/perception.yaml }
   - { path: $(dirname)/planning.yaml }
   - { path: $(dirname)/others.yaml }
-  # - { path: $(dirname)/sensing.yaml }
+  - { path: $(dirname)/sensing.yaml }
   - { path: $(dirname)/system.yaml }
   - { path: $(dirname)/vehicle.yaml }
 
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml
index 92bde38413..bf7e053a35 100644
--- a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml
+++ b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml
@@ -27,7 +27,7 @@ units:
       - { 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/010-max_distance_deviation-error }
       - { type: link, link: /control/011-slip_detection }
 
   - path: /control/comfortable_stop
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml
index 62ae6d2483..c0294052c3 100644
--- a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml
+++ b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml
@@ -25,8 +25,8 @@ units:
     list:
       #- { type: link, link: /others/002-blockage_validation-error }
       - { type: link, link: /others/004-concat_status }
-      - { type: link, link: /others/005-visibility_validation-error }
-      - { type: link, link: /others/012-vehicle_stuck_checker }
+      # - { type: link, link: /others/005-visibility_validation-error }
+      # - { type: link, link: /others/012-vehicle_stuck_checker }
 
   - path: /others/comfortable_stop
     type: and
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml
index e983c9e799..3a8240aedd 100644
--- a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml
+++ b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml
@@ -35,7 +35,7 @@ units:
       - { 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 }
+      # - { type: link, link: /planning/013-collision_checker-error }
 
   - path: /planning/comfortable_stop
     type: and
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml b/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml
index 748d9f4e42..9641633a3c 100644
--- a/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml
+++ b/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml
@@ -25,16 +25,16 @@ units:
     list:
       # - { type: link, link: /sensing/imu/001-monitor-error }
       # - { type: link, link: /sensing/imu/002-connection }
-      # - { type: link, link: /sensing/lidar/pndr/001-connection }
-      # - { type: link, link: /sensing/lidar/pndr/002-temperature-error }
-      # - { type: link, link: /sensing/lidar/pndr/003-ptp }
+      - { type: link, link: /sensing/lidar/pndr/001-connection }
+      - { type: link, link: /sensing/lidar/pndr/002-temperature-error }
+      - { type: link, link: /sensing/lidar/pndr/003-ptp }
       - { type: link, link: /sensing/camera/001-connection }
       - { type: link, link: /sensing/radar/001-connection }
 
   - path: /sensing/pull_over
     type: and
     list:
-      # - { type: link, link: /sensing/lidar/pndr/002-temperature }
+      - { type: link, link: /sensing/lidar/pndr/002-temperature }
       - { type: link, link: /sensing/gnss/001-connection-error }
 
   - path: /sensing/comfortable_stop
@@ -47,10 +47,11 @@ units:
       - { type: link, link: /sensing/gnss/002-quality }
 
   # Intermediate paths
-  # - path: /sensing/lidar/pndr/002-temperature-error
-  #   type: warn-to-ok
-  #   list:
-  #     - { type: link, link: /sensing/lidar/pndr/002-temperature }
+  - path: /sensing/lidar/pndr/002-temperature-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /sensing/lidar/pndr/002-temperature
 
   # - path: /sensing/imu/001-monitor-error
   #   type: warn-to-ok
@@ -64,41 +65,41 @@ units:
       type: link
       link: /sensing/gnss/001-connection
 
-  # - path: /sensing/lidar/pndr/001-connection
-  #   type: and
-  #   list:
-  #     - { type: link, link: /sensing/lidar/front_lower/connection }
-  #     - { type: link, link: /sensing/lidar/front_upper/connection }
-  #     - { type: link, link: /sensing/lidar/left_lower/connection }
-  #     - { type: link, link: /sensing/lidar/left_upper/connection }
-  #     - { type: link, link: /sensing/lidar/right_lower/connection }
-  #     - { type: link, link: /sensing/lidar/right_upper/connection }
-  #     - { type: link, link: /sensing/lidar/rear_lower/connection }
-  #     - { type: link, link: /sensing/lidar/rear_upper/connection }
-
-  # - path: /sensing/lidar/pndr/002-temperature
-  #   type: and
-  #   list:
-  #     - { type: link, link: /sensing/lidar/front_lower/temperature }
-  #     - { type: link, link: /sensing/lidar/front_upper/temperature }
-  #     - { type: link, link: /sensing/lidar/left_lower/temperature }
-  #     - { type: link, link: /sensing/lidar/left_upper/temperature }
-  #     - { type: link, link: /sensing/lidar/right_lower/temperature }
-  #     - { type: link, link: /sensing/lidar/right_upper/temperature }
-  #     - { type: link, link: /sensing/lidar/rear_lower/temperature }
-  #     - { type: link, link: /sensing/lidar/rear_upper/temperature }
-
-  # - path: /sensing/lidar/pndr/003-ptp
-  #   type: and
-  #   list:
-  #     - { type: link, link: /sensing/lidar/front_lower/ptp }
-  #     - { type: link, link: /sensing/lidar/front_upper/ptp }
-  #     - { type: link, link: /sensing/lidar/left_lower/ptp }
-  #     - { type: link, link: /sensing/lidar/left_upper/ptp }
-  #     - { type: link, link: /sensing/lidar/right_lower/ptp }
-  #     - { type: link, link: /sensing/lidar/right_upper/ptp }
-  #     - { type: link, link: /sensing/lidar/rear_lower/ptp }
-  #     - { type: link, link: /sensing/lidar/rear_upper/ptp }
+  - path: /sensing/lidar/pndr/001-connection
+    type: and
+    list:
+      - { type: link, link: /sensing/lidar/front_lower/connection }
+      - { type: link, link: /sensing/lidar/front_upper/connection }
+      - { type: link, link: /sensing/lidar/left_lower/connection }
+      - { type: link, link: /sensing/lidar/left_upper/connection }
+      - { type: link, link: /sensing/lidar/right_lower/connection }
+      - { type: link, link: /sensing/lidar/right_upper/connection }
+      - { type: link, link: /sensing/lidar/rear_lower/connection }
+      - { type: link, link: /sensing/lidar/rear_upper/connection }
+
+  - path: /sensing/lidar/pndr/002-temperature
+    type: and
+    list:
+      - { type: link, link: /sensing/lidar/front_lower/temperature }
+      - { type: link, link: /sensing/lidar/front_upper/temperature }
+      - { type: link, link: /sensing/lidar/left_lower/temperature }
+      - { type: link, link: /sensing/lidar/left_upper/temperature }
+      - { type: link, link: /sensing/lidar/right_lower/temperature }
+      - { type: link, link: /sensing/lidar/right_upper/temperature }
+      - { type: link, link: /sensing/lidar/rear_lower/temperature }
+      - { type: link, link: /sensing/lidar/rear_upper/temperature }
+
+  - path: /sensing/lidar/pndr/003-ptp
+    type: and
+    list:
+      - { type: link, link: /sensing/lidar/front_lower/ptp }
+      - { type: link, link: /sensing/lidar/front_upper/ptp }
+      - { type: link, link: /sensing/lidar/left_lower/ptp }
+      - { type: link, link: /sensing/lidar/left_upper/ptp }
+      - { type: link, link: /sensing/lidar/right_lower/ptp }
+      - { type: link, link: /sensing/lidar/right_upper/ptp }
+      - { type: link, link: /sensing/lidar/rear_lower/ptp }
+      - { type: link, link: /sensing/lidar/rear_upper/ptp }
 
   - path: /sensing/camera/001-connection
     type: and
@@ -207,3 +208,131 @@ units:
     node: topic_state_monitor_radar_rear_right
     name: radar_rear_right_topic_status
     timeout: 1.0
+
+  - path: /sensing/lidar/front_lower/connection
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/front_lower:"
+    name: pandar_connection
+    timeout: 5.0
+  - path: /sensing/lidar/front_lower/ptp
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/front_lower:"
+    name: pandar_ptp
+    timeout: 5.0
+  - path: /sensing/lidar/front_lower/temperature
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/front_lower:"
+    name: pandar_temperature
+    timeout: 5.0
+
+  - path: /sensing/lidar/front_upper/connection
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/front_upper:"
+    name: pandar_connection
+    timeout: 5.0
+  - path: /sensing/lidar/front_upper/ptp
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/front_upper:"
+    name: pandar_ptp
+    timeout: 5.0
+  - path: /sensing/lidar/front_upper/temperature
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/front_upper:"
+    name: pandar_temperature
+    timeout: 5.0
+
+  - path: /sensing/lidar/left_lower/connection
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/left_lower:"
+    name: pandar_connection
+    timeout: 5.0
+  - path: /sensing/lidar/left_lower/ptp
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/left_lower:"
+    name: pandar_ptp
+    timeout: 5.0
+  - path: /sensing/lidar/left_lower/temperature
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/left_lower:"
+    name: pandar_temperature
+    timeout: 5.0
+
+  - path: /sensing/lidar/left_upper/connection
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/left_upper:"
+    name: pandar_connection
+    timeout: 5.0
+  - path: /sensing/lidar/left_upper/ptp
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/left_upper:"
+    name: pandar_ptp
+    timeout: 5.0
+  - path: /sensing/lidar/left_upper/temperature
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/left_upper:"
+    name: pandar_temperature
+    timeout: 5.0
+
+  - path: /sensing/lidar/right_lower/connection
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/right_lower:"
+    name: pandar_connection
+    timeout: 5.0
+  - path: /sensing/lidar/right_lower/ptp
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/right_lower:"
+    name: pandar_ptp
+    timeout: 5.0
+  - path: /sensing/lidar/right_lower/temperature
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/right_lower:"
+    name: pandar_temperature
+    timeout: 5.0
+
+  - path: /sensing/lidar/right_upper/connection
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/right_upper:"
+    name: pandar_connection
+    timeout: 5.0
+  - path: /sensing/lidar/right_upper/ptp
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/right_upper:"
+    name: pandar_ptp
+    timeout: 5.0
+  - path: /sensing/lidar/right_upper/temperature
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/right_upper:"
+    name: pandar_temperature
+    timeout: 5.0
+
+  - path: /sensing/lidar/rear_lower/connection
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/rear_lower:"
+    name: pandar_connection
+    timeout: 5.0
+  - path: /sensing/lidar/rear_lower/ptp
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/rear_lower:"
+    name: pandar_ptp
+    timeout: 5.0
+  - path: /sensing/lidar/rear_lower/temperature
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/rear_lower:"
+    name: pandar_temperature
+    timeout: 5.0
+
+  - path: /sensing/lidar/rear_upper/connection
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/rear_upper:"
+    name: pandar_connection
+    timeout: 5.0
+  - path: /sensing/lidar/rear_upper/ptp
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/rear_upper:"
+    name: pandar_ptp
+    timeout: 5.0
+  - path: /sensing/lidar/rear_upper/temperature
+    type: diag
+    node: "pandar_monitor: /sensing/lidar/rear_upper:"
+    name: pandar_temperature
+    timeout: 5.0
diff --git a/autoware_launch/launch/autoware.main.launch.xml b/autoware_launch/launch/autoware.main.launch.xml
new file mode 100644
index 0000000000..9c517db7a4
--- /dev/null
+++ b/autoware_launch/launch/autoware.main.launch.xml
@@ -0,0 +1,153 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <!-- Essential parameters -->
+  <arg name="map_path" description="point cloud and lanelet2 map directory path"/>
+  <arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
+  <arg name="sensor_model" default="sample_sensor_kit" description="sensor model name"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="data_path" default="$(env HOME)/autoware_data" description="packages data and artifacts directory path"/>
+
+  <!-- launch module preset -->
+  <arg name="planning_module_preset" default="x2" description="planning module preset"/>
+
+  <!-- Optional parameters -->
+  <!-- Modules to be launched -->
+  <arg name="launch_vehicle" default="true" description="launch vehicle"/>
+  <arg name="launch_system" default="true" description="launch system"/>
+  <arg name="launch_map" default="true" description="launch map"/>
+  <arg name="launch_sensing" default="true" description="launch sensing"/>
+  <arg name="launch_sensing_driver" default="true" description="launch sensing driver"/>
+  <arg name="launch_localization" default="true" description="launch localization"/>
+  <arg name="launch_perception" default="true" description="launch perception"/>
+  <arg name="launch_planning" default="true" description="launch planning"/>
+  <arg name="launch_control" default="true" description="launch control"/>
+  <arg name="launch_api" default="true" description="launch api"/>
+  <arg name="launch_v2x" default="true" description="launch V2X"/>
+  <arg name="launch_l4_toolkit" default="true" description="launch L4 Toolkit"/>
+  <!-- Global parameters -->
+  <arg name="use_sim_time" default="false" description="use_sim_time"/>
+  <!-- Vehicle -->
+  <arg name="vehicle_id" default="$(env VEHICLE_ID default)" description="vehicle specific ID"/>
+  <arg name="launch_vehicle_interface" default="true" description="launch vehicle interface"/>
+  <!-- Control -->
+  <arg name="check_external_emergency_heartbeat" default="false"/>
+  <!-- Map -->
+  <arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
+  <arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>
+  <!-- System -->
+  <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="system_error_monitor_param_path" default="$(find-pkg-share autoware_launch)/config/system/system_error_monitor/system_error_monitor.param.yaml"/>
+  <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"/>
+  <!-- Tools -->
+  <arg name="rviz" default="false" description="launch rviz"/>
+  <arg name="rviz_config" default="$(find-pkg-share autoware_launch)/rviz/autoware.rviz" description="rviz config"/>
+  <arg name="rviz_respawn" default="true"/>
+  <!-- Perception -->
+  <arg name="perception_mode" default="camera_lidar_radar_fusion" description="select perception mode. camera_lidar_radar_fusion, camera_lidar_fusion, lidar_radar_fusion, lidar, radar"/>
+  <arg name="traffic_light_recognition/enable_fine_detection" default="true" description="enable traffic light fine detection"/>
+  <arg name="centerpoint_model_path" default="/opt/autoware/mlmodels/centerpoint" description="path of centerpoint model file"/>
+  <!-- Auto mode setting-->
+  <arg name="enable_all_modules_auto_mode" default="false" description="enable all module's auto mode"/>
+  <arg name="is_simulation" default="false" description="Autoware's behavior will change depending on whether this is a simulation or not."/>
+
+  <!-- Global parameters -->
+  <group scoped="false">
+    <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+    </include>
+  </group>
+
+  <!-- Pointcloud container -->
+  <include file="$(find-pkg-share autoware_launch)/launch/pointcloud_container.launch.py">
+    <arg name="use_multithread" value="true"/>
+    <arg name="container_name" value="$(var pointcloud_container_name)"/>
+  </include>
+
+  <!-- Vehicle -->
+  <group if="$(var launch_vehicle)">
+    <include file="$(find-pkg-share tier4_vehicle_launch)/launch/vehicle.launch.xml">
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+      <arg name="sensor_model" value="$(var sensor_model)"/>
+      <arg name="vehicle_id" value="$(var vehicle_id)"/>
+      <arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>
+      <arg name="config_dir" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/$(var sensor_model)"/>
+      <arg name="raw_vehicle_cmd_converter_param_path" value="$(find-pkg-share autoware_launch)/config/vehicle/raw_vehicle_cmd_converter/raw_vehicle_cmd_converter.param.yaml"/>
+    </include>
+  </group>
+
+  <!-- System -->
+  <group if="$(var launch_system)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_system_component.launch.xml"/>
+  </group>
+
+  <!-- Map -->
+  <group if="$(var launch_map)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_map_component.launch.xml"/>
+  </group>
+
+  <!-- Sensing -->
+  <group if="$(var launch_sensing)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_sensing_component.launch.xml"/>
+  </group>
+
+  <!-- Localization -->
+  <group if="$(var launch_localization)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_localization_component.launch.xml"/>
+  </group>
+
+  <!-- Perception -->
+  <group if="$(var launch_perception)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_perception_component.launch.xml">
+      <arg name="data_path" value="$(var data_path)"/>
+    </include>
+  </group>
+
+  <!-- Planning -->
+  <!-- <group if="$(var launch_planning)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_planning_component.launch.xml">
+      <arg name="module_preset" value="$(var planning_module_preset)"/>
+      <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+      <arg name="is_simulation" value="$(var is_simulation)"/>
+    </include>
+  </group> -->
+
+  <!-- Control -->
+  <!-- <group if="$(var launch_control)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_control_component.launch.xml"/>
+  </group> -->
+
+  <!-- API -->
+  <!-- <group if="$(var launch_api)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_autoware_api_component.launch.xml"/>
+  </group> -->
+
+  <!-- V2X -->
+  <group if="$(var launch_v2x)">
+    <include file="$(find-pkg-share jpn_signal_launcher)/launch/jpn_signal.launch.xml">
+      <arg name="map_path" value="$(var map_path)"/>
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+      <arg name="vehicle_id" value="1"/>
+    </include>
+  </group>
+
+  <!-- L4 Toolkit -->
+  <group if="$(var launch_l4_toolkit)">
+    <include file="$(find-pkg-share l4_toolkit_launch)/launch/l4_toolkit.launch.xml"/>
+  </group>
+
+  <!-- Tools -->
+  <!-- <group>
+    <node
+      pkg="rviz2"
+      exec="rviz2"
+      name="rviz2"
+      output="screen"
+      args="-d $(var rviz_config) -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png"
+      if="$(var rviz)"
+      respawn="$(var rviz_respawn)"
+    />
+  </group> -->
+</launch>
diff --git a/autoware_launch/launch/autoware.sub.launch.xml b/autoware_launch/launch/autoware.sub.launch.xml
new file mode 100644
index 0000000000..873db6a22d
--- /dev/null
+++ b/autoware_launch/launch/autoware.sub.launch.xml
@@ -0,0 +1,153 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <!-- Essential parameters -->
+  <arg name="map_path" description="point cloud and lanelet2 map directory path"/>
+  <arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
+  <arg name="sensor_model" default="sample_sensor_kit" description="sensor model name"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="data_path" default="$(env HOME)/autoware_data" description="packages data and artifacts directory path"/>
+
+  <!-- launch module preset -->
+  <arg name="planning_module_preset" default="x2" description="planning module preset"/>
+
+  <!-- Optional parameters -->
+  <!-- Modules to be launched -->
+  <arg name="launch_vehicle" default="true" description="launch vehicle"/>
+  <arg name="launch_system" default="true" description="launch system"/>
+  <arg name="launch_map" default="true" description="launch map"/>
+  <arg name="launch_sensing" default="true" description="launch sensing"/>
+  <arg name="launch_sensing_driver" default="true" description="launch sensing driver"/>
+  <arg name="launch_localization" default="true" description="launch localization"/>
+  <arg name="launch_perception" default="true" description="launch perception"/>
+  <arg name="launch_planning" default="true" description="launch planning"/>
+  <arg name="launch_control" default="true" description="launch control"/>
+  <arg name="launch_api" default="true" description="launch api"/>
+  <arg name="launch_v2x" default="true" description="launch V2X"/>
+  <arg name="launch_l4_toolkit" default="true" description="launch L4 Toolkit"/>
+  <!-- Global parameters -->
+  <arg name="use_sim_time" default="false" description="use_sim_time"/>
+  <!-- Vehicle -->
+  <arg name="vehicle_id" default="$(env VEHICLE_ID default)" description="vehicle specific ID"/>
+  <arg name="launch_vehicle_interface" default="true" description="launch vehicle interface"/>
+  <!-- Control -->
+  <arg name="check_external_emergency_heartbeat" default="false"/>
+  <!-- Map -->
+  <arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
+  <arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>
+  <!-- System -->
+  <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="system_error_monitor_param_path" default="$(find-pkg-share autoware_launch)/config/system/system_error_monitor/system_error_monitor.param.yaml"/>
+  <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"/>
+  <!-- Tools -->
+  <arg name="rviz" default="false" description="launch rviz"/>
+  <arg name="rviz_config" default="$(find-pkg-share autoware_launch)/rviz/autoware.rviz" description="rviz config"/>
+  <arg name="rviz_respawn" default="true"/>
+  <!-- Perception -->
+  <arg name="perception_mode" default="camera_lidar_radar_fusion" description="select perception mode. camera_lidar_radar_fusion, camera_lidar_fusion, lidar_radar_fusion, lidar, radar"/>
+  <arg name="traffic_light_recognition/enable_fine_detection" default="true" description="enable traffic light fine detection"/>
+  <arg name="centerpoint_model_path" default="/opt/autoware/mlmodels/centerpoint" description="path of centerpoint model file"/>
+  <!-- Auto mode setting-->
+  <arg name="enable_all_modules_auto_mode" default="false" description="enable all module's auto mode"/>
+  <arg name="is_simulation" default="false" description="Autoware's behavior will change depending on whether this is a simulation or not."/>
+
+  <!-- Global parameters -->
+  <group scoped="false">
+    <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+    </include>
+  </group>
+
+  <!-- Pointcloud container -->
+  <!-- <include file="$(find-pkg-share autoware_launch)/launch/pointcloud_container.launch.py">
+    <arg name="use_multithread" value="true"/>
+    <arg name="container_name" value="$(var pointcloud_container_name)"/>
+  </include> -->
+
+  <!-- Vehicle -->
+  <!-- <group if="$(var launch_vehicle)">
+    <include file="$(find-pkg-share tier4_vehicle_launch)/launch/vehicle.launch.xml">
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+      <arg name="sensor_model" value="$(var sensor_model)"/>
+      <arg name="vehicle_id" value="$(var vehicle_id)"/>
+      <arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>
+      <arg name="config_dir" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/$(var sensor_model)"/>
+      <arg name="raw_vehicle_cmd_converter_param_path" value="$(find-pkg-share autoware_launch)/config/vehicle/raw_vehicle_cmd_converter/raw_vehicle_cmd_converter.param.yaml"/>
+    </include>
+  </group> -->
+
+  <!-- System -->
+  <!-- <group if="$(var launch_system)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_system_component.launch.xml"/>
+  </group> -->
+
+  <!-- Map -->
+  <!-- <group if="$(var launch_map)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_map_component.launch.xml"/>
+  </group> -->
+
+  <!-- Sensing -->
+  <!-- <group if="$(var launch_sensing)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_sensing_component.launch.xml"/>
+  </group> -->
+
+  <!-- Localization -->
+  <!-- <group if="$(var launch_localization)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_localization_component.launch.xml"/>
+  </group> -->
+
+  <!-- Perception -->
+  <!-- <group if="$(var launch_perception)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_perception_component.launch.xml">
+      <arg name="data_path" value="$(var data_path)"/>
+    </include>
+  </group> -->
+
+  <!-- Planning -->
+  <group if="$(var launch_planning)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_planning_component.launch.xml">
+      <arg name="module_preset" value="$(var planning_module_preset)"/>
+      <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+      <arg name="is_simulation" value="$(var is_simulation)"/>
+    </include>
+  </group>
+
+  <!-- Control -->
+  <group if="$(var launch_control)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_control_component.launch.xml"/>
+  </group>
+
+  <!-- API -->
+  <group if="$(var launch_api)">
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_autoware_api_component.launch.xml"/>
+  </group>
+
+  <!-- V2X -->
+  <!-- <group if="$(var launch_v2x)">
+    <include file="$(find-pkg-share jpn_signal_launcher)/launch/jpn_signal.launch.xml">
+      <arg name="map_path" value="$(var map_path)"/>
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+      <arg name="vehicle_id" value="1"/>
+    </include>
+  </group> -->
+
+  <!-- L4 Toolkit -->
+  <!-- <group if="$(var launch_l4_toolkit)">
+    <include file="$(find-pkg-share l4_toolkit_launch)/launch/l4_toolkit.launch.xml"/>
+  </group> -->
+
+  <!-- Tools -->
+  <group>
+    <node
+      pkg="rviz2"
+      exec="rviz2"
+      name="rviz2"
+      output="screen"
+      args="-d $(var rviz_config) -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png"
+      if="$(var rviz)"
+      respawn="$(var rviz_respawn)"
+    />
+  </group>
+</launch>
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index fc757b8156..d5d13aa3d2 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -2396,7 +2396,7 @@ Visualization Manager:
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /control/trajectory_follower/lateral/predicted_trajectory
+            Value: /control/trajectory_follower/controller_node_exe/debug/predicted_trajectory_in_frenet_coordinate
           Value: true
           View Path:
             Alpha: 1