feat(system_diagnostic_monitor): add mrm recoverable option (autoware…

…foundation#847) * feat: add mrm recoverable option Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * feat: add fatal_error path to implement MRM auto-recover Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * set vehicle-emergency-button as fatal error Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> --------- Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
technolojin · Nov 23, 2024 · 7589136 · 7589136
1 parent 0edf0e6
commit 7589136
Show file tree

Hide file tree

Showing 9 changed files with 200 additions and 0 deletions.
diff --git a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml
@@ -11,6 +11,8 @@
     use_parking_after_stopped: false
     use_pull_over: false
     use_comfortable_stop: true
+    is_mrm_recoverable: false
+    use_pull_over_after_stopped: false
 
     # setting whether to turn hazard lamp on for each situation
     turning_hazard_on:

diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml
@@ -33,6 +33,7 @@ units:
       - { type: link, link: /sensing/autonomous_available }
       - { type: link, link: /system/autonomous_available }
       - { type: link, link: /vehicle/autonomous_available }
+      - { type: link, link: /autoware/fatal_error/autonomous_available }
 
   - path: /autoware/modes/pull_over
     type: and
@@ -47,6 +48,7 @@ units:
       - { type: link, link: /sensing/pull_over_available }
       - { type: link, link: /system/pull_over_available }
       - { type: link, link: /vehicle/pull_over_available }
+      - { type: link, link: /autoware/fatal_error/pull_over_available }
 
   - path: /autoware/modes/comfortable_stop
     type: and
@@ -61,6 +63,37 @@ units:
       - { type: link, link: /sensing/comfortable_stop_available }
       - { type: link, link: /system/comfortable_stop_available }
       - { type: link, link: /vehicle/comfortable_stop_available }
+      - { type: link, link: /autoware/fatal_error/comfortable_stop_available }
+
+  - path: /autoware/fatal_error/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /hardware/fatal_error/autonomous_available }
+      - { type: link, link: /planning/fatal_error/autonomous_available }
+      - { type: link, link: /control/fatal_error/autonomous_available }
+      - { type: link, link: /vehicle/fatal_error/autonomous_available }
+      - { type: link, link: /system/fatal_error/autonomous_available }
+      - { type: link, link: /others/fatal_error/autonomous_available }
+
+  - path: /autoware/fatal_error/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /hardware/fatal_error/pull_over_available }
+      - { type: link, link: /planning/fatal_error/pull_over_available }
+      - { type: link, link: /control/fatal_error/pull_over_available }
+      - { type: link, link: /vehicle/fatal_error/pull_over_available }
+      - { type: link, link: /system/fatal_error/pull_over_available }
+      - { type: link, link: /others/fatal_error/pull_over_available }
+
+  - path: /autoware/fatal_error/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /hardware/fatal_error/comfortable_stop_available }
+      - { type: link, link: /planning/fatal_error/comfortable_stop_available }
+      - { type: link, link: /control/fatal_error/comfortable_stop_available }
+      - { type: link, link: /vehicle/fatal_error/comfortable_stop_available }
+      - { type: link, link: /system/fatal_error/comfortable_stop_available }
+      - { type: link, link: /others/fatal_error/comfortable_stop_available }
 
   - path: /autoware/modes/emergency_stop
     type: ok
diff --git a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml
@@ -17,6 +17,24 @@ units:
     list:
       - { type: link, link: /control/emergency_stop }
 
+  - path: /control/fatal_error/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /control/fatal_error/emergency_stop }
+      - { type: link, link: /control/fatal_error/pull_over }
+      - { type: link, link: /control/fatal_error/comfortable_stop }
+
+  - path: /control/fatal_error/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /control/fatal_error/emergency_stop }
+      - { type: link, link: /control/fatal_error/comfortable_stop }
+
+  - path: /control/fatal_error/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /control/fatal_error/emergency_stop }
+
   # *******************************************************************************
   # NOTE: Please modify this section according to your environment and requirements.
   # *******************************************************************************
@@ -30,12 +48,21 @@ units:
       # - { type: link, link: /control/010-max_distance_deviation-error }
       - { type: link, link: /control/011-slip_detection }
 
+  - path: /control/fatal_error/emergency_stop
+    type: and
+
   - path: /control/comfortable_stop
     type: and
 
+  - path: /control/fatal_error/comfortable_stop
+    type: and
+
   - path: /control/pull_over
     type: and
 
+  - path: /control/fatal_error/pull_over
+    type: and
+
   - path: /control/none
     type: and
     list:

diff --git a/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml b/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml
@@ -17,6 +17,24 @@ units:
     list:
       - { type: link, link: /hardware/emergency_stop }
 
+  - path: /hardware/fatal_error/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /hardware/fatal_error/emergency_stop }
+      - { type: link, link: /hardware/fatal_error/pull_over }
+      - { type: link, link: /hardware/fatal_error/comfortable_stop }
+
+  - path: /hardware/fatal_error/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /hardware/fatal_error/emergency_stop }
+      - { type: link, link: /hardware/fatal_error/comfortable_stop }
+
+  - path: /hardware/fatal_error/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /hardware/fatal_error/emergency_stop }
+
   # *******************************************************************************
   # NOTE: Please modify this section according to your environment and requirements.
   # *******************************************************************************
@@ -40,14 +58,23 @@ units:
       # - { type: link, link: /hardware/gpu/004-throttling-error }
       # - { type: link, link: /hardware/gpu/005-frequency-error }
 
+  - path: /hardware/fatal_error/emergency_stop
+    type: and
+
   - path: /hardware/pull_over
     type: and
     list:
       - { type: link, link: /hardware/hdd/002-usage-error }
 
+  - path: /hardware/fatal_error/pull_over
+    type: and
+
   - path: /hardware/comfortable_stop
     type: and
 
+  - path: /hardware/fatal_error/comfortable_stop
+    type: and
+
   - path: /hardware/none
     type: and
     list:

diff --git a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml
@@ -17,6 +17,24 @@ units:
     list:
       - { type: link, link: /others/emergency_stop }
 
+  - path: /others/fatal_error/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /others/fatal_error/emergency_stop }
+      - { type: link, link: /others/fatal_error/pull_over }
+      - { type: link, link: /others/fatal_error/comfortable_stop }
+
+  - path: /others/fatal_error/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /others/fatal_error/emergency_stop }
+      - { type: link, link: /others/fatal_error/comfortable_stop }
+
+  - path: /others/fatal_error/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /others/fatal_error/emergency_stop }
+
   # *******************************************************************************
   # NOTE: Please modify this section according to your environment and requirements.
   # *******************************************************************************
@@ -28,16 +46,25 @@ units:
       # - { type: link, link: /others/005-visibility_validation-error }
       # - { type: link, link: /others/012-vehicle_stuck_checker }
 
+  - path: /others/fatal_error/emergency_stop
+    type: and
+
   - path: /others/comfortable_stop
     type: and
     # list:
     # - { type: link, link: /others/010-emergency_vehicle-error }
 
+  - path: /others/fatal_error/comfortable_stop
+    type: and
+
   - path: /others/pull_over
     type: and
     list:
       - { type: link, link: /others/011-daytime_monitor }
 
+  - path: /others/fatal_error/pull_over
+    type: and
+
   - path: /others/none
     type: and
 

diff --git a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml
@@ -17,6 +17,24 @@ units:
     list:
       - { type: link, link: /planning/emergency_stop }
 
+  - path: /planning/fatal_error/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /planning/fatal_error/emergency_stop }
+      - { type: link, link: /planning/fatal_error/pull_over }
+      - { type: link, link: /planning/fatal_error/comfortable_stop }
+
+  - path: /planning/fatal_error/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /planning/fatal_error/emergency_stop }
+      - { type: link, link: /planning/fatal_error/comfortable_stop }
+
+  - path: /planning/fatal_error/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /planning/fatal_error/emergency_stop }
+
   # *******************************************************************************
   # NOTE: Please modify this section according to your environment and requirements.
   # *******************************************************************************
@@ -37,12 +55,21 @@ units:
       - { type: link, link: /planning/012-trajectory_velocity_deviation_validation-error }
       # - { type: link, link: /planning/013-collision_checker-error }
 
+  - path: /planning/fatal_error/emergency_stop
+    type: and
+
   - path: /planning/comfortable_stop
     type: and
 
+  - path: /planning/fatal_error/comfortable_stop
+    type: and
+
   - path: /planning/pull_over
     type: and
 
+  - path: /planning/fatal_error/pull_over
+    type: and
+
   - path: /planning/none
     type: and
 

diff --git a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml
@@ -17,6 +17,24 @@ units:
     list:
       - { type: link, link: /system/emergency_stop }
 
+  - path: /system/fatal_error/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /system/fatal_error/emergency_stop }
+      - { type: link, link: /system/fatal_error/pull_over }
+      - { type: link, link: /system/fatal_error/comfortable_stop }
+
+  - path: /system/fatal_error/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /system/fatal_error/emergency_stop }
+      - { type: link, link: /system/fatal_error/comfortable_stop }
+
+  - path: /system/fatal_error/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /system/fatal_error/emergency_stop }
+
   # *******************************************************************************
   # NOTE: Please modify this section according to your environment and requirements.
   # *******************************************************************************
@@ -26,14 +44,23 @@ units:
       - { type: link, link: /system/001-topic_status-error }
       - { type: link, link: /system/002-emergency_stop_operation-error }
 
+  - path: /system/fatal_error/emergency_stop
+    type: and
+
   - path: /system/comfortable_stop
     type: and
 
+  - path: /system/fatal_error/comfortable_stop
+    type: and
+
   - path: /system/pull_over
     type: and
     list:
       - { type: link, link: /system/005-fms_connection-error }
 
+  - path: /system/fatal_error/pull_over
+    type: and
+
   - path: /system/none
     type: and
     list:

diff --git a/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml b/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml
@@ -17,6 +17,24 @@ units:
     list:
       - { type: link, link: /vehicle/emergency_stop }
 
+  - path: /vehicle/fatal_error/autonomous_available
+    type: and
+    list:
+      - { type: link, link: /vehicle/fatal_error/emergency_stop }
+      - { type: link, link: /vehicle/fatal_error/pull_over }
+      - { type: link, link: /vehicle/fatal_error/comfortable_stop }
+
+  - path: /vehicle/fatal_error/pull_over_available
+    type: and
+    list:
+      - { type: link, link: /vehicle/fatal_error/emergency_stop }
+      - { type: link, link: /vehicle/fatal_error/comfortable_stop }
+
+  - path: /vehicle/fatal_error/comfortable_stop_available
+    type: and
+    list:
+      - { type: link, link: /vehicle/fatal_error/emergency_stop }
+
   # *******************************************************************************
   # NOTE: Please modify this section according to your environment and requirements.
   # *******************************************************************************
@@ -28,15 +46,25 @@ units:
       - { type: link, link: /vehicle/005-vehicle_heartbeat-error }
       - { type: link, link: /vehicle/006-vehicle_errors-error }
 
+  - path: /vehicle/fatal_error/emergency_stop
+    type: and
+
   - path: /vehicle/comfortable_stop
     type: and
     list:
       - { type: link, link: /vehicle/006-vehicle_errors }
+
+  - path: /vehicle/fatal_error/comfortable_stop
+    type: and
+    list:
       - { type: link, link: /vehicle/007-vehicle-emergency-button }
 
   - path: /vehicle/pull_over
     type: and
 
+  - path: /vehicle/fatal_error/pull_over
+    type: and
+
   - path: /vehicle/none
     type: and
 

diff --git a/autoware_launch/launch/components/tier4_system_component.launch.xml b/autoware_launch/launch/components/tier4_system_component.launch.xml
@@ -34,6 +34,8 @@
     <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)"/>
     <arg name="diagnostic_graph_aggregator_planning_simulator_graph_path" value="$(find-pkg-share autoware_launch)/config/system/diagnostic_graph_aggregator/autoware.yaml"/>
+
+    <arg name="launch_system_recover_operator" value="true"/>
   </include>
 
   <!-- For logging of diagnostics_graph error -->