feat(topic_state_monitor): support transform topic check (tier4#1586)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

system/topic_state_monitor/Readme.md

@@ -35,14 +35,16 @@ The types of topic status and corresponding diagnostic status are following.
 
 ### Node Parameters
 
-| Name              | Type   | Default Value | Description                                                 |
-| ----------------- | ------ | ------------- | ----------------------------------------------------------- |
-| `topic`           | string | -             | Name of target topic                                        |
-| `topic_type`      | string | -             | Type of target topic                                        |
-| `transient_local` | bool   | false         | QoS policy of topic subscription (Transient Local/Volatile) |
-| `best_effort`     | bool   | false         | QoS policy of topic subscription (Best Effort/Reliable)     |
-| `diag_name`       | string | -             | Name used for the diagnostics to publish                    |
-| `update_rate`     | double | 10.0          | Timer callback period [Hz]                                  |
+| Name              | Type   | Default Value | Description                                                   |
+| ----------------- | ------ | ------------- | ------------------------------------------------------------- |
+| `topic`           | string | -             | Name of target topic                                          |
+| `topic_type`      | string | -             | Type of target topic (used if the topic is not transform)     |
+| `frame_id`        | string | -             | Frame ID of transform parent (used if the topic is transform) |
+| `child_frame_id`  | string | -             | Frame ID of transform child (used if the topic is transform)  |
+| `transient_local` | bool   | false         | QoS policy of topic subscription (Transient Local/Volatile)   |
+| `best_effort`     | bool   | false         | QoS policy of topic subscription (Best Effort/Reliable)       |
+| `diag_name`       | string | -             | Name used for the diagnostics to publish                      |
+| `update_rate`     | double | 10.0          | Timer callback period [Hz]                                    |
 
 ### Core Parameters
 

system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp

@@ -20,6 +20,8 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <tf2_msgs/msg/tf_message.hpp>
+
 #include <deque>
 #include <map>
 #include <memory>
@@ -34,8 +36,11 @@ struct NodeParam
   std::string diag_name;
   std::string topic;
   std::string topic_type;
+  std::string frame_id;
+  std::string child_frame_id;
   bool transient_local;
   bool best_effort;
+  bool is_transform;
 };
 
 class TopicStateMonitorNode : public rclcpp::Node
@@ -58,6 +63,7 @@ class TopicStateMonitorNode : public rclcpp::Node
 
   // Subscriber
   rclcpp::GenericSubscription::SharedPtr sub_topic_;
+  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_transform_;
 
   // Timer
   void onTimer();

system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml

@@ -0,0 +1,26 @@
+<launch>
+  <arg name="node_name_suffix" description="node name suffix"/>
+  <arg name="topic" description="transform topic name"/>
+  <arg name="frame_id" description="parent frame id"/>
+  <arg name="child_frame_id" description="child frame id"/>
+  <arg name="transient_local" default="false" description="add transient_local option to subscriber or not"/>
+  <arg name="best_effort" default="false" description="add best_effort option to subscriber or not"/>
+  <arg name="diag_name" description="diag name"/>
+  <arg name="warn_rate" description="warn rate[Hz]"/>
+  <arg name="error_rate" description="error rate[Hz]"/>
+  <arg name="timeout" description="timeout period[s]"/>
+  <arg name="window_size" default="10" description="window size"/>
+
+  <node pkg="topic_state_monitor" exec="topic_state_monitor_node" name="topic_state_monitor_$(var node_name_suffix)" output="screen">
+    <param name="topic" value="$(var topic)"/>
+    <param name="frame_id" value="$(var frame_id)"/>
+    <param name="child_frame_id" value="$(var child_frame_id)"/>
+    <param name="transient_local" value="$(var transient_local)"/>
+    <param name="best_effort" value="$(var best_effort)"/>
+    <param name="diag_name" value="$(var diag_name)"/>
+    <param name="warn_rate" value="$(var warn_rate)"/>
+    <param name="error_rate" value="$(var error_rate)"/>
+    <param name="timeout" value="$(var timeout)"/>
+    <param name="window_size" value="$(var window_size)"/>
+  </node>
+</launch>

system/topic_state_monitor/package.xml

@@ -15,6 +15,7 @@
   <depend>diagnostic_updater</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tf2_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

system/topic_state_monitor/src/topic_state_monitor_core.cpp

@@ -44,10 +44,18 @@ TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_op
   // Parameter
   node_param_.update_rate = declare_parameter("update_rate", 10.0);
   node_param_.topic = declare_parameter<std::string>("topic");
-  node_param_.topic_type = declare_parameter<std::string>("topic_type");
   node_param_.transient_local = declare_parameter("transient_local", false);
   node_param_.best_effort = declare_parameter("best_effort", false);
   node_param_.diag_name = declare_parameter<std::string>("diag_name");
+  node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static");
+
+  if (node_param_.is_transform) {
+    node_param_.frame_id = declare_parameter<std::string>("frame_id");
+    node_param_.child_frame_id = declare_parameter<std::string>("child_frame_id");
+  } else {
+    node_param_.topic_type = declare_parameter<std::string>("topic_type");
+  }
+
   param_.warn_rate = declare_parameter("warn_rate", 0.5);
   param_.error_rate = declare_parameter("error_rate", 0.1);
   param_.timeout = declare_parameter("timeout", 1.0);
@@ -69,11 +77,25 @@ TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_op
   if (node_param_.best_effort) {
     qos.best_effort();
   }
-  sub_topic_ = this->create_generic_subscription(
-    node_param_.topic, node_param_.topic_type, qos,
-    [this]([[maybe_unused]] std::shared_ptr<rclcpp::SerializedMessage> msg) {
-      topic_state_monitor_->update();
-    });
+
+  if (node_param_.is_transform) {
+    sub_transform_ = this->create_subscription<tf2_msgs::msg::TFMessage>(
+      node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
+        for (const auto & transform : msg->transforms) {
+          if (
+            transform.header.frame_id == node_param_.frame_id &&
+            transform.child_frame_id == node_param_.child_frame_id) {
+            topic_state_monitor_->update();
+          }
+        }
+      });
+  } else {
+    sub_topic_ = this->create_generic_subscription(
+      node_param_.topic, node_param_.topic_type, qos,
+      [this]([[maybe_unused]] std::shared_ptr<rclcpp::SerializedMessage> msg) {
+        topic_state_monitor_->update();
+      });
+  }
 
   // Diagnostic Updater
   updater_.setHardwareID("topic_state_monitor");
@@ -122,7 +144,12 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu
   const auto topic_rate = topic_state_monitor_->getTopicRate();
 
   // Add topic name
-  stat.addf("topic", "%s", node_param_.topic.c_str());
+  if (node_param_.is_transform) {
+    const auto frame = "(" + node_param_.frame_id + " to " + node_param_.child_frame_id + ")";
+    stat.addf("topic", "%s %s", node_param_.topic.c_str(), frame.c_str());
+  } else {
+    stat.addf("topic", "%s", node_param_.topic.c_str());
+  }
 
   // Judge level
   int8_t level = DiagnosticStatus::OK;