From 7606409acadf0c598fcebe6a98464519fefaf7b2 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Wed, 15 Jan 2025 13:02:40 +0900 Subject: [PATCH] feat(redundancy_relay_manager): add redundancy switcher interface node (#1758) * feat: add base branch Signed-off-by: TetsuKawa * style(pre-commit): autofix * feat: add subscriber Signed-off-by: TetsuKawa * style(pre-commit): autofix * feat: add more subscriber Signed-off-by: TetsuKawa * modify: delete const Signed-off-by: TetsuKawa * style(pre-commit): autofix * feat: add service Signed-off-by: TetsuKawa * style(pre-commit): autofix * feat: repalace polling_subscriber Signed-off-by: TetsuKawa * style(pre-commit): autofix * feat: add schema Signed-off-by: TetsuKawa * style(pre-commit): autofix --------- Signed-off-by: TetsuKawa Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 20 +++ .../README.md | 15 ++ .../redundancy_relay_manager.param.yaml | 4 + .../redundancy_relay_manager.launch.xml | 17 +++ .../package.xml | 25 +++ .../redundancy_relay_manager.schema.json | 33 ++++ .../src/redundancy_relay_manager_node.cpp | 142 ++++++++++++++++++ .../src/redundancy_relay_manager_node.hpp | 76 ++++++++++ .../topic_relay_controller_delete.launch.xml | 26 ++++ ...opic_relay_controller_tf_delete.launch.xml | 26 ++++ 10 files changed, 384 insertions(+) create mode 100644 system/autoware_redundancy_relay_manager/CMakeLists.txt create mode 100644 system/autoware_redundancy_relay_manager/README.md create mode 100644 system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml create mode 100644 system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml create mode 100644 system/autoware_redundancy_relay_manager/package.xml create mode 100644 system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json create mode 100644 system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp create mode 100644 system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp create mode 100644 system/autoware_topic_relay_controller/launch/topic_relay_controller_delete.launch.xml create mode 100644 system/autoware_topic_relay_controller/launch/topic_relay_controller_tf_delete.launch.xml diff --git a/system/autoware_redundancy_relay_manager/CMakeLists.txt b/system/autoware_redundancy_relay_manager/CMakeLists.txt new file mode 100644 index 0000000000000..4881d937c6b57 --- /dev/null +++ b/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 +) diff --git a/system/autoware_redundancy_relay_manager/README.md b/system/autoware_redundancy_relay_manager/README.md new file mode 100644 index 0000000000000..0e0208a8cf09a --- /dev/null +++ b/system/autoware_redundancy_relay_manager/README.md @@ -0,0 +1,15 @@ +# redundancy_relay_manger + +## Purpose + +## Inputs / Outputs + +### Input + +### Output + +## Parameters + +## Assumptions / Known limits + +TBD. diff --git a/system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml b/system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml new file mode 100644 index 0000000000000..41ad9f71c6ddd --- /dev/null +++ b/system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml @@ -0,0 +1,4 @@ +--- +/**: + ros__parameters: + service_timeout_ms: 10 # [ms] diff --git a/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml b/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml new file mode 100644 index 0000000000000..f6b7bb44416e4 --- /dev/null +++ b/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/system/autoware_redundancy_relay_manager/package.xml b/system/autoware_redundancy_relay_manager/package.xml new file mode 100644 index 0000000000000..713b69ef2532b --- /dev/null +++ b/system/autoware_redundancy_relay_manager/package.xml @@ -0,0 +1,25 @@ + + + + autoware_redundancy_relay_manager + 0.1.0 + The redundancy_relay_manger ROS 2 package + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_universe_utils + rclcpp + rclcpp_components + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json b/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json new file mode 100644 index 0000000000000..2382a8bc7d06b --- /dev/null +++ b/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 +} diff --git a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp new file mode 100644 index 0000000000000..a733ee073b499 --- /dev/null +++ b/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 +#include +#include + +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("service_timeout_ms"); + + // Subscribers + sub_main_election_status_ = create_subscription( + "~/input/main/election/status", rclcpp::QoS{1}, + std::bind(&RedundancyRelayManager::onMainElectionStatus, this, std::placeholders::_1)); + sub_sub_election_status_ = create_subscription( + "~/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( + "~/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( + "~/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::SharedPtr client, + std::string srv_name) +{ + auto request = std::make_shared(); + 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(autoware::redundancy_relay_manager::RedundancyRelayManager) diff --git a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp new file mode 100644 index 0000000000000..1c0b603a98f45 --- /dev/null +++ b/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 +#include + +#include +#include +#include + +#include + +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::SharedPtr sub_main_election_status_; + rclcpp::Subscription::SharedPtr sub_sub_election_status_; + + // Clients + rclcpp::CallbackGroup::SharedPtr client_relay_trajectory_group_; + rclcpp::Client::SharedPtr + client_relay_trajectory_; + rclcpp::CallbackGroup::SharedPtr client_relay_pose_with_covariance_group_; + rclcpp::Client::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::SharedPtr client, + std::string topic_name); +}; +} // namespace autoware::redundancy_relay_manager + +#endif // REDUNDANCY_RELAY_MANAGER_NODE_HPP_ diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_delete.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_delete.launch.xml new file mode 100644 index 0000000000000..5e3de6baec8b5 --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_delete.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf_delete.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf_delete.launch.xml new file mode 100644 index 0000000000000..8e36240d35e6f --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf_delete.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +