feat: mrm v0.6 add topc rely controller node

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

system/autoware_topic_relay_controller/README.md

@@ -0,0 +1,15 @@
+# topic_relay_controller
+
+## Purpose
+
+## Inputs / Outputs
+
+### Input
+
+### Output
+
+## Parameters
+
+## Assumptions / Known limits
+
+TBD.

system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml

@@ -0,0 +1,4 @@
+---
+/**:
+  ros__parameters:
+    tmp: 0

system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml

@@ -0,0 +1,22 @@
+<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="enabele_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"/>
+
+  <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 enabele_relay_control)"/>
+    <param name="srv_name" value="$(var srv_name)"/>
+  </node>
+</launch>

system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml

@@ -0,0 +1,22 @@
+<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="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="enabele_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"/>
+
+  <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="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 enabele_relay_control)"/>
+    <param name="srv_name" value="$(var srv_name)"/>
+  </node>
+</launch>

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="tetsuhiro.kawaguchi@tier4.jp">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>

system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json

@@ -0,0 +1,27 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for topic relay controller",
+  "type": "object",
+  "definitions": {
+    "topic_rely_controller": {
+      "type": "object",
+      "properties": {},
+      "required": [],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/topic_rely_controller"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp

@@ -0,0 +1,88 @@
+// 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"
+
+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");
+
+  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;
+        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::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 && is_relaying_) {
+            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_) pub_topic_->publish(*msg);
+      });
+  }
+}
+}  // namespace autoware::topic_relay_controller
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_relay_controller::TopicRelayController)

system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp

@@ -0,0 +1,67 @@
+// 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>
+
+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;
+};
+
+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_;
+
+  // State
+  bool is_relaying_;
+};
+}  // namespace autoware::topic_relay_controller
+
+#endif  // TOPIC_RELAY_CONTROLLER_NODE_HPP_

system/redundancy_switcher_interface/src/common/converter/reset_converter.cpp

@@ -35,13 +35,15 @@ void ResetConverter::setUdpSender(const std::string & dest_ip, const std::string
 
 void ResetConverter::setUdpReceiver(const std::string & src_ip, const std::string & src_port)
 {
-  udp_reset_response_receiver_ = std::make_unique<UdpReceiver<ResetResponse>>(src_ip, src_port, false);
+  udp_reset_response_receiver_ =
+    std::make_unique<UdpReceiver<ResetResponse>>(src_ip, src_port, false);
 }
 
 void ResetConverter::setService()
 {
   srv_reset_ = node_->create_service<autoware_adapi_v1_msgs::srv::RedundancySwitcherReset>(
-    "~/service/reset", std::bind(&ResetConverter::onResetRequest, this, std::placeholders::_1, std::placeholders::_2));
+    "~/service/reset",
+    std::bind(&ResetConverter::onResetRequest, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 void ResetConverter::onResetRequest(
@@ -56,7 +58,8 @@ void ResetConverter::onResetRequest(
 
     ResetResponse udp_response;
     try {
-      bool result = udp_reset_response_receiver_->receive(udp_response, 30);;
+      bool result = udp_reset_response_receiver_->receive(udp_response, 30);
+      ;
       if (!result) {
         response->status.success = false;
         response->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::SERVICE_TIMEOUT;
@@ -72,7 +75,7 @@ void ResetConverter::onResetRequest(
         response->status.message = "Reset successfully.";
         RCLCPP_INFO(node_->get_logger(), "Reset successfully.");
       }
-    } catch (const std::exception &e) {
+    } catch (const std::exception & e) {
       response->status.success = false;
       response->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::TRANSFORM_ERROR;
       response->status.message = "Failed to receive UDP response.";

system/redundancy_switcher_interface/src/common/converter/reset_converter.hpp

@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RESET__CONVERTER__RESET_CONVERTER_HPP_
-#define RESET__CONVERTER__RESET_CONVERTER_HPP_
+#ifndef COMMON__CONVERTER__RESET_CONVERTER_HPP_
+#define COMMON__CONVERTER__RESET_CONVERTER_HPP_
 
-#include "udp_sender.hpp"
 #include "udp_receiver.hpp"
+#include "udp_sender.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -60,4 +60,4 @@ class ResetConverter
 
 }  // namespace redundancy_switcher_interface
 
-#endif  // RESET__CONVERTER__RESET_CONVERTER_HPP_
+#endif  // COMMON__CONVERTER__RESET_CONVERTER_HPP_

system/redundancy_switcher_interface/src/common/converter/udp_receiver.hpp

@@ -45,8 +45,8 @@ class UdpReceiver
   ~UdpReceiver();
 
   bool receive(T & data, int timeout);  // for non callback and timeout
-  bool receive(T & data);           // for non callback
-  void receive();          // for callback
+  bool receive(T & data);               // for non callback
+  void receive();                       // for callback
 
 private:
   int socketfd_;
@@ -132,7 +132,8 @@ bool UdpReceiver<T>::receive(T & data, int timeout)
   memset(&addr, 0, sizeof(addr));
 
   if (has_received_udp_date(timeout)) {
-    ssize_t recv_size = recvfrom(socketfd_, &data, sizeof(T), 0, (struct sockaddr *)&addr, &addr_len);
+    ssize_t recv_size =
+      recvfrom(socketfd_, &data, sizeof(T), 0, (struct sockaddr *)&addr, &addr_len);
     if (recv_size < 0) {
       if (errno == EAGAIN || errno == EWOULDBLOCK) {
         return false;