feat(rviz): use multi_data_monitor to display useful status

autoware_launch/CMakeLists.txt

@@ -15,3 +15,8 @@ ament_auto_package(
   launch
   rviz
 )
+
+install(PROGRAMS
+  scripts/hazard_convert
+  DESTINATION lib/${PROJECT_NAME}
+)

autoware_launch/config/multi_data_monitor/status_panel.yaml

@@ -0,0 +1,205 @@
+# yamllint disable rule:quoted-strings
+version: 2.0
+
+stylesheets:
+  - { path: "package://multi_data_monitor/css/plugins/simple.css", class: Simple }
+  - { path: "package://multi_data_monitor/css/plugins/titled.css", class: Titled }
+  - { path: "package://multi_data_monitor/css/colors.css" }
+
+widgets:
+  - class: Matrix
+    items:
+      - indicator-status
+      - gear-status
+      - emergency-status
+      # - current-status
+      - target-status
+      - localization-status
+      - planning-status
+
+  - class: Matrix
+    label: indicator-status
+    cols: 3
+    items:
+      - class: Simple
+        const: LEFT
+        input: { class: subscription, topic: /vehicle/status/turn_indicators_status, field: report }
+        rules: { class: SetIf, type: uint, eq: 2, attrs: { back-color: yellow } }
+      - class: Simple
+        const: HAZARD
+        input: { class: subscription, topic: /vehicle/status/hazard_lights_status, field: report }
+        rules: { class: SetIf, type: uint, eq: 2, attrs: { back-color: red } }
+      - class: Simple
+        const: RIGHT
+        input: { class: subscription, topic: /vehicle/status/turn_indicators_status, field: report }
+        rules: { class: SetIf, type: uint, eq: 3, attrs: { back-color: yellow } }
+
+  - class: Matrix
+    label: gear-status
+    cols: 5
+    items:
+      - class: Simple
+        const: P
+        input: { class: subscription, topic: /vehicle/status/gear_status, field: report }
+        rules: { class: SetIf, type: uint, eq: 22, attrs: { back-color: lime } }
+      - class: Simple
+        const: R
+        input: { class: subscription, topic: /vehicle/status/gear_status, field: report }
+        rules: { class: SetIf, type: uint, eq: 20, attrs: { back-color: lime } }
+      - class: Simple
+        const: N
+        input: { class: subscription, topic: /vehicle/status/gear_status, field: report }
+        rules: { class: SetIf, type: uint, eq: 1, attrs: { back-color: lime } }
+      - class: Simple
+        const: D
+        input: { class: subscription, topic: /vehicle/status/gear_status, field: report }
+        rules: { class: SetIf, type: uint, eq: 2, attrs: { back-color: lime } }
+      - class: Simple
+        const: L
+        input: { class: subscription, topic: /vehicle/status/gear_status, field: report }
+        rules: { class: SetIf, type: uint, eq: 13, attrs: { back-color: lime } }
+
+  - class: Matrix
+    label: emergency-status
+    cols: 2
+    items:
+      - class: Simple
+        const: Emergency
+        input: { class: subscription, topic: /emergency_status, field: data }
+        rules: { class: SetIf, type: text, eq: true, attrs: { back-color: red } }
+      - class: Simple
+        const: Emergency Holding
+        input: { class: subscription, topic: /emergency_hold_status, field: data }
+        rules: { class: SetIf, type: text, eq: true, attrs: { back-color: red } }
+
+  # - class: Matrix
+  #   label: current-status
+  #   cols: 3
+  #   items:
+  #     - class: Titled
+  #       title: current vel [km/h]
+  #       rules: mps-to-kph-precision-2
+  #       input:
+  #         {
+  #           class: subscription,
+  #           topic: /vehicle/status/velocity_status,
+  #           field: longitudinal_velocity,
+  #         }
+  #     - class: Titled
+  #       title: current steering [deg]
+  #       rules: rad-to-deg-precision-2
+  #       input:
+  #         {
+  #           class: subscription,
+  #           topic: /vehicle/status/steering_status,
+  #           field: steering_tire_angle,
+  #         }
+  #     - class: Simple
+  #       const: ""
+
+  - class: Matrix
+    label: target-status
+    cols: 3
+    items:
+      - class: Titled
+        title: target vel [km/h]
+        rules: mps-to-kph-precision-2
+        input:
+          { class: subscription, topic: /control/command/control_cmd, field: longitudinal.speed }
+      - class: Titled
+        title: target steering [deg]
+        rules: rad-to-deg-precision-2
+        input:
+          {
+            class: subscription,
+            topic: /control/command/control_cmd,
+            field: lateral.steering_tire_angle,
+          }
+      - class: Titled
+        title: target acc [m/s^2]
+        rules: precision-2
+        input:
+          {
+            class: subscription,
+            topic: /control/command/control_cmd,
+            field: longitudinal.acceleration,
+          }
+
+  - class: Matrix
+    label: localization-status
+    cols: 2
+    items:
+      - class: Titled
+        title: NVTL [-]
+        rules:
+          - { class: SetIf, type: real, le: 1.0, attrs: { back-color: red } }
+          - { class: Precision, digits: 2 }
+        input:
+          {
+            class: subscription,
+            topic: /localization/pose_estimator/nearest_voxel_transformation_likelihood,
+            field: data,
+          }
+      - class: Titled
+        title: pose estimation time [ms]
+        rules: precision-2
+        input: { class: subscription, topic: /localization/pose_estimator/exe_time_ms, field: data }
+
+  - class: Matrix
+    label: planning-status
+    cols: 3
+    items:
+      - class: Titled
+        title: stop line distance [m]
+        rules: precision-2
+        input:
+          {
+            class: subscription,
+            topic: /planning/scenario_planning/motion_velocity_smoother/distance_to_stopline,
+            field: data,
+          }
+      - class: Titled
+        title: planning max velocity [km/h]
+        rules: mps-to-kph-precision-2
+        input:
+          {
+            class: subscription,
+            topic: /planning/scenario_planning/current_max_velocity,
+            field: max_velocity,
+          }
+      - class: Titled
+        title: traffic light color
+        input:
+          {
+            class: subscription,
+            topic: /planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal,
+            field: "",
+          }
+        rules:
+          - class: Access
+            fails: ERROR-VALUE
+            field: [elements, 0, color]
+          - class: SetFirstIf
+            type: text
+            list:
+              - { eq: 1, value: Red, attrs: { back-color: red } }
+              - { eq: 2, value: Yellow, attrs: { back-color: yellow } }
+              - { eq: 3, value: Green, attrs: { back-color: lime } }
+              - { eq: 0, value: Unknown }
+              - { eq: ERROR-VALUE, value: None }
+
+filters:
+  - class: function
+    label: mps-to-kph-precision-2
+    rules:
+      - { class: Units, mode: mps_to_kph }
+      - { class: Precision, digits: 2 }
+  - class: function
+    label: precision-2
+    rules:
+      - { class: Precision, digits: 2 }
+  - class: function
+    label: rad-to-deg-precision-2
+    rules:
+      - { class: Units, mode: rad_to_deg }
+      - { class: Precision, digits: 2 }

autoware_launch/launch/autoware.launch.xml

@@ -123,4 +123,9 @@
       respawn="$(var rviz_respawn)"
     />
   </group>
+
+  <!-- hazard_status convert modules for multi_data_monitor -->
+  <group>
+    <node pkg="autoware_launch" exec="hazard_convert" name="hazard_convert" output="screen"/>
+  </group>
 </launch>

autoware_launch/rviz/autoware.rviz

@@ -20,6 +20,9 @@ Panels:
     Name: AutowareDateTimePanel
   - Class: rviz_plugins::AutowareStatePanel
     Name: AutowareStatePanel
+  - Class: multi_data_monitor::MultiDataMonitor
+    Name: MultiDataMonitor
+    Path: package://autoware_launch/config/multi_data_monitor/status_panel.yaml
 Visualization Manager:
   Class: ""
   Displays:
@@ -3583,7 +3586,7 @@ Window Geometry:
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 000000ff00000000fd0000000400000000000002d6000005c3fc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb00000020004d0075006c007400690044006100740061004d006f006e00690074006f0072010000003d000001c5000000ee00fffffffb00000024004100750074006f00770061007200650053007400610074006500500061006e0065006c0100000208000001a70000017200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d00610067006501000003b5000001380000002800fffffffb0000002a004100750074006f0077006100720065004400610074006500540069006d006500500061006e0065006c01000004f30000005b0000004100fffffffb000000240050006f0069006e00740063006c006f00750064004f006e00430061006d0065007200610100000554000000ac0000002800fffffffb00000020004d0075006c007400690044006100740061004d006f006e00690074006f007201000005bf000000410000000000000000000000010000015f000005c3fc0200000004fb000000100044006900730070006c006100790073010000003d000004f0000000c900fffffffc00000533000000cd000000c10100001cfa000000000100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000009d00fffffffb0000000a00560069006500770073010000099e0000015f0000010000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7a0000005afc0100000002fb0000000800540069006d0065010000000000000e7a0000000000000000fb0000000800540069006d00650100000000000004500000000000000000000006bc000005c300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RecognitionResultOnImage:
     collapsed: false
   Selection:

autoware_launch/scripts/hazard_convert

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+
+from autoware_auto_system_msgs.msg import HazardStatusStamped
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Bool
+
+
+class EmergencyPublisher(Node):
+    def __init__(self):
+        super().__init__("hazard_convert")
+        profile = rclpy.qos.QoSProfile(depth=1)
+        self.subscriber = self.create_subscription(
+            HazardStatusStamped,
+            "/system/emergency/hazard_status",
+            self.hazard_status_callback,
+            profile,
+        )
+        self.em_publisher = self.create_publisher(Bool, "/emergency_status", 10)
+        self.em_hold_publisher = self.create_publisher(Bool, "/emergency_hold_status", 10)
+
+    def hazard_status_callback(self, msg):
+        em_msg = Bool()
+        em_hold_msg = Bool()
+        em_msg.data = msg.status.emergency
+        em_hold_msg.data = msg.status.emergency_holding
+        self.em_publisher.publish(em_msg)
+        self.em_hold_publisher.publish(em_hold_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = EmergencyPublisher()
+
+    while rclpy.ok():
+        rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()