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 9af30b4f..e94d57dd 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 @@ -71,12 +71,12 @@ "v4l2_camera_camera7: capture_status": default ## /sensing/radar/001-connection - "topic_state_monitor_radar_front_center: radar_front_center_topic_status": default - "topic_state_monitor_radar_front_left: radar_front_left_topic_status": default - "topic_state_monitor_radar_front_right: radar_front_right_topic_status": default - "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status": default - "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status": default - "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status": default + "radar_monitor: /sensing/radar/front_center: radar_status": default + "radar_monitor: /sensing/radar/front_left: radar_status": default + "radar_monitor: /sensing/radar/front_right: radar_status": default + "radar_monitor: /sensing/radar/rear_center: radar_status": default + "radar_monitor: /sensing/radar/rear_left: radar_status": default + "radar_monitor: /sensing/radar/rear_right: radar_status": default ## /others/002-blockage_validation "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation": default diff --git a/aip_x2_launch/launch/radar.launch.xml b/aip_x2_launch/launch/radar.launch.xml index 73e36ecf..539a555e 100644 --- a/aip_x2_launch/launch/radar.launch.xml +++ b/aip_x2_launch/launch/radar.launch.xml @@ -7,7 +7,7 @@ - + @@ -16,7 +16,7 @@ - + @@ -25,7 +25,7 @@ - + @@ -34,7 +34,7 @@ - + @@ -43,7 +43,7 @@ - + @@ -52,20 +52,12 @@ - + - - - - - - - - diff --git a/aip_x2_launch/launch/topic_state_monitor.launch.py b/aip_x2_launch/launch/topic_state_monitor.launch.py index 0eafa9e0..3281f8c2 100644 --- a/aip_x2_launch/launch/topic_state_monitor.launch.py +++ b/aip_x2_launch/launch/topic_state_monitor.launch.py @@ -58,121 +58,6 @@ def generate_launch_description(): 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}], - ) - # ComposableNodeContainer to run all ComposableNodes container = ComposableNodeContainer( name="topic_state_monitor_container", @@ -182,12 +67,6 @@ def generate_launch_description(): 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, ], output="screen", )