feat(autoware_redundancy_relay_manager): add redundancy relay manager node

system/autoware_redundancy_relay_manager/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_redundancy_relay_manager)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/redundancy_relay_manager_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::redundancy_relay_manager::RedundancyRelayManager"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR MultiThreadedExecutor
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

system/autoware_redundancy_relay_manager/README.md

@@ -0,0 +1,33 @@
+## Purpose
+
+The `redundancy_relay_manager` node subscribes to the election status topics from both the Main ECU and Sub ECU. It manages topic relays between these ECUs, ensuring that when the control path changes, the topic relay from the Main ECU to the Sub ECU stops.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                           | Type                                              | Description                              |
+| ------------------------------ | ------------------------------------------------- | ---------------------------------------- |
+| `~/input/main/election/status` | `tier4_system_msgs::msg::ElectionStatus`          | Election status topic from the Main ECU. |
+| `~/input/sub/election/status`  | `tier4_system_msgs::msg::ElectionStatus`          | Election status topic from the Sub ECU.  |
+| `~/input/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Current operation mode of the system.    |
+
+### Output
+
+| Name                                                           | Type                                              | Description                                |
+| -------------------------------------------------------------- | ------------------------------------------------- | ------------------------------------------ |
+| `~/output/topic_relay_controller_trajectory/operate`           | `tier4_system_msgs::srv::ChangeTopicRelayControl` | Service to control trajectory topic relay. |
+| `~/output/topic_relay_controller_pose_with_covariance/operate` | `tier4_system_msgs::srv::ChangeTopicRelayControl` | Service to control pose topic relay.       |
+
+## Parameters
+
+| Name                 | Type  | Default Value | Description                                           |
+| -------------------- | ----- | ------------- | ----------------------------------------------------- |
+| `service_timeout_ms` | `int` | `1000`        | Timeout duration (in milliseconds) for service calls. |
+
+## Assumptions / Known limits
+
+- The node assumes the availability of the election status topics (`~/input/main/election/status` and `~/input/sub/election/status`) and the operation mode state (`~/input/operation_mode/state`).
+- The node dynamically enables or disables topic relays based on the `path_info` field in the election status messages.
+- The system relies on proper remapping and configuration of input and output topics through the launch file.
+- Relay behavior is controlled through service calls, which are subject to timeout if the service server does not respond in time.

system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml

@@ -0,0 +1,4 @@
+---
+/**:
+  ros__parameters:
+    service_timeout_ms: 10 # [ms]

system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml

@@ -0,0 +1,17 @@
+<launch>
+  <arg name="input_operation_mode_state" default="/system/operation_mode/state"/>
+  <arg name="input_main_election_status" default="/system/election/status"/>
+  <arg name="input_sub_election_status" default="/sub/system/election/status"/>
+  <arg name="output_topic_relay_controller_trajectory_operate" default="/system/topic_relay_controller_trajectory/operate"/>
+  <arg name="output_topic_relay_controller_pose_with_covariance_operate" default="/system/topic_relay_controller_pose_with_covariance/operate"/>
+  <arg name="config_file" default="$(find-pkg-share autoware_redundancy_relay_manager)/config/redundancy_relay_manager.param.yaml"/>
+
+  <node pkg="autoware_redundancy_relay_manager" exec="autoware_redundancy_relay_manager_node" name="redundancy_relay_manager" output="screen">
+    <remap from="~/input/operation_mode/state" to="$(var input_operation_mode_state)"/>
+    <remap from="~/input/main/election/status" to="$(var input_main_election_status)"/>
+    <remap from="~/input/sub/election/status" to="$(var input_sub_election_status)"/>
+    <remap from="~/output/topic_relay_controller_trajectory/operate" to="$(var output_topic_relay_controller_trajectory_operate)"/>
+    <remap from="~/output/topic_relay_controller_pose_with_covariance/operate" to="$(var output_topic_relay_controller_pose_with_covariance_operate)"/>
+    <param from="$(var config_file)"/>
+  </node>
+</launch>

system/autoware_redundancy_relay_manager/package.xml

@@ -0,0 +1,26 @@
+<?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_redundancy_relay_manager</name>
+  <version>0.1.0</version>
+  <description>The redundancy_relay_manger ROS 2 package</description>
+  <maintainer email="[email protected]">Tetsuhiro Kawaguchi</maintainer>
+  <maintainer email="[email protected]">Makoto Kurihara</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</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_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json

@@ -0,0 +1,33 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for topic relay controller",
+  "type": "object",
+  "definitions": {
+    "topic_rely_controller": {
+      "type": "object",
+      "properties": {
+        "service_timeout_ms": {
+          "type": "integer",
+          "description": "Timeout for service call in milliseconds.",
+          "default": 10
+        }
+      },
+      "required": ["service_timeout_ms"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/topic_rely_controller"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp

@@ -0,0 +1,142 @@
+// 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 "redundancy_relay_manager_node.hpp"
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+namespace autoware::redundancy_relay_manager
+{
+RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & options)
+: Node("redundancy_relay_manager", options), is_relaying_(true), is_stopped_by_main_(true)
+{
+  // Params
+  node_param_.service_timeout_ms = declare_parameter<int>("service_timeout_ms");
+
+  // Subscribers
+  sub_main_election_status_ = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
+    "~/input/main/election/status", rclcpp::QoS{1},
+    std::bind(&RedundancyRelayManager::onMainElectionStatus, this, std::placeholders::_1));
+  sub_sub_election_status_ = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
+    "~/input/sub/election/status", rclcpp::QoS{1},
+    std::bind(&RedundancyRelayManager::onSubElectionStatus, this, std::placeholders::_1));
+
+  // Clients
+  client_relay_trajectory_group_ =
+    create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  client_relay_trajectory_ = create_client<tier4_system_msgs::srv::ChangeTopicRelayControl>(
+    "~/output/topic_relay_controller_trajectory/operate", rmw_qos_profile_services_default,
+    client_relay_trajectory_group_);
+  client_relay_pose_with_covariance_group_ =
+    create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  client_relay_pose_with_covariance_ =
+    create_client<tier4_system_msgs::srv::ChangeTopicRelayControl>(
+      "~/output/topic_relay_controller_pose_with_covariance/operate",
+      rmw_qos_profile_services_default, client_relay_pose_with_covariance_group_);
+}
+
+void RedundancyRelayManager::onMainElectionStatus(
+  const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg)
+{
+  const auto tmp_election_status = main_election_status_;
+  main_election_status_ = msg;
+
+  auto operation_mode_state = sub_operation_mode_state_.takeData();
+  if (operation_mode_state == nullptr) return;
+
+  if (is_relaying_) {
+    if (tmp_election_status == nullptr) return;
+    if (operation_mode_state->mode != autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS)
+      return;
+    if (((msg->path_info >> 3) & 0x01) == 1 || ((tmp_election_status->path_info >> 3) & 0x01) == 0)
+      return;
+
+    requestTopicRelayControl(false, client_relay_trajectory_, "topic_relay_control_trajectory");
+    requestTopicRelayControl(
+      false, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
+    is_relaying_ = false;
+    is_stopped_by_main_ = true;
+  } else {
+    if (((msg->path_info >> 3) & 0x01) == 1 && is_stopped_by_main_) {
+      requestTopicRelayControl(true, client_relay_trajectory_, "topic_relay_control_trajectory");
+      requestTopicRelayControl(
+        true, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
+      is_relaying_ = true;
+    }
+  }
+}
+
+void RedundancyRelayManager::onSubElectionStatus(
+  const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg)
+{
+  const auto tmp_election_status = sub_election_status_;
+  sub_election_status_ = msg;
+
+  auto operation_mode_state = sub_operation_mode_state_.takeData();
+  if (operation_mode_state == nullptr) return;
+
+  if (is_relaying_) {
+    if (tmp_election_status == nullptr) return;
+    if (operation_mode_state->mode != autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS)
+      return;
+    if (((msg->path_info >> 3) & 0x01) == 1 || ((tmp_election_status->path_info >> 3) & 0x01) == 0)
+      return;
+
+    requestTopicRelayControl(false, client_relay_trajectory_, "topic_relay_control_trajectory");
+    requestTopicRelayControl(
+      false, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
+    is_relaying_ = false;
+    is_stopped_by_main_ = false;
+  } else {
+    if (((msg->path_info >> 3) & 0x01) == 1 && !is_stopped_by_main_) {
+      requestTopicRelayControl(true, client_relay_trajectory_, "topic_relay_control_trajectory");
+      requestTopicRelayControl(
+        true, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance");
+      is_relaying_ = true;
+    }
+  }
+}
+
+void RedundancyRelayManager::requestTopicRelayControl(
+  const bool relay_on,
+  rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr client,
+  std::string srv_name)
+{
+  auto request = std::make_shared<tier4_system_msgs::srv::ChangeTopicRelayControl::Request>();
+  request->relay_on = relay_on;
+
+  const auto duration = std::chrono::milliseconds(node_param_.service_timeout_ms);
+  auto future = client->async_send_request(request).future.share();
+
+  if (future.wait_for(duration) == std::future_status::ready) {
+    auto response = future.get();
+    if (response->status.success) {
+      RCLCPP_INFO(
+        get_logger(), "Changed %s relay control: %s", srv_name.c_str(), relay_on ? "ON" : "OFF");
+    } else {
+      RCLCPP_ERROR(
+        get_logger(), "Failed to change %s relay control: %s", srv_name.c_str(),
+        relay_on ? "ON" : "OFF");
+    }
+  } else {
+    RCLCPP_ERROR(
+      get_logger(), "Service timeout %s relay control: %s", srv_name.c_str(),
+      relay_on ? "ON" : "OFF");
+  }
+}
+}  // namespace autoware::redundancy_relay_manager
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::redundancy_relay_manager::RedundancyRelayManager)

system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_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 REDUNDANCY_RELAY_MANAGER_NODE_HPP_
+#define REDUNDANCY_RELAY_MANAGER_NODE_HPP_
+
+// ROS 2 core
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <tier4_system_msgs/msg/election_status.hpp>
+#include <tier4_system_msgs/srv/change_topic_relay_control.hpp>
+
+#include <string>
+
+namespace autoware::redundancy_relay_manager
+{
+struct NodeParam
+{
+  int service_timeout_ms;
+};
+
+class RedundancyRelayManager : public rclcpp::Node
+{
+public:
+  explicit RedundancyRelayManager(const rclcpp::NodeOptions & options);
+
+private:
+  // Params
+  NodeParam node_param_;
+  // Subscribers
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    autoware_adapi_v1_msgs::msg::OperationModeState>
+    sub_operation_mode_state_{this, "~/input/operation_mode/state"};
+  rclcpp::Subscription<tier4_system_msgs::msg::ElectionStatus>::SharedPtr sub_main_election_status_;
+  rclcpp::Subscription<tier4_system_msgs::msg::ElectionStatus>::SharedPtr sub_sub_election_status_;
+
+  // Clients
+  rclcpp::CallbackGroup::SharedPtr client_relay_trajectory_group_;
+  rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr
+    client_relay_trajectory_;
+  rclcpp::CallbackGroup::SharedPtr client_relay_pose_with_covariance_group_;
+  rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr
+    client_relay_pose_with_covariance_;
+
+  // Callbacks
+  void onOperationModeState(const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg);
+  void onMainElectionStatus(const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg);
+  void onSubElectionStatus(const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg);
+
+  // State
+  bool is_relaying_;
+  bool is_stopped_by_main_;
+  tier4_system_msgs::msg::ElectionStatus::SharedPtr main_election_status_;
+  tier4_system_msgs::msg::ElectionStatus::SharedPtr sub_election_status_;
+
+  // Functions
+  void requestTopicRelayControl(
+    const bool relay_on,
+    const rclcpp::Client<tier4_system_msgs::srv::ChangeTopicRelayControl>::SharedPtr client,
+    std::string topic_name);
+};
+}  // namespace autoware::redundancy_relay_manager
+
+#endif  // REDUNDANCY_RELAY_MANAGER_NODE_HPP_