From 1a9b241141a390bd2d82dd1abd29ab039f1a562b Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 26 Mar 2024 16:54:01 +0900 Subject: [PATCH 1/3] feat(aip_x2_launch): add topic state monitor for camera, radar, gnss and imu (#227) * feat: add topic state monitor for sensors Signed-off-by: Tomohito Ando * fix comments Signed-off-by: Tomohito Ando * replace launch.xml with launch.py Signed-off-by: Tomohito Ando * ci(pre-commit): autofix * add copyright Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x2_launch/launch/sensing.launch.xml | 4 + .../launch/topic_state_monitor.launch.py | 356 ++++++++++++++++++ 2 files changed, 360 insertions(+) create mode 100644 aip_x2_launch/launch/topic_state_monitor.launch.py diff --git a/aip_x2_launch/launch/sensing.launch.xml b/aip_x2_launch/launch/sensing.launch.xml index 274f6334..2a527f54 100644 --- a/aip_x2_launch/launch/sensing.launch.xml +++ b/aip_x2_launch/launch/sensing.launch.xml @@ -35,6 +35,10 @@ + + + + diff --git a/aip_x2_launch/launch/topic_state_monitor.launch.py b/aip_x2_launch/launch/topic_state_monitor.launch.py new file mode 100644 index 00000000..33752590 --- /dev/null +++ b/aip_x2_launch/launch/topic_state_monitor.launch.py @@ -0,0 +1,356 @@ +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # GNSS topic monitor + gnss_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_gnss_pose", + parameters=[ + { + "topic": "/sensing/gnss/pose", + "topic_type": "geometry_msgs/msg/PoseStamped", + "best_effort": True, + "diag_name": "gnss_topic_status", + "warn_rate": 2.5, + "error_rate": 0.5, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # IMU topic monitor + imu_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_imu_data", + parameters=[ + { + "topic": "/sensing/imu/imu_data", + "topic_type": "sensor_msgs/msg/Imu", + "best_effort": True, + "diag_name": "imu_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # Radar topic monitors + radar_front_center_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_center", + parameters=[ + { + "topic": "/sensing/radar/front_center/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_center_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_front_left_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_left", + parameters=[ + { + "topic": "/sensing/radar/front_left/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_left_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_front_right_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_right", + parameters=[ + { + "topic": "/sensing/radar/front_right/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_right_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_center_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_center", + parameters=[ + { + "topic": "/sensing/radar/rear_center/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_center_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_left_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_left", + parameters=[ + { + "topic": "/sensing/radar/rear_left/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_left_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_right_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_right", + parameters=[ + { + "topic": "/sensing/radar/rear_right/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_right_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + 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", + namespace="topic_state_monitor", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + gnss_topic_monitor, + imu_topic_monitor, + radar_front_center_monitor, + radar_front_left_monitor, + radar_front_right_monitor, + 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", + ) + + return LaunchDescription([container]) From cc534b2cdc270fb7da3e6a7be0b14e68cf5671bf Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Tue, 27 Aug 2024 12:07:03 +0900 Subject: [PATCH 2/3] remove topic_state_monitor for camera --- .../launch/topic_state_monitor.launch.py | 161 ------------------ 1 file changed, 161 deletions(-) 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", ) From c5348b51aed0b20bd73212c2a243d04d1c58dc26 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 16 Feb 2024 15:24:45 +0900 Subject: [PATCH 3/3] feat: add imu_monitor and dummy diag (#212) --- .../config/dummy_diag_publisher/sensor_kit.param.yaml | 3 +++ aip_x2_launch/launch/imu.launch.xml | 4 ++++ 2 files changed, 7 insertions(+) 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 888cd52c..1c0ec798 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 @@ -15,6 +15,9 @@ # gnss gnss: default + # imu + yaw_rate_status: default + # lidar pandar_connection: default pandar_temperature: default diff --git a/aip_x2_launch/launch/imu.launch.xml b/aip_x2_launch/launch/imu.launch.xml index e3f1a056..a71854ea 100644 --- a/aip_x2_launch/launch/imu.launch.xml +++ b/aip_x2_launch/launch/imu.launch.xml @@ -36,6 +36,10 @@ + + + +