forked from autowarefoundation/autoware_launch
-
Notifications
You must be signed in to change notification settings - Fork 4
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(rviz): use multi data monitor to display useful status
- Loading branch information
Showing
5 changed files
with
263 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -15,3 +15,8 @@ ament_auto_package( | |
| launch | ||
| rviz | ||
| ) | ||
|
|
||
| install(PROGRAMS | ||
| scripts/hazard_convert | ||
| DESTINATION lib/${PROJECT_NAME} | ||
| ) | ||
205 changes: 205 additions & 0 deletions
205
autoware_launch/config/multi_data_monitor/status_panel.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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: transform probability value [-] | ||
| 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 } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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() |