From 385fb8f4259af8da797c36a2f450da1500652397 Mon Sep 17 00:00:00 2001
From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com>
Date: Mon, 17 Apr 2023 16:47:20 +0900
Subject: [PATCH] chore: update diagnostics config (#124)

* chore: add imu aggregator

* chore: update diag settings
---
 .../sensor_kit.param.yaml                     | 19 +++++++
 .../dummy_diag_publisher/sensing.launch.xml   |  8 +++
 aip_x2_launch/launch/sensing.launch.xml       |  5 +-
 .../launch/topic_state_monitor.launch.py      | 52 +++++++++++++++++++
 4 files changed, 83 insertions(+), 1 deletion(-)
 create mode 100644 aip_x2_launch/launch/topic_state_monitor.launch.py

diff --git a/aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml
index 9f2c44a2..25308a1b 100644
--- a/aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml
+++ b/aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml
@@ -49,3 +49,22 @@
                       path: gnss
                       contains: [": gnss"]
                       timeout: 1.0
+                node_alive_monitoring:
+                  type: diagnostic_aggregator/AnalyzerGroup
+                  path: node_alive_monitoring
+                  analyzers:
+                    topic_status:
+                      type: diagnostic_aggregator/GenericAnalyzer
+                      path: topic_status
+                      contains: [": septentrio_topic_status"]
+                      timeout: 1.0
+
+        imu:
+          type: diagnostic_aggregator/AnalyzerGroup
+          path: imu
+          analyzers:
+            yaw_rate:
+              type: diagnostic_aggregator/GenericAnalyzer
+              path: yaw_rate
+              contains: [": yaw_rate_status"]
+              timeout: 1.0
diff --git a/aip_x2_launch/launch/dummy_diag_publisher/sensing.launch.xml b/aip_x2_launch/launch/dummy_diag_publisher/sensing.launch.xml
index df568850..b92ed02e 100644
--- a/aip_x2_launch/launch/dummy_diag_publisher/sensing.launch.xml
+++ b/aip_x2_launch/launch/dummy_diag_publisher/sensing.launch.xml
@@ -7,6 +7,10 @@
     <arg name="diag_name" value="sensing_topic_status"/>
   </include>
 
+  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
+    <arg name="diag_name" value="septentrio_topic_status"/>
+  </include>
+
   <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
     <arg name="diag_name" value="concat_status"/>
   </include>
@@ -15,4 +19,8 @@
     <arg name="diag_name" value="temperature_status"/>
   </include>
 
+  <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher_node.launch.xml">
+    <arg name="diag_name" value="emergency_vehicle"/>
+  </include>
+
 </launch>
diff --git a/aip_x2_launch/launch/sensing.launch.xml b/aip_x2_launch/launch/sensing.launch.xml
index 04833a2d..401656e7 100644
--- a/aip_x2_launch/launch/sensing.launch.xml
+++ b/aip_x2_launch/launch/sensing.launch.xml
@@ -36,6 +36,9 @@
       <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/$(env VEHICLE_ID default)/aip_x2/vehicle_velocity_converter.param.yaml" />
     </include>
-  </group>
 
+    <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
new file mode 100644
index 00000000..a1fd1c63
--- /dev/null
+++ b/aip_x2_launch/launch/topic_state_monitor.launch.py
@@ -0,0 +1,52 @@
+# Copyright 2021 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.launch_description import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    topic_state_monitor_gnss = ComposableNode(
+        package="topic_state_monitor",
+        plugin="topic_state_monitor::TopicStateMonitorNode",
+        name="topic_state_monitor_gnss",
+        parameters=[
+            {
+                "topic": "/sensing/gnss/pose",
+                "topic_type": "geometry_msgs/msg/PoseStamped",
+                "best_effort": False,
+                "diag_name": "septentrio_topic_status",
+                "warn_rate": 2.5,
+                "error_rate": 0.5,
+                "timeout": 1.0,
+                "window_size": 10,
+            },
+        ],
+        extra_arguments=[{"use_intra_process_comms": True}],
+    )
+
+    # set container to run all required components in the same process
+    container = ComposableNodeContainer(
+        name="topic_state_monitor_container",
+        namespace="topic_state_monitor",
+        package="rclcpp_components",
+        executable="component_container",
+        composable_node_descriptions=[
+            topic_state_monitor_gnss,
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([container])