diff --git a/system/topic_rely_controller/CMakeLists.txt b/system/topic_rely_controller/CMakeLists.txt new file mode 100644 index 0000000000000..a4a040c449cdb --- /dev/null +++ b/system/topic_rely_controller/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(topic_state_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(topic_state_monitor SHARED + src/topic_state_monitor/topic_state_monitor.cpp + src/topic_state_monitor_core.cpp +) + +rclcpp_components_register_node(topic_state_monitor + PLUGIN "topic_state_monitor::TopicStateMonitorNode" + EXECUTABLE topic_state_monitor_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/system/topic_rely_controller/README.md b/system/topic_rely_controller/README.md new file mode 100644 index 0000000000000..28333305ef5a9 --- /dev/null +++ b/system/topic_rely_controller/README.md @@ -0,0 +1,60 @@ +# topic_state_monitor + +## Purpose + +This node monitors input topic for abnormalities such as timeout and low frequency. +The result of topic status is published as diagnostics. + +## Inner-workings / Algorithms + +The types of topic status and corresponding diagnostic status are following. + +| Topic status | Diagnostic status | Description | +| ------------- | ----------------- | ---------------------------------------------------- | +| `OK` | OK | The topic has no abnormalities | +| `NotReceived` | ERROR | The topic has not been received yet | +| `WarnRate` | WARN | The frequency of the topic is dropped | +| `ErrorRate` | ERROR | The frequency of the topic is significantly dropped | +| `Timeout` | ERROR | The topic subscription is stopped for a certain time | + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------- | -------- | --------------------------------- | +| any name | any type | Subscribe target topic to monitor | + +### Output + +| Name | Type | Description | +| -------------- | --------------------------------- | ------------------- | +| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +## Parameters + +### Node Parameters + +| 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 + +| Name | Type | Default Value | Description | +| ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------- | +| `warn_rate` | double | 0.5 | If the topic rate is lower than this value, the topic status becomes `WarnRate` | +| `error_rate` | double | 0.1 | If the topic rate is lower than this value, the topic status becomes `ErrorRate` | +| `timeout` | double | 1.0 | If the topic subscription is stopped for more than this time [s], the topic status becomes `Timeout` | +| `window_size` | int | 10 | Window size of target topic for calculating frequency | + +## Assumptions / Known limits + +TBD. diff --git a/system/topic_rely_controller/include/topic_state_monitor/topic_state_monitor.hpp b/system/topic_rely_controller/include/topic_state_monitor/topic_state_monitor.hpp new file mode 100644 index 0000000000000..2057ca4c5c86c --- /dev/null +++ b/system/topic_rely_controller/include/topic_state_monitor/topic_state_monitor.hpp @@ -0,0 +1,73 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#ifndef TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ +#define TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ + +#include + +#include +#include + +namespace topic_state_monitor +{ +struct Param +{ + double warn_rate; + double error_rate; + double timeout; + int window_size; +}; + +enum class TopicStatus : int8_t { + Ok, + NotReceived, + WarnRate, + ErrorRate, + Timeout, +}; + +class TopicStateMonitor +{ +public: + explicit TopicStateMonitor(rclcpp::Node & node); + + void setParam(const Param & param) { param_ = param; } + + rclcpp::Time getLastMessageTime() const { return last_message_time_; } + double getTopicRate() const { return topic_rate_; } + + void update(); + TopicStatus getTopicStatus() const; + +private: + Param param_; + + static constexpr double max_rate = 100000.0; + + std::deque time_buffer_; + rclcpp::Time last_message_time_ = rclcpp::Time(0); + double topic_rate_ = TopicStateMonitor::max_rate; + + rclcpp::Clock::SharedPtr clock_; + + double calcTopicRate() const; + bool isNotReceived() const; + bool isWarnRate() const; + bool isErrorRate() const; + bool isTimeout() const; +}; +} // namespace topic_state_monitor + +#endif // TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ diff --git a/system/topic_rely_controller/include/topic_state_monitor/topic_state_monitor_core.hpp b/system/topic_rely_controller/include/topic_state_monitor/topic_state_monitor_core.hpp new file mode 100644 index 0000000000000..4696823ffbd4b --- /dev/null +++ b/system/topic_rely_controller/include/topic_state_monitor/topic_state_monitor_core.hpp @@ -0,0 +1,79 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#ifndef TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ +#define TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ + +#include "topic_state_monitor/topic_state_monitor.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace topic_state_monitor +{ +struct NodeParam +{ + double update_rate; + 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 +{ +public: + explicit TopicStateMonitorNode(const rclcpp::NodeOptions & node_options); + +private: + // Parameter + NodeParam node_param_; + Param param_; + + // Parameter Reconfigure + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + // Core + std::unique_ptr topic_state_monitor_; + + // Subscriber + rclcpp::GenericSubscription::SharedPtr sub_topic_; + rclcpp::Subscription::SharedPtr sub_transform_; + + // Timer + void onTimer(); + rclcpp::TimerBase::SharedPtr timer_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_; + + void checkTopicStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); +}; +} // namespace topic_state_monitor + +#endif // TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ diff --git a/system/topic_rely_controller/launch/topic_state_monitor.launch.xml b/system/topic_rely_controller/launch/topic_state_monitor.launch.xml new file mode 100644 index 0000000000000..7304dc8c5a273 --- /dev/null +++ b/system/topic_rely_controller/launch/topic_state_monitor.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/topic_rely_controller/launch/topic_state_monitor_tf.launch.xml b/system/topic_rely_controller/launch/topic_state_monitor_tf.launch.xml new file mode 100644 index 0000000000000..1a255a8a5859a --- /dev/null +++ b/system/topic_rely_controller/launch/topic_state_monitor_tf.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/topic_rely_controller/package.xml b/system/topic_rely_controller/package.xml new file mode 100644 index 0000000000000..057d58d00d5a1 --- /dev/null +++ b/system/topic_rely_controller/package.xml @@ -0,0 +1,25 @@ + + + + topic_state_monitor + 0.1.0 + The topic_state_monitor package + Ryohsuke Mitsudome + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + ament_index_cpp + diagnostic_updater + rclcpp + rclcpp_components + tf2_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/topic_rely_controller/src/topic_state_monitor/topic_state_monitor.cpp b/system/topic_rely_controller/src/topic_state_monitor/topic_state_monitor.cpp new file mode 100644 index 0000000000000..ff6806e96fac9 --- /dev/null +++ b/system/topic_rely_controller/src/topic_state_monitor/topic_state_monitor.cpp @@ -0,0 +1,102 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#include "topic_state_monitor/topic_state_monitor.hpp" + +namespace topic_state_monitor +{ +TopicStateMonitor::TopicStateMonitor(rclcpp::Node & node) : clock_(node.get_clock()) +{ +} + +void TopicStateMonitor::update() +{ + // Add data + last_message_time_ = clock_->now(); + time_buffer_.push_back(last_message_time_); + + // Remove old data + while (static_cast(time_buffer_.size()) > param_.window_size) { + time_buffer_.pop_front(); + } + + // Calc topic rate + topic_rate_ = calcTopicRate(); +} + +TopicStatus TopicStateMonitor::getTopicStatus() const +{ + if (isNotReceived()) { + return TopicStatus::NotReceived; + } + if (isTimeout()) { + return TopicStatus::Timeout; + } + if (isErrorRate()) { + return TopicStatus::ErrorRate; + } + if (isWarnRate()) { + return TopicStatus::WarnRate; + } + return TopicStatus::Ok; +} + +double TopicStateMonitor::calcTopicRate() const +{ + // Output max_rate when topic rate can't be calculated. + // In this case, it's assumed timeout is used instead. + if (time_buffer_.size() < 2) { + return TopicStateMonitor::max_rate; + } + + const auto time_diff = (time_buffer_.back() - time_buffer_.front()).seconds(); + const auto num_intervals = time_buffer_.size() - 1; + + return static_cast(num_intervals) / time_diff; +} + +bool TopicStateMonitor::isNotReceived() const +{ + return time_buffer_.empty(); +} + +bool TopicStateMonitor::isWarnRate() const +{ + if (param_.warn_rate == 0.0) { + return false; + } + + return getTopicRate() < param_.warn_rate; +} + +bool TopicStateMonitor::isErrorRate() const +{ + if (param_.error_rate == 0.0) { + return false; + } + + return getTopicRate() < param_.error_rate; +} + +bool TopicStateMonitor::isTimeout() const +{ + if (param_.timeout == 0.0) { + return false; + } + + const auto time_diff = (clock_->now() - time_buffer_.back()).seconds(); + + return time_diff > param_.timeout; +} +} // namespace topic_state_monitor diff --git a/system/topic_rely_controller/src/topic_state_monitor_core.cpp b/system/topic_rely_controller/src/topic_state_monitor_core.cpp new file mode 100644 index 0000000000000..ff58a0c6fd584 --- /dev/null +++ b/system/topic_rely_controller/src/topic_state_monitor_core.cpp @@ -0,0 +1,211 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#include "topic_state_monitor/topic_state_monitor_core.hpp" + +#include +#include +#include +#include + +namespace +{ +template +void update_param( + const std::vector & parameters, const std::string & name, T & value) +{ + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + value = it->template get_value(); + } +} +} // namespace + +namespace topic_state_monitor +{ +TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_options) +: Node("topic_state_monitor", node_options), updater_(this) +{ + using std::placeholders::_1; + + // Parameter + node_param_.update_rate = declare_parameter("update_rate", 10.0); + node_param_.topic = declare_parameter("topic"); + 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); + param_.window_size = declare_parameter("window_size", 10); + + // Parameter Reconfigure + set_param_res_ = + this->add_on_set_parameters_callback(std::bind(&TopicStateMonitorNode::onParameter, this, _1)); + + // Core + topic_state_monitor_ = std::make_unique(*this); + topic_state_monitor_->setParam(param_); + + // Subscriber + rclcpp::QoS qos = rclcpp::QoS{1}; + if (node_param_.transient_local) { + qos.transient_local(); + } + if (node_param_.best_effort) { + qos.best_effort(); + } + + 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"); + updater_.add(node_param_.diag_name, this, &TopicStateMonitorNode::checkTopicStatus); + + // Timer + const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&TopicStateMonitorNode::onTimer, this)); +} + +rcl_interfaces::msg::SetParametersResult TopicStateMonitorNode::onParameter( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + try { + update_param(parameters, "warn_rate", param_.warn_rate); + update_param(parameters, "error_rate", param_.error_rate); + update_param(parameters, "timeout", param_.timeout); + update_param(parameters, "window_size", param_.window_size); + topic_state_monitor_->setParam(param_); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + + return result; +} + +void TopicStateMonitorNode::onTimer() +{ + // Publish diagnostics + updater_.force_update(); +} + +void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using diagnostic_msgs::msg::DiagnosticStatus; + + // Get information + const auto topic_status = topic_state_monitor_->getTopicStatus(); + const auto last_message_time = topic_state_monitor_->getLastMessageTime(); + const auto topic_rate = topic_state_monitor_->getTopicRate(); + + // Add topic name + 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()); + } + + const auto print_warn = [&](const std::string & msg) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, "%s", msg.c_str()); + }; + const auto print_info = [&](const std::string & msg) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 3000, "%s", msg.c_str()); + }; + + // Judge level + int8_t level = DiagnosticStatus::OK; + if (topic_status == TopicStatus::Ok) { + level = DiagnosticStatus::OK; + stat.add("status", "OK"); + } else if (topic_status == TopicStatus::NotReceived) { + level = DiagnosticStatus::ERROR; + stat.add("status", "NotReceived"); + print_info(node_param_.topic + " has not received. Set ERROR in diagnostics."); + } else if (topic_status == TopicStatus::WarnRate) { + level = DiagnosticStatus::WARN; + stat.add("status", "WarnRate"); + print_warn( + node_param_.topic + " topic rate has dropped to the warning level. Set WARN in diagnostics."); + } else if (topic_status == TopicStatus::ErrorRate) { + level = DiagnosticStatus::ERROR; + stat.add("status", "ErrorRate"); + print_warn( + node_param_.topic + " topic rate has dropped to the error level. Set ERROR in diagnostics."); + } else if (topic_status == TopicStatus::Timeout) { + level = DiagnosticStatus::ERROR; + stat.add("status", "Timeout"); + print_warn(node_param_.topic + " topic is timeout. Set ERROR in diagnostics."); + } + + // Add key-value + stat.addf("warn_rate", "%.2f [Hz]", param_.warn_rate); + stat.addf("error_rate", "%.2f [Hz]", param_.error_rate); + stat.addf("timeout", "%.2f [s]", param_.timeout); + stat.addf("measured_rate", "%.2f [Hz]", topic_rate); + stat.addf("now", "%.2f [s]", this->now().seconds()); + stat.addf("last_message_time", "%.2f [s]", last_message_time.seconds()); + + // Create message + std::string msg; + if (level == DiagnosticStatus::OK) { + msg = "OK"; + } else if (level == DiagnosticStatus::WARN) { + msg = "Warn"; + } else if (level == DiagnosticStatus::ERROR) { + msg = "Error"; + } + + // Add summary + stat.summary(level, msg); +} + +} // namespace topic_state_monitor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(topic_state_monitor::TopicStateMonitorNode)