feat(topic_relay_controller): mrm v0.6 add topic relay controller (#1763

) * feat: add node base Signed-off-by: TetsuKawa <[email protected]> * feat: add subscriber 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]> * feat: delete config Signed-off-by: TetsuKawa <[email protected]> * feat: delete schema Signed-off-by: TetsuKawa <[email protected]> * modify: add include Signed-off-by: TetsuKawa <[email protected]> * modify: include guard Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix * feat: add enable_keep_publishing Signed-off-by: TetsuKawa <[email protected]> * modify: add qos setting Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix * modify: add include memory * 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>
tier4 · Jan 15, 2025 · 4fe5a9f · 4fe5a9f
1 parent 5bec14a
commit 4fe5a9f
Show file tree

Hide file tree

Showing 7 changed files with 308 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,19 @@
+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
+  launch
+)
diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md
@@ -0,0 +1,15 @@
+# topic_relay_controller
+
+## Purpose
+
+## Inputs / Outputs
+
+### Input
+
+### Output
+
+## Parameters
+
+## Assumptions / Known limits
+
+TBD.
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,26 @@
+<launch>
+  <arg name="node_name_suffix" description="node name suffix"/>
+  <arg name="topic" description="input topic name"/>
+  <arg name="remap_topic" description="output topic name"/>
+  <arg name="topic_type" description="input topic type"/>
+  <arg name="qos" default="1" description="QoS profile"/>
+  <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="enable_relay_control" default="true" description="enable relay control or not"/>
+  <arg name="srv_name" default="/system/topic_relay_controller_$(var node_name_suffix)/operate" description="service name for relay control"/>
+  <arg name="enable_keep_publishing" default="false" description="enable keep publish last topic when not subscribed"/>
+  <arg name="update_rate" default="10" description="update rate for topic publish"/>
+
+  <node pkg="autoware_topic_relay_controller" exec="autoware_topic_relay_controller_node" name="topic_relay_controller_$(var node_name_suffix)" output="screen">
+    <param name="topic" value="$(var topic)"/>
+    <param name="remap_topic" value="$(var remap_topic)"/>
+    <param name="topic_type" value="$(var topic_type)"/>
+    <param name="qos" value="$(var qos)"/>
+    <param name="transient_local" value="$(var transient_local)"/>
+    <param name="best_effort" value="$(var best_effort)"/>
+    <param name="enable_relay_control" value="$(var enable_relay_control)"/>
+    <param name="srv_name" value="$(var srv_name)"/>
+    <param name="enable_keep_publishing" value="$(var enable_keep_publishing)"/>
+    <param name="update_rate" value="$(var update_rate)"/>
+  </node>
+</launch>
diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml
@@ -0,0 +1,28 @@
+<launch>
+  <arg name="node_name_suffix" description="node name suffix"/>
+  <arg name="topic" description="transform topic name"/>
+  <arg name="remap_topic" description="remap topic name"/>
+  <arg name="qos" default="1" description="QoS profile"/>
+  <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="enable_relay_control" default="true" description="enable relay control or not"/>
+  <arg name="srv_name" default="/system/topic_relay_controller_$(var node_name_suffix)/operate" description="service name for relay control"/>
+  <arg name="enable_keep_publishing" default="false" description="enable keep publish last topic when not subscribed"/>
+  <arg name="update_rate" default="10" description="update rate for tf publish"/>
+
+  <node pkg="autoware_topic_relay_controller" exec="autoware_topic_relay_controller_node" name="topic_relay_controller_$(var node_name_suffix)" output="screen">
+    <param name="topic" value="$(var topic)"/>
+    <param name="remap_topic" value="$(var remap_topic)"/>
+    <param name="qos" value="$(var qos)"/>
+    <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="enable_relay_control" value="$(var enable_relay_control)"/>
+    <param name="srv_name" value="$(var srv_name)"/>
+    <param name="enable_keep_publishing" value="$(var enable_keep_publishing)"/>
+    <param name="update_rate" value="$(var update_rate)"/>
+  </node>
+</launch>
diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml
@@ -0,0 +1,24 @@
+<?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]">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,119 @@
+// 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 = declare_parameter("qos", 1);
+  node_param_.transient_local = declare_parameter("transient_local", false);
+  node_param_.best_effort = declare_parameter("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");
+  node_param_.srv_name = declare_parameter<std::string>("srv_name");
+  node_param_.enable_keep_publishing = declare_parameter<bool>("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::QoS{node_param_.qos};
+  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;
+  size_t qos;
+  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_