From c6c5c881fc0d23211d915f090bec28f505f03639 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 18:44:55 +0900 Subject: [PATCH 01/16] feat: add base branch Signed-off-by: TetsuKawa --- .../CMakeLists.txt | 20 ++++++++++++ .../README.md | 15 +++++++++ .../redundancy_relay_manager.param.yaml | 4 +++ .../redundancy_relay_manager.launch.xml | 4 +++ .../package.xml | 22 +++++++++++++ .../redundancy_relay_manager.schema.json | 30 +++++++++++++++++ .../src/redundancy_relay_manager_node.cpp | 25 +++++++++++++++ .../src/redundancy_relay_manager_node.hpp | 32 +++++++++++++++++++ 8 files changed, 152 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 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..3a506213f054d --- /dev/null +++ b/system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml @@ -0,0 +1,4 @@ +--- +/**: + ros__parameters: + tmp: 0 \ No newline at end of file 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..8e1f4911db6ae --- /dev/null +++ b/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/system/autoware_redundancy_relay_manager/package.xml b/system/autoware_redundancy_relay_manager/package.xml new file mode 100644 index 0000000000000..8e2ab444a3c4b --- /dev/null +++ b/system/autoware_redundancy_relay_manager/package.xml @@ -0,0 +1,22 @@ + + + + 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 + + rclcpp + rclcpp_components + + 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..d51b0ae4ed1b6 --- /dev/null +++ b/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json @@ -0,0 +1,30 @@ +{ + "$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 + } + \ No newline at end of file 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..53447cd689486 --- /dev/null +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -0,0 +1,25 @@ +// 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" + +namespace autoware::redundancy_relay_manager +{ +RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & options) : Node("redundancy_relay_manager", options) +{ + RCLCPP_INFO(get_logger(), "redundancy_relay_manager_node started."); +} +} // 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..455e8756ceed9 --- /dev/null +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -0,0 +1,32 @@ +// 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 + +namespace autoware::redundancy_relay_manager +{ +class RedundancyRelayManager : public rclcpp::Node +{ +public: + explicit RedundancyRelayManager(const rclcpp::NodeOptions & options); + +private: +}; +} // namespace autoware::redundancy_relay_manager + +#endif // REDUNDANCY_RELAY_MANAGER_NODE_HPP_ \ No newline at end of file From 300ab2c87c024539b4939800a4123a13c3a364bd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 10 Jan 2025 09:48:56 +0000 Subject: [PATCH 02/16] style(pre-commit): autofix --- .../redundancy_relay_manager.param.yaml | 2 +- .../redundancy_relay_manager.launch.xml | 3 +- .../redundancy_relay_manager.schema.json | 55 +++++++++---------- .../src/redundancy_relay_manager_node.cpp | 5 +- .../src/redundancy_relay_manager_node.hpp | 2 +- 5 files changed, 32 insertions(+), 35 deletions(-) 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 index 3a506213f054d..f0dbecf93a9c1 100644 --- a/system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml +++ b/system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml @@ -1,4 +1,4 @@ --- /**: ros__parameters: - tmp: 0 \ No newline at end of file + tmp: 0 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 index 8e1f4911db6ae..cb1cdb4847655 100644 --- a/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml +++ b/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml @@ -1,4 +1,3 @@ - - + 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 index d51b0ae4ed1b6..f9795fb4b87ea 100644 --- a/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json +++ b/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json @@ -1,30 +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 - } - \ No newline at end of file + "$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 +} 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 index 53447cd689486..49b25d3810491 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -15,11 +15,12 @@ namespace autoware::redundancy_relay_manager { -RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & options) : Node("redundancy_relay_manager", options) +RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & options) +: Node("redundancy_relay_manager", options) { RCLCPP_INFO(get_logger(), "redundancy_relay_manager_node started."); } -} // namespace autoware::redundancy_relay_manager +} // 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 index 455e8756ceed9..c6c8f93c2648f 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -29,4 +29,4 @@ class RedundancyRelayManager : public rclcpp::Node }; } // namespace autoware::redundancy_relay_manager -#endif // REDUNDANCY_RELAY_MANAGER_NODE_HPP_ \ No newline at end of file +#endif // REDUNDANCY_RELAY_MANAGER_NODE_HPP_ From c3dd4c90e40a34033061f0d7a7c8bdb1ad7b53bc Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 14 Jan 2025 18:24:36 +0900 Subject: [PATCH 03/16] feat: add subscriber Signed-off-by: TetsuKawa --- .../redundancy_relay_manager.launch.xml | 8 +++++++- .../package.xml | 2 ++ .../src/redundancy_relay_manager_node.cpp | 20 ++++++++++++++++++- .../src/redundancy_relay_manager_node.hpp | 10 ++++++++++ 4 files changed, 38 insertions(+), 2 deletions(-) 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 index cb1cdb4847655..62d6c1c1340e0 100644 --- a/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml +++ b/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml @@ -1,3 +1,9 @@ - + + + + + + + diff --git a/system/autoware_redundancy_relay_manager/package.xml b/system/autoware_redundancy_relay_manager/package.xml index 8e2ab444a3c4b..bd780ac1ccee5 100644 --- a/system/autoware_redundancy_relay_manager/package.xml +++ b/system/autoware_redundancy_relay_manager/package.xml @@ -12,6 +12,8 @@ rclcpp rclcpp_components + autoware_adapi_v1_msgs + tier4_system_msgs ament_lint_auto autoware_lint_common 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 index 49b25d3810491..1f5b17789414b 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -18,7 +18,25 @@ namespace autoware::redundancy_relay_manager RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & options) : Node("redundancy_relay_manager", options) { - RCLCPP_INFO(get_logger(), "redundancy_relay_manager_node started."); + // Subscribers + sub_operation_mode_state_ = create_subscription( + "~/input/operation_mode/state", rclcpp::QoS{1}, + std::bind(&RedundancyRelayManager::onOperationModeState, this, std::placeholders::_1)); + sub_election_status_ = create_subscription( + "~/input/election/status", rclcpp::QoS{1}, + std::bind(&RedundancyRelayManager::onElectionStatus, this, std::placeholders::_1)); +} + +void RedundancyRelayManager::onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "Received operation mode state: %d", msg->mode); +} + +void RedundancyRelayManager::onElectionStatus( + const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "Received election state: %d", msg->path_info); } } // namespace autoware::redundancy_relay_manager 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 index c6c8f93c2648f..789fa4a921e66 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -18,6 +18,9 @@ // ROS 2 core #include +#include +#include + namespace autoware::redundancy_relay_manager { class RedundancyRelayManager : public rclcpp::Node @@ -26,6 +29,13 @@ class RedundancyRelayManager : public rclcpp::Node explicit RedundancyRelayManager(const rclcpp::NodeOptions & options); private: + // Subscribers + rclcpp::Subscription::SharedPtr sub_operation_mode_state_; + rclcpp::Subscription::SharedPtr sub_election_status_; + + // Callbacks + void onOperationModeState(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); }; } // namespace autoware::redundancy_relay_manager From 7e7b5b7130be9f8ac064481e476286b2597ddcc2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 09:29:26 +0000 Subject: [PATCH 04/16] style(pre-commit): autofix --- system/autoware_redundancy_relay_manager/package.xml | 2 +- .../src/redundancy_relay_manager_node.hpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/system/autoware_redundancy_relay_manager/package.xml b/system/autoware_redundancy_relay_manager/package.xml index bd780ac1ccee5..6cdce28a05ebe 100644 --- a/system/autoware_redundancy_relay_manager/package.xml +++ b/system/autoware_redundancy_relay_manager/package.xml @@ -10,9 +10,9 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs rclcpp rclcpp_components - autoware_adapi_v1_msgs tier4_system_msgs ament_lint_auto 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 index 789fa4a921e66..142c5cbae1093 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -30,11 +30,13 @@ class RedundancyRelayManager : public rclcpp::Node private: // Subscribers - rclcpp::Subscription::SharedPtr sub_operation_mode_state_; + rclcpp::Subscription::SharedPtr + sub_operation_mode_state_; rclcpp::Subscription::SharedPtr sub_election_status_; // Callbacks - void onOperationModeState(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); }; } // namespace autoware::redundancy_relay_manager From 486b9b75b81242a821e683e9d9ddb86e24b5d4ef Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 14 Jan 2025 22:41:42 +0900 Subject: [PATCH 05/16] feat: add more subscriber Signed-off-by: TetsuKawa --- .../redundancy_relay_manager.launch.xml | 6 ++++-- .../src/redundancy_relay_manager_node.cpp | 20 ++++++++++++++----- .../src/redundancy_relay_manager_node.hpp | 6 ++++-- 3 files changed, 23 insertions(+), 9 deletions(-) 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 index 62d6c1c1340e0..7dd64c24f9b83 100644 --- a/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml +++ b/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml @@ -1,9 +1,11 @@ - + + - + + 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 index 1f5b17789414b..4bed7cee847a4 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -22,9 +22,13 @@ RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & optio sub_operation_mode_state_ = create_subscription( "~/input/operation_mode/state", rclcpp::QoS{1}, std::bind(&RedundancyRelayManager::onOperationModeState, this, std::placeholders::_1)); - sub_election_status_ = create_subscription( - "~/input/election/status", rclcpp::QoS{1}, - std::bind(&RedundancyRelayManager::onElectionStatus, this, std::placeholders::_1)); + 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)); + } void RedundancyRelayManager::onOperationModeState( @@ -33,10 +37,16 @@ void RedundancyRelayManager::onOperationModeState( RCLCPP_INFO(get_logger(), "Received operation mode state: %d", msg->mode); } -void RedundancyRelayManager::onElectionStatus( +void RedundancyRelayManager::onMainElectionStatus( const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) { - RCLCPP_INFO(get_logger(), "Received election state: %d", msg->path_info); + RCLCPP_INFO(get_logger(), "Received main election state: %d", msg->path_info); +} + +void RedundancyRelayManager::onSubElectionStatus( + const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "Received sub election state: %d", msg->path_info); } } // namespace autoware::redundancy_relay_manager 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 index 142c5cbae1093..302269aed98d1 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -32,12 +32,14 @@ class RedundancyRelayManager : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr sub_operation_mode_state_; - rclcpp::Subscription::SharedPtr sub_election_status_; + rclcpp::Subscription::SharedPtr sub_main_election_status_; + rclcpp::Subscription::SharedPtr sub_sub_election_status_; // Callbacks void onOperationModeState( const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); - void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); + void onMainElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); + void onSubElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); }; } // namespace autoware::redundancy_relay_manager From 51137af7a6118bb2d1088896bedffcd25b3a6738 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 14 Jan 2025 22:43:19 +0900 Subject: [PATCH 06/16] modify: delete const Signed-off-by: TetsuKawa --- .../src/redundancy_relay_manager_node.cpp | 6 +++--- .../src/redundancy_relay_manager_node.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) 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 index 4bed7cee847a4..ac1e2e6f59963 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -32,19 +32,19 @@ RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & optio } void RedundancyRelayManager::onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) + const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg) { RCLCPP_INFO(get_logger(), "Received operation mode state: %d", msg->mode); } void RedundancyRelayManager::onMainElectionStatus( - const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) + const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg) { RCLCPP_INFO(get_logger(), "Received main election state: %d", msg->path_info); } void RedundancyRelayManager::onSubElectionStatus( - const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) + const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg) { RCLCPP_INFO(get_logger(), "Received sub election state: %d", msg->path_info); } 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 index 302269aed98d1..f8be2741b3e7f 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -37,9 +37,9 @@ class RedundancyRelayManager : public rclcpp::Node // Callbacks void onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); - void onMainElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); - void onSubElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); + 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); }; } // namespace autoware::redundancy_relay_manager From f86fc915cfcc235e69ddd71fc14a2125ef297e59 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 13:43:40 +0000 Subject: [PATCH 07/16] style(pre-commit): autofix --- .../src/redundancy_relay_manager_node.cpp | 1 - 1 file changed, 1 deletion(-) 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 index ac1e2e6f59963..a716ae71feb4d 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -28,7 +28,6 @@ RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & optio sub_sub_election_status_ = create_subscription( "~/input/sub/election/status", rclcpp::QoS{1}, std::bind(&RedundancyRelayManager::onSubElectionStatus, this, std::placeholders::_1)); - } void RedundancyRelayManager::onOperationModeState( From 3a111302e268fce72244c9edda2eda25a70d398a Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 15 Jan 2025 00:29:20 +0900 Subject: [PATCH 08/16] feat: add service Signed-off-by: TetsuKawa --- .../redundancy_relay_manager.param.yaml | 2 +- .../redundancy_relay_manager.launch.xml | 6 ++ .../src/redundancy_relay_manager_node.cpp | 87 ++++++++++++++++++- .../src/redundancy_relay_manager_node.hpp | 28 ++++++ 4 files changed, 118 insertions(+), 5 deletions(-) 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 index f0dbecf93a9c1..41ad9f71c6ddd 100644 --- a/system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml +++ b/system/autoware_redundancy_relay_manager/config/redundancy_relay_manager.param.yaml @@ -1,4 +1,4 @@ --- /**: ros__parameters: - tmp: 0 + 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 index 7dd64c24f9b83..f6b7bb44416e4 100644 --- a/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml +++ b/system/autoware_redundancy_relay_manager/launch/redundancy_relay_manager.launch.xml @@ -2,10 +2,16 @@ + + + + + + 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 index a716ae71feb4d..588ff2e4ff3d4 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -13,11 +13,20 @@ #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) +: 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_operation_mode_state_ = create_subscription( "~/input/operation_mode/state", rclcpp::QoS{1}, @@ -28,24 +37,94 @@ RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & optio 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::onOperationModeState( const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg) { - RCLCPP_INFO(get_logger(), "Received operation mode state: %d", msg->mode); + operation_mode_state_ = msg; } void RedundancyRelayManager::onMainElectionStatus( const tier4_system_msgs::msg::ElectionStatus::SharedPtr msg) { - RCLCPP_INFO(get_logger(), "Received main election state: %d", msg->path_info); + const auto tmp_election_status = main_election_status_; + main_election_status_ = msg; + + if (is_relaying_) { + if (tmp_election_status == nullptr || operation_mode_state_ == 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) { - RCLCPP_INFO(get_logger(), "Received sub election state: %d", msg->path_info); + const auto tmp_election_status = sub_election_status_; + sub_election_status_ = msg; + + if (is_relaying_) { + if (tmp_election_status == nullptr || operation_mode_state_ == 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 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 index f8be2741b3e7f..e98dbbed360a7 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -20,26 +20,54 @@ #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 rclcpp::Subscription::SharedPtr sub_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 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_; + autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr operation_mode_state_; + 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 From b632f353a3a2be1efd51f31202385796554ba6db Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 15:33:07 +0000 Subject: [PATCH 09/16] style(pre-commit): autofix --- .../src/redundancy_relay_manager_node.cpp | 53 ++++++++++++------- .../src/redundancy_relay_manager_node.hpp | 12 +++-- 2 files changed, 40 insertions(+), 25 deletions(-) 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 index 588ff2e4ff3d4..00069324ac5e8 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -20,9 +20,7 @@ namespace autoware::redundancy_relay_manager { RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & options) -: Node("redundancy_relay_manager", options), - is_relaying_(true), - is_stopped_by_main_(true) +: Node("redundancy_relay_manager", options), is_relaying_(true), is_stopped_by_main_(true) { // Params node_param_.service_timeout_ms = declare_parameter("service_timeout_ms"); @@ -37,17 +35,19 @@ RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & optio 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_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_); + 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::onOperationModeState( @@ -64,17 +64,21 @@ void RedundancyRelayManager::onMainElectionStatus( if (is_relaying_) { if (tmp_election_status == nullptr || operation_mode_state_ == 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; + 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"); + 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"); + requestTopicRelayControl( + true, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance"); is_relaying_ = true; } } @@ -88,24 +92,28 @@ void RedundancyRelayManager::onSubElectionStatus( if (is_relaying_) { if (tmp_election_status == nullptr || operation_mode_state_ == 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; + 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"); + 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"); + requestTopicRelayControl( + true, client_relay_pose_with_covariance_, "topic_relay_control_pose_with_covariance"); is_relaying_ = true; } } } void RedundancyRelayManager::requestTopicRelayControl( - const bool relay_on, + const bool relay_on, rclcpp::Client::SharedPtr client, std::string srv_name) { @@ -118,12 +126,17 @@ void RedundancyRelayManager::requestTopicRelayControl( 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"); + 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"); + 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"); + RCLCPP_ERROR( + get_logger(), "Service timeout %s relay control: %s", srv_name.c_str(), + relay_on ? "ON" : "OFF"); } } } // namespace autoware::redundancy_relay_manager 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 index e98dbbed360a7..947c087ba0766 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -47,13 +47,14 @@ class RedundancyRelayManager : public rclcpp::Node // Clients rclcpp::CallbackGroup::SharedPtr client_relay_trajectory_group_; - rclcpp::Client::SharedPtr client_relay_trajectory_; + rclcpp::Client::SharedPtr + client_relay_trajectory_; rclcpp::CallbackGroup::SharedPtr client_relay_pose_with_covariance_group_; - rclcpp::Client::SharedPtr client_relay_pose_with_covariance_; + rclcpp::Client::SharedPtr + client_relay_pose_with_covariance_; // Callbacks - void onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg); + 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); @@ -66,7 +67,8 @@ class RedundancyRelayManager : public rclcpp::Node // Functions void requestTopicRelayControl( - const bool relay_on, const rclcpp::Client::SharedPtr client, + const bool relay_on, + const rclcpp::Client::SharedPtr client, std::string topic_name); }; } // namespace autoware::redundancy_relay_manager From 90c0eaa29f85f1baae540f167a4fc562ed651eb7 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 15 Jan 2025 00:51:19 +0900 Subject: [PATCH 10/16] feat: repalace polling_subscriber Signed-off-by: TetsuKawa --- .../package.xml | 1 + .../src/redundancy_relay_manager_node.cpp | 31 +++++++------------ .../src/redundancy_relay_manager_node.hpp | 6 ++-- 3 files changed, 16 insertions(+), 22 deletions(-) diff --git a/system/autoware_redundancy_relay_manager/package.xml b/system/autoware_redundancy_relay_manager/package.xml index 6cdce28a05ebe..713b69ef2532b 100644 --- a/system/autoware_redundancy_relay_manager/package.xml +++ b/system/autoware_redundancy_relay_manager/package.xml @@ -11,6 +11,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_universe_utils rclcpp rclcpp_components tier4_system_msgs 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 index 00069324ac5e8..86d2b9033478c 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -26,9 +26,6 @@ RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & optio node_param_.service_timeout_ms = declare_parameter("service_timeout_ms"); // Subscribers - sub_operation_mode_state_ = create_subscription( - "~/input/operation_mode/state", rclcpp::QoS{1}, - std::bind(&RedundancyRelayManager::onOperationModeState, this, std::placeholders::_1)); sub_main_election_status_ = create_subscription( "~/input/main/election/status", rclcpp::QoS{1}, std::bind(&RedundancyRelayManager::onMainElectionStatus, this, std::placeholders::_1)); @@ -50,24 +47,19 @@ RedundancyRelayManager::RedundancyRelayManager(const rclcpp::NodeOptions & optio rmw_qos_profile_services_default, client_relay_pose_with_covariance_group_); } -void RedundancyRelayManager::onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg) -{ - operation_mode_state_ = msg; -} - 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 || operation_mode_state_ == 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; + 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( @@ -90,12 +82,13 @@ void RedundancyRelayManager::onSubElectionStatus( 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 || operation_mode_state_ == 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; + 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( 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 index 947c087ba0766..14003c5165d85 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -18,6 +18,7 @@ // ROS 2 core #include +#include #include #include #include @@ -40,8 +41,8 @@ class RedundancyRelayManager : public rclcpp::Node // Params NodeParam node_param_; // Subscribers - rclcpp::Subscription::SharedPtr - sub_operation_mode_state_; + autoware::universe_utils::InterProcessPollingSubscriber + sub_operation_mode_state_{this, "~/input/operation_mode/state"}; rclcpp::Subscription::SharedPtr sub_main_election_status_; rclcpp::Subscription::SharedPtr sub_sub_election_status_; @@ -61,7 +62,6 @@ class RedundancyRelayManager : public rclcpp::Node // State bool is_relaying_; bool is_stopped_by_main_; - autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr operation_mode_state_; tier4_system_msgs::msg::ElectionStatus::SharedPtr main_election_status_; tier4_system_msgs::msg::ElectionStatus::SharedPtr sub_election_status_; From 0e5d7ad78e50b060c818c6d85c9e4bee7e38199f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 15:55:06 +0000 Subject: [PATCH 11/16] style(pre-commit): autofix --- .../src/redundancy_relay_manager_node.cpp | 12 ++++++++---- .../src/redundancy_relay_manager_node.hpp | 5 +++-- 2 files changed, 11 insertions(+), 6 deletions(-) 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 index 86d2b9033478c..a733ee073b499 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.cpp @@ -58,8 +58,10 @@ void RedundancyRelayManager::onMainElectionStatus( 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; + 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( @@ -87,8 +89,10 @@ void RedundancyRelayManager::onSubElectionStatus( 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; + 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( 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 index 14003c5165d85..8cc342709eb18 100644 --- a/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp +++ b/system/autoware_redundancy_relay_manager/src/redundancy_relay_manager_node.hpp @@ -16,9 +16,9 @@ #define REDUNDANCY_RELAY_MANAGER_NODE_HPP_ // ROS 2 core +#include #include -#include #include #include #include @@ -41,7 +41,8 @@ class RedundancyRelayManager : public rclcpp::Node // Params NodeParam node_param_; // Subscribers - autoware::universe_utils::InterProcessPollingSubscriber + 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_; From 5d6ae9d489d388b4301731be7461e6235c00026a Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 15 Jan 2025 00:58:45 +0900 Subject: [PATCH 12/16] feat: add schema Signed-off-by: TetsuKawa --- .../schema/redundancy_relay_manager.schema.json | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) 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 index f9795fb4b87ea..3b2fdd7f37af1 100644 --- a/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json +++ b/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json @@ -5,8 +5,16 @@ "definitions": { "topic_rely_controller": { "type": "object", - "properties": {}, - "required": [], + "properties": { + "service_timeout_ms": { + "type": "integer", + "description": "Timeout for service call in milliseconds.", + "default": 10 + } + }, + "required": [ + "service_timeout_ms" + ], "additionalProperties": false } }, From 5fe98fd801918df802ed574097d16c5732546e96 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 16:02:20 +0000 Subject: [PATCH 13/16] style(pre-commit): autofix --- .../schema/redundancy_relay_manager.schema.json | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 index 3b2fdd7f37af1..2382a8bc7d06b 100644 --- a/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json +++ b/system/autoware_redundancy_relay_manager/schema/redundancy_relay_manager.schema.json @@ -12,9 +12,7 @@ "default": 10 } }, - "required": [ - "service_timeout_ms" - ], + "required": ["service_timeout_ms"], "additionalProperties": false } }, From c6f0722b52364700b507f798db4b49b53902da2c Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Mon, 20 Jan 2025 11:13:46 +0900 Subject: [PATCH 14/16] feat: add maintainer Signed-off-by: TetsuKawa --- system/autoware_redundancy_relay_manager/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/autoware_redundancy_relay_manager/package.xml b/system/autoware_redundancy_relay_manager/package.xml index 713b69ef2532b..43effd396ed6c 100644 --- a/system/autoware_redundancy_relay_manager/package.xml +++ b/system/autoware_redundancy_relay_manager/package.xml @@ -5,6 +5,7 @@ 0.1.0 The redundancy_relay_manger ROS 2 package Tetsuhiro Kawaguchi + Makoto Kurihara Apache License 2.0 ament_cmake_auto From b95ec87f04b7e4b4d0608591e3ef60f0b5e3d5d0 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Mon, 20 Jan 2025 12:19:05 +0900 Subject: [PATCH 15/16] feat: add readme Signed-off-by: TetsuKawa --- .../README.md | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/system/autoware_redundancy_relay_manager/README.md b/system/autoware_redundancy_relay_manager/README.md index 0e0208a8cf09a..9c743470601e0 100644 --- a/system/autoware_redundancy_relay_manager/README.md +++ b/system/autoware_redundancy_relay_manager/README.md @@ -1,15 +1,33 @@ -# redundancy_relay_manger - ## 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 -TBD. +- 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. From d8d44a736792f5d4e0ba3b94c60eb0c29ae3ada2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 20 Jan 2025 04:17:22 +0000 Subject: [PATCH 16/16] style(pre-commit): autofix --- .../README.md | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/system/autoware_redundancy_relay_manager/README.md b/system/autoware_redundancy_relay_manager/README.md index 9c743470601e0..526eb995cb285 100644 --- a/system/autoware_redundancy_relay_manager/README.md +++ b/system/autoware_redundancy_relay_manager/README.md @@ -6,24 +6,24 @@ The `redundancy_relay_manager` node subscribes to the election status topics fro ### 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. | +| 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. | +| 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. | +| Name | Type | Default Value | Description | +| -------------------- | ----- | ------------- | ----------------------------------------------------- | +| `service_timeout_ms` | `int` | `1000` | Timeout duration (in milliseconds) for service calls. | ## Assumptions / Known limits