From 5487f9fa3a0226bddf793454e85b9124d6cee9de Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Tue, 20 Sep 2022 16:02:48 +0900 Subject: [PATCH] feat(topic_state_monitor): support transform topic check (#1586) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu --- system/topic_state_monitor/Readme.md | 18 ++++---- .../topic_state_monitor_core.hpp | 6 +++ .../launch/topic_state_monitor_tf.launch.xml | 26 ++++++++++++ system/topic_state_monitor/package.xml | 1 + .../src/topic_state_monitor_core.cpp | 41 +++++++++++++++---- 5 files changed, 77 insertions(+), 15 deletions(-) create mode 100644 system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml diff --git a/system/topic_state_monitor/Readme.md b/system/topic_state_monitor/Readme.md index 86e7fc2c3c80c..28333305ef5a9 100644 --- a/system/topic_state_monitor/Readme.md +++ b/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 diff --git a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp b/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp index 3df7d0d3a5472..4696823ffbd4b 100644 --- a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp +++ b/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp @@ -20,6 +20,8 @@ #include #include +#include + #include #include #include @@ -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::SharedPtr sub_transform_; // Timer void onTimer(); diff --git a/system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml b/system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml new file mode 100644 index 0000000000000..1a255a8a5859a --- /dev/null +++ b/system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/topic_state_monitor/package.xml b/system/topic_state_monitor/package.xml index 13c182cc715ee..2707a74ff5413 100644 --- a/system/topic_state_monitor/package.xml +++ b/system/topic_state_monitor/package.xml @@ -15,6 +15,7 @@ diagnostic_updater rclcpp rclcpp_components + tf2_msgs ament_lint_auto autoware_lint_common diff --git a/system/topic_state_monitor/src/topic_state_monitor_core.cpp b/system/topic_state_monitor/src/topic_state_monitor_core.cpp index a5a3d4ad8b96d..326193a58dc4e 100644 --- a/system/topic_state_monitor/src/topic_state_monitor_core.cpp +++ b/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("topic"); - node_param_.topic_type = declare_parameter("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("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("frame_id"); + node_param_.child_frame_id = declare_parameter("child_frame_id"); + } else { + node_param_.topic_type = declare_parameter("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 msg) { - topic_state_monitor_->update(); - }); + + if (node_param_.is_transform) { + sub_transform_ = this->create_subscription( + 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 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;