From 342f3819d6f1b31f3eba74b25e9844e9440bf4d8 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Fri, 17 May 2024 19:38:34 +0900 Subject: [PATCH] feat: use new diagsnotics for camera topic rate (#240) * test * fix: update dummy_diag --- .../sensor_kit.param.yaml | 16 +- .../launch/topic_state_monitor.launch.py | 161 ------------------ 2 files changed, 8 insertions(+), 169 deletions(-) diff --git a/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index 58fdc890..7caeef6f 100644 --- a/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -61,14 +61,14 @@ "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": default ## /sensing/camera/001-connection - "topic_state_monitor_camera0: camera0_topic_status": default - "topic_state_monitor_camera1: camera1_topic_status": default - "topic_state_monitor_camera2: camera2_topic_status": default - "topic_state_monitor_camera3: camera3_topic_status": default - "topic_state_monitor_camera4: camera4_topic_status": default - "topic_state_monitor_camera5: camera5_topic_status": default - "topic_state_monitor_camera6: camera6_topic_status": default - "topic_state_monitor_camera7: camera7_topic_status": default + "v4l2_camera_camera0: capture_status": default + "v4l2_camera_camera1: capture_status": default + "v4l2_camera_camera2: capture_status": default + "v4l2_camera_camera3: capture_status": default + "v4l2_camera_camera4: capture_status": default + "v4l2_camera_camera5: capture_status": default + "v4l2_camera_camera6: capture_status": default + "v4l2_camera_camera7: capture_status": default ## /sensing/radar/001-connection "topic_state_monitor_radar_front_center: radar_front_center_topic_status": default diff --git a/aip_x2_launch/launch/topic_state_monitor.launch.py b/aip_x2_launch/launch/topic_state_monitor.launch.py index 33752590..0eafa9e0 100644 --- a/aip_x2_launch/launch/topic_state_monitor.launch.py +++ b/aip_x2_launch/launch/topic_state_monitor.launch.py @@ -173,159 +173,6 @@ def generate_launch_description(): extra_arguments=[{"use_intra_process_comms": True}], ) - # Camera topic monitors - camera0_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera0", - parameters=[ - { - "topic": "/sensing/camera/camera0/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera0_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 10, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera1_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera1", - parameters=[ - { - "topic": "/sensing/camera/camera1/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera1_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 10, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera2_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera2", - parameters=[ - { - "topic": "/sensing/camera/camera2/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera2_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 10, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera3_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera3", - parameters=[ - { - "topic": "/sensing/camera/camera3/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera3_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 10, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera4_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera4", - parameters=[ - { - "topic": "/sensing/camera/camera4/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera4_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 10, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera5_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera5", - parameters=[ - { - "topic": "/sensing/camera/camera5/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera5_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 10, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera6_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera6", - parameters=[ - { - "topic": "/sensing/camera/camera6/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera6_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 10, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera7_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera7", - parameters=[ - { - "topic": "/sensing/camera/camera7/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera7_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 10, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - # ComposableNodeContainer to run all ComposableNodes container = ComposableNodeContainer( name="topic_state_monitor_container", @@ -341,14 +188,6 @@ def generate_launch_description(): radar_rear_center_monitor, radar_rear_left_monitor, radar_rear_right_monitor, - camera0_topic_monitor, - camera1_topic_monitor, - camera2_topic_monitor, - camera3_topic_monitor, - camera4_topic_monitor, - camera5_topic_monitor, - camera6_topic_monitor, - camera7_topic_monitor, ], output="screen", )