feat: add topic_state_monitor package (autowarefoundation#15)

* Ros2 v0.8.0 topic state monitor (autowarefoundation#283) * Add node_name_suffix to topic_state_monitor.launch (autowarefoundation#1157) (autowarefoundation#370) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix launch file (autowarefoundation#411) * add transient local option to topic state monitor (autowarefoundation#410) * add transient local option to topic state monitor * sort parameters * sort parameter * [topic_state_monitor]: Add best effort option (autowarefoundation#430) Signed-off-by: autoware <autoware@example.com> Co-authored-by: autoware <autoware@example.com> * add use_sim-time option (autowarefoundation#454) * Fix for rolling (autowarefoundation#1226) * Replace doc by description Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Replace ns by push-ros-namespace Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * change to composable node (autowarefoundation#1233) * Unify Apache-2.0 license name (autowarefoundation#1242) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix lint errors (autowarefoundation#1378) * Fix lint errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix variable names Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use integrated generic subscription (autowarefoundation#1342) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * suppress warnings for declare parameters (autowarefoundation#1724) * fix for lanelet2_extension * fix for traffic light ssd fine detector * fix for topic_state_monitor * fix for dummy diag publisher * fix for remote cmd converter * fix for vehicle_info_util * fix for multi object tracker * fix for freespace planner * fix for autoware_error_monitor * add Werror for multi object tracker * fix for multi object tracker * add Werror for liraffic light ssd fine detector * add Werror for topic state monitor * add Werror * add Werror * add Werror * add Werror * fix style * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * remove COLCON_IGNORE in system_packages and map_tf_generator (autowarefoundation#532) * [topic_state_monitor]add readme (autowarefoundation#565) * add readme * Update system/topic_state_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update system/topic_state_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update system/topic_state_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update system/topic_state_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update system/topic_state_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: autoware <autoware@example.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
badai-nguyen · Dec 2, 2021 · c4d49d2 · c4d49d2
1 parent d6c2d47
commit c4d49d2
Show file tree

Hide file tree

Showing 8 changed files with 556 additions and 0 deletions.
diff --git a/system/topic_state_monitor/CMakeLists.txt b/system/topic_state_monitor/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.5)
+project(topic_state_monitor)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+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
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Install
+## directories
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)
diff --git a/system/topic_state_monitor/Readme.md b/system/topic_state_monitor/Readme.md
@@ -0,0 +1,58 @@
+# 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                                           |
+| ------------- | ------ | ------------- | ----------------------------------------------------- |
+| `update_rate` | double | 10.0          | Timer callback period [Hz]                            |
+| `window_size` | int    | 10            | Window size of target topic for calculating frequency |
+
+### Core 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                                                             |
+| `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` |
+
+## Assumptions / Known limits
+
+TBD.
diff --git a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp b/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp
@@ -0,0 +1,78 @@
+// 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 <rclcpp/rclcpp.hpp>
+
+#include <deque>
+#include <string>
+
+namespace topic_state_monitor
+{
+struct Param
+{
+  std::string topic;
+  std::string topic_type;
+  bool transient_local;
+  bool best_effort;
+  std::string diag_name;
+  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<rclcpp::Time> 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_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
@@ -0,0 +1,68 @@
+// 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 <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <deque>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace topic_state_monitor
+{
+struct NodeParam
+{
+  double update_rate;
+};
+
+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<rclcpp::Parameter> & parameters);
+
+  // Core
+  std::unique_ptr<TopicStateMonitor> topic_state_monitor_;
+
+  // Subscriber
+  rclcpp::GenericSubscription::SharedPtr sub_topic_;
+
+  // 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_state_monitor/launch/topic_state_monitor.launch.xml b/system/topic_state_monitor/launch/topic_state_monitor.launch.xml
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="node_name_suffix" description="node name suffix" />
+  <arg name="topic" description="input topic name" />
+  <arg name="topic_type" description="input topic type" />
+  <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="topic_type" value="$(var topic_type)" />
+    <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>
diff --git a/system/topic_state_monitor/package.xml b/system/topic_state_monitor/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>topic_state_monitor</name>
+  <version>0.1.0</version>
+  <description>The topic_state_monitor package</description>
+  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>ament_index_cpp</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp b/system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp
@@ -0,0 +1,97 @@
+// 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<int>(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<double>(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