MRM when MPT fails

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

Author: PanConChicharron

autoware_launch/config/system/diagnostics/autoware-main.yaml

@@ -61,8 +61,9 @@ units:
   - path: /autoware/modes/emergency_stop
     type: and
     list:
-      - { type: link, link: /autoware/vehicle }
+      - { type: link, link: /autoware/planning }
       - { type: link, link: /autoware/system }
+      - { type: link, link: /autoware/vehicle }
 
   - path: /autoware/debug/tools
     type: and

autoware_launch/config/system/diagnostics/planning.yaml

@@ -8,7 +8,8 @@ units:
         list:
           - { type: link, link: /autoware/planning/topic_rate_check/route }
           - { type: link, link: /autoware/planning/topic_rate_check/trajectory }
-          - { type: link, link: /autoware/planning/trajectory_validation }
+          - { type: link, link: /autoware/planning/trajectory_validation } # This line references the path defined below for /autoware/planning/trajectory_validation
+          - { type: link, link: /autoware/planning/emergency_stop } # This line references the path defined below for /autoware/planning/emergency_stop
 
   - path: /autoware/planning/trajectory_validation
     type: and
@@ -24,6 +25,11 @@ units:
       - { type: link, link: /autoware/planning/trajectory_validation/steering_rate }
       - { type: link, link: /autoware/planning/trajectory_validation/velocity_deviation }
 
+  - path: /autoware/planning/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /planning/path_optimizer_emergency_stop-error }
+
   - path: /autoware/planning/routing/state
     type: diag
     node: component_state_diagnostics
@@ -88,3 +94,15 @@ units:
     type: diag
     node: planning_validator
     name: trajectory_validation_velocity_deviation
+
+  - path: /planning/path_optimizer_emergency_stop-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/path_optimizer_emergency_stop
+
+  - path: /planning/path_optimizer_emergency_stop
+    type: diag
+    node: path_optimizer
+    name: path_optimizer_emergency_stop
+    timeout: 1.0