feat(autoware_topic_relay_controller): add topic relay controller node (

#9964) * feat: add node base Signed-off-by: TetsuKawa <[email protected]> * modify: include guard Signed-off-by: TetsuKawa <[email protected]> * feat: delete schema Signed-off-by: TetsuKawa <[email protected]> * feat: delete config Signed-off-by: TetsuKawa <[email protected]> * feat: add subscriber Signed-off-by: TetsuKawa <[email protected]> * modify: add include Signed-off-by: TetsuKawa <[email protected]> * feat: add publisher Signed-off-by: TetsuKawa <[email protected]> * feat: add service Signed-off-by: TetsuKawa <[email protected]> * modify: typo Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix Signed-off-by: TetsuKawa <[email protected]> * modify: add include memory Signed-off-by: TetsuKawa <[email protected]> * modify: add qos setting Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix Signed-off-by: TetsuKawa <[email protected]> * feat: add enable_keep_publishing Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix Signed-off-by: TetsuKawa <[email protected]> * feat: add readme Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix Signed-off-by: TetsuKawa <[email protected]> * feat: change qos name and add parameter type Signed-off-by: TetsuKawa <[email protected]> * feat: add config and delete arg from launch Signed-off-by: TetsuKawa <[email protected]> * modify: typo Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix * modify: add comment Signed-off-by: TetsuKawa <[email protected]> * modify: modify comment Signed-off-by: TetsuKawa <[email protected]> * feat: add maintainer Signed-off-by: TetsuKawa <[email protected]> * modift: change readme param format Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: TetsuKawa <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jan 22, 2025 · 0c118ba · 0c118ba
1 parent 30ab90e
commit 0c118ba
Show file tree

Hide file tree

Showing 7 changed files with 318 additions and 0 deletions.
diff --git a/system/autoware_topic_relay_controller/CMakeLists.txt b/system/autoware_topic_relay_controller/CMakeLists.txt
@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_topic_relay_controller)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/topic_relay_controller_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::topic_relay_controller::TopicRelayController"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR MultiThreadedExecutor
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)
diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md
@@ -0,0 +1,45 @@
+# topic_relay_controller
+
+## Purpose
+
+The node subscribes to a specified topic, remaps it, and republishes it. Additionally, it has the capability to continue publishing the last received value if the subscription stops.
+
+## Inputs / Outputs
+
+### Input
+
+| Name         | Type                       | Description                                                          |
+| ------------ | -------------------------- | -------------------------------------------------------------------- |
+| `/<topic>`   | `<specified message type>` | Topic to be subscribed, as defined by the `topic` parameter.         |
+| `/tf`        | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf`, used for transform message relay.  |
+| `/tf_static` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf_static`, used for static transforms. |
+
+### Output
+
+| Name             | Type                       | Description                                                                   |
+| ---------------- | -------------------------- | ----------------------------------------------------------------------------- |
+| `/<remap_topic>` | `<specified message type>` | Republished topic after remapping, as defined by the `remap_topic` parameter. |
+
+## Parameters
+
+| Variable               | Type    | Description                                                                                          |
+| ---------------------- | ------- | ---------------------------------------------------------------------------------------------------- |
+| topic                  | string  | The name of the input topic to subscribe to                                                          |
+| remap_topic            | string  | The name of the output topic to publish to                                                           |
+| topic_type             | string  | The type of messages being relayed                                                                   |
+| qos                    | integer | QoS profile to use for subscriptions and publications (default: `1`)                                 |
+| transient_local        | boolean | Enables transient local QoS for subscribers (default: `false`)                                       |
+| best_effort            | boolean | Enables best-effort QoS for subscribers (default: `false`)                                           |
+| enable_relay_control   | boolean | Allows dynamic relay control via a service (default: `true`)                                         |
+| srv_name               | string  | The service name for relay control when `enable_relay_control` is `true`                             |
+| enable_keep_publishing | boolean | Keeps publishing the last received topic value when not subscribed (default: `false`)                |
+| update_rate            | integer | The rate (Hz) for publishing the last topic value when `enable_keep_publishing` is `true` (optional) |
+| frame_id               | string  | Frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional)                 |
+| child_frame_id         | string  | Child frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional)           |
+
+## Assumptions / Known limits
+
+- The node assumes that the specified `topic` and `remap_topic` are valid and accessible within the ROS 2 environment.
+- If `enable_keep_publishing` is `true`, the node continuously republishes the last received value even if no new messages are being received.
+- For `/tf` and `/tf_static`, additional parameters like `frame_id` and `child_frame_id` are required for selective transformation relays.
+- QoS settings must be carefully chosen to match the requirements of the subscribed and published topics.
diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml
@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    topic: "/default"
+    topic_type: "std_msgs/msg/String" # unneccessary for /tf or /tf_static
+    remap_topic: "/remap_default"
+    #frame_id: neccessary for /tf or /tf_static
+    #child_frame_id: neccessary for /tf or /tf_static
+    qos_depth: 1
+    transient_local: false # Change qos to transient_local. Default is volatile.
+    best_effort: false # Change qos to best_effort. Default is reliable.
+    enable_relay_control: true
+    srv_name: "/system/topic_relay_controller_default/operate" # neccessary for enable_relay_control is true
+    enable_keep_publishing: false
+    #update_rate: #neccessary for enable_keep_publishing is true
diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml
@@ -0,0 +1,8 @@
+<launch>
+  <arg name="node_name_suffix" default="default" description="node name suffix"/>
+  <arg name="config_file" default="$(find-pkg-share autoware_topic_relay_controller)/config/topic_relay_controller.param.yaml" description="config file path"/>
+
+  <node pkg="autoware_topic_relay_controller" exec="autoware_topic_relay_controller_node" name="topic_relay_controller_$(var node_name_suffix)" output="screen">
+    <param from="$(var config_file)"/>
+  </node>
+</launch>
diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_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>autoware_topic_relay_controller</name>
+  <version>0.1.0</version>
+  <description>The topic_relay_controller ROS 2 package</description>
+  <maintainer email="[email protected]">Makoto Kurihara</maintainer>
+  <maintainer email="[email protected]">Tetsuhiro Kawaguchi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tier4_system_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>
diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp
@@ -0,0 +1,129 @@
+// Copyright 2025 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_relay_controller_node.hpp"
+
+#include <memory>
+#include <string>
+
+namespace autoware::topic_relay_controller
+{
+TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options)
+: Node("topic_relay_controller", options), is_relaying_(true)
+{
+  // Parameter
+  node_param_.topic = declare_parameter<std::string>("topic");
+  node_param_.remap_topic = declare_parameter<std::string>("remap_topic");
+  node_param_.qos_depth = declare_parameter<int>("qos_depth", 1);
+  node_param_.transient_local = declare_parameter<bool>("transient_local", false);
+  node_param_.best_effort = declare_parameter<bool>("best_effort", false);
+  node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static");
+  node_param_.enable_relay_control = declare_parameter<bool>("enable_relay_control");
+  if (node_param_.enable_relay_control)
+    node_param_.srv_name = declare_parameter<std::string>("srv_name");
+  node_param_.enable_keep_publishing = declare_parameter<bool>("enable_keep_publishing");
+  if (node_param_.enable_keep_publishing)
+    node_param_.update_rate = declare_parameter<int>("update_rate");
+
+  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");
+  }
+
+  // Service
+  if (node_param_.enable_relay_control) {
+    srv_change_relay_control_ = create_service<tier4_system_msgs::srv::ChangeTopicRelayControl>(
+      node_param_.srv_name,
+      [this](
+        const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request,
+        tier4_system_msgs::srv::ChangeTopicRelayControl::Response::SharedPtr response) {
+        is_relaying_ = request->relay_on;
+        RCLCPP_INFO(get_logger(), "relay control: %s", is_relaying_ ? "ON" : "OFF");
+        response->status.success = true;
+      });
+  }
+
+  // Subscriber
+  rclcpp::QoS qos = rclcpp::SystemDefaultsQoS();
+  if (node_param_.qos_depth > 0) {
+    size_t qos_depth = static_cast<size_t>(node_param_.qos_depth);
+    qos.keep_last(qos_depth);
+  } else {
+    RCLCPP_ERROR(get_logger(), "qos_depth must be greater than 0");
+    return;
+  }
+
+  if (node_param_.transient_local) {
+    qos.transient_local();
+  }
+  if (node_param_.best_effort) {
+    qos.best_effort();
+  }
+
+  if (node_param_.is_transform) {
+    // Publisher
+    pub_transform_ = this->create_publisher<tf2_msgs::msg::TFMessage>(node_param_.remap_topic, qos);
+
+    sub_transform_ = this->create_subscription<tf2_msgs::msg::TFMessage>(
+      node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::SharedPtr 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 || !is_relaying_)
+            return;
+
+          if (node_param_.enable_keep_publishing) {
+            last_tf_topic_ = msg;
+          } else {
+            pub_transform_->publish(*msg);
+          }
+        }
+      });
+  } else {
+    // Publisher
+    pub_topic_ =
+      this->create_generic_publisher(node_param_.remap_topic, node_param_.topic_type, qos);
+
+    sub_topic_ = this->create_generic_subscription(
+      node_param_.topic, node_param_.topic_type, qos,
+      [this]([[maybe_unused]] std::shared_ptr<rclcpp::SerializedMessage> msg) {
+        if (!is_relaying_) return;
+
+        if (node_param_.enable_keep_publishing) {
+          last_topic_ = msg;
+        } else {
+          pub_topic_->publish(*msg);
+        }
+      });
+  }
+
+  // Timer
+  if (node_param_.enable_keep_publishing) {
+    const auto update_period_ns = rclcpp::Rate(node_param_.update_rate).period();
+    timer_ = rclcpp::create_timer(this, get_clock(), update_period_ns, [this]() {
+      if (!is_relaying_) return;
+
+      if (node_param_.is_transform) {
+        if (last_tf_topic_) pub_transform_->publish(*last_tf_topic_);
+      } else {
+        if (last_topic_) pub_topic_->publish(*last_topic_);
+      }
+    });
+  }
+}
+}  // namespace autoware::topic_relay_controller
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_relay_controller::TopicRelayController)
diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp
@@ -0,0 +1,77 @@
+// Copyright 2025 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_RELAY_CONTROLLER_NODE_HPP_
+#define TOPIC_RELAY_CONTROLLER_NODE_HPP_
+
+// ROS 2 core
+#include <rclcpp/rclcpp.hpp>
+
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tier4_system_msgs/srv/change_topic_relay_control.hpp>
+
+#include <memory>
+#include <string>
+
+namespace autoware::topic_relay_controller
+{
+struct NodeParam
+{
+  std::string topic;
+  std::string remap_topic;
+  std::string topic_type;
+  int qos_depth;
+  std::string frame_id;
+  std::string child_frame_id;
+  bool transient_local;
+  bool best_effort;
+  bool is_transform;
+  bool enable_relay_control;
+  std::string srv_name;
+  bool enable_keep_publishing;
+  int update_rate;
+};
+
+class TopicRelayController : public rclcpp::Node
+{
+public:
+  explicit TopicRelayController(const rclcpp::NodeOptions & options);
+
+private:
+  // Parameter
+  NodeParam node_param_;
+
+  // Subscriber
+  rclcpp::GenericSubscription::SharedPtr sub_topic_;
+  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_transform_;
+
+  // Publisher
+  rclcpp::GenericPublisher::SharedPtr pub_topic_;
+  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_transform_;
+
+  // Service
+  rclcpp::Service<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr
+    srv_change_relay_control_;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // State
+  bool is_relaying_;
+  tf2_msgs::msg::TFMessage::SharedPtr last_tf_topic_;
+  std::shared_ptr<rclcpp::SerializedMessage> last_topic_;
+};
+}  // namespace autoware::topic_relay_controller
+
+#endif  // TOPIC_RELAY_CONTROLLER_NODE_HPP_