From 1a9b241141a390bd2d82dd1abd29ab039f1a562b Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 26 Mar 2024 16:54:01 +0900 Subject: [PATCH] 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])