feat(aip_x2_launch): add topic state monitor for sensing diags (#288)

* 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 <[email protected]> * fix comments Signed-off-by: Tomohito Ando <[email protected]> * replace launch.xml with launch.py Signed-off-by: Tomohito Ando <[email protected]> * ci(pre-commit): autofix * add copyright Signed-off-by: Tomohito Ando <[email protected]> --------- Signed-off-by: Tomohito Ando <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * remove topic_state_monitor for camera * feat: add imu_monitor and dummy diag (#212) --------- Signed-off-by: Tomohito Ando <[email protected]> Co-authored-by: Tomohito ANDO <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Hiroki OTA <[email protected]>
tier4 · Aug 30, 2024 · cfed45f · cfed45f
1 parent 667532f
commit cfed45f
Show file tree

Hide file tree

Showing 4 changed files with 206 additions and 0 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
@@ -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
@@ -36,6 +36,10 @@
       <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
       <arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
     </include>
+
+    <include file="$(find-pkg-share imu_monitor)/launch/imu_monitor.launch.xml">
+      <arg name="config_file" value="$(find-pkg-share imu_monitor)/config/imu_monitor.param.yaml"/>
+    </include>
   </group>
 
 </launch>
diff --git a/aip_x2_launch/launch/sensing.launch.xml b/aip_x2_launch/launch/sensing.launch.xml
@@ -35,6 +35,10 @@
       <arg name="output_twist_with_covariance" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
       <arg name="config_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/vehicle_velocity_converter.param.yaml" />
     </include>
+
+    <!-- Topic state monitor for each sensor -->
+    <include file="$(find-pkg-share aip_x2_launch)/launch/topic_state_monitor.launch.py"/>
+
   </group>
 
 </launch>
diff --git a/aip_x2_launch/launch/topic_state_monitor.launch.py b/aip_x2_launch/launch/topic_state_monitor.launch.py
@@ -0,0 +1,195 @@
+# 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}],
+    )
+
+    # 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,
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([container])