feat: copy topic state monitor node

Signed-off-by: TetsuKawa <[email protected]>

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
+)

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.

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 <rclcpp/rclcpp.hpp>
+
+#include <deque>
+#include <string>
+
+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<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_

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 <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include <deque>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+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<rclcpp::Parameter> & parameters);
+
+  // Core
+  std::unique_ptr<TopicStateMonitor> topic_state_monitor_;
+
+  // Subscriber
+  rclcpp::GenericSubscription::SharedPtr sub_topic_;
+  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::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_

system/topic_rely_controller/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>

system/topic_rely_controller/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_rely_controller/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>topic_state_monitor</name>
+  <version>0.1.0</version>
+  <description>The topic_state_monitor package</description>
+  <maintainer email="[email protected]">Ryohsuke Mitsudome</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>ament_index_cpp</depend>
+  <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>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>