feat(redundancy_relay_manager): add redundancy switcher interface node (

#1758)

* feat: add base branch

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

* style(pre-commit): autofix

* feat: add subscriber

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

* style(pre-commit): autofix

* feat: add more subscriber

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

* modify: delete const

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

* style(pre-commit): autofix

* feat: add service

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

* style(pre-commit): autofix

* feat: repalace polling_subscriber

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

* style(pre-commit): autofix

* feat: add schema

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>

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,15 @@
+# redundancy_relay_manger
+
+## Purpose
+
+## Inputs / Outputs
+
+### Input
+
+### Output
+
+## Parameters
+
+## Assumptions / Known limits
+
+TBD.

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,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_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>
+  <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,76 @@
+// 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 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_

system/autoware_topic_relay_controller/launch/topic_relay_controller_delete.launch.xml

@@ -0,0 +1,26 @@
+<launch>
+  <arg name="node_name_suffix" default="aaa" description="node name suffix"/>
+  <arg name="topic" default="aaa" description="input topic name"/>
+  <arg name="remap_topic" default="bbb" description="output topic name"/>
+  <arg name="topic_type" default="tier4_system_msgs/msg/ElectionStatus" 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="true" 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>

system/autoware_topic_relay_controller/launch/topic_relay_controller_tf_delete.launch.xml

@@ -0,0 +1,26 @@
+<launch>
+  <arg name="node_name_suffix" default="aaa" description="node name suffix"/>
+  <arg name="topic" default="/tf" description="transform topic name"/>
+  <arg name="remap_topic" default="bbb" description="remap topic name"/>
+  <arg name="frame_id" default="parent" description="parent frame id"/>
+  <arg name="child_frame_id" default="child" 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="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>