From ed77394f56b3e9fd30523917726d73d362594927 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 25 Dec 2024 17:16:12 +0900 Subject: [PATCH] feat(redundancy_switcher_interface): add reset_converter Signed-off-by: TetsuKawa --- .../CMakeLists.txt | 1 + .../redundancy_switcher_interface.param.yaml | 14 ++-- .../redundancy_switcher_interface.launch.xml | 2 + .../src/common/converter/log_converter.cpp | 3 +- .../src/common/converter/mrm_converter.cpp | 2 +- .../src/common/converter/reset_converter.cpp | 84 +++++++++++++++++++ .../src/common/converter/reset_converter.hpp | 63 ++++++++++++++ .../src/common/converter/udp_receiver.hpp | 45 ++++++++-- .../node/redundancy_switcher_interface.cpp | 12 ++- .../node/redundancy_switcher_interface.hpp | 6 ++ 10 files changed, 215 insertions(+), 17 deletions(-) create mode 100644 system/redundancy_switcher_interface/src/common/converter/reset_converter.cpp create mode 100644 system/redundancy_switcher_interface/src/common/converter/reset_converter.hpp diff --git a/system/redundancy_switcher_interface/CMakeLists.txt b/system/redundancy_switcher_interface/CMakeLists.txt index 708165efb5366..48e85c0c3fcd9 100644 --- a/system/redundancy_switcher_interface/CMakeLists.txt +++ b/system/redundancy_switcher_interface/CMakeLists.txt @@ -8,6 +8,7 @@ ament_auto_add_library(common_converter SHARED src/common/converter/availability_converter.cpp src/common/converter/mrm_converter.cpp src/common/converter/log_converter.cpp + src/common/converter/reset_converter.cpp ) target_include_directories(common_converter PRIVATE diff --git a/system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml b/system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml index 5269a558cd107..f3a2ae80a79cc 100644 --- a/system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml +++ b/system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml @@ -1,12 +1,16 @@ /**: ros__parameters: availability_dest_ip: "127.0.0.1" - availability_dest_port: "9000" + availability_dest_port: "59000" mrm_state_dest_ip: "127.0.0.1" - mrm_state_dest_port: "9001" + mrm_state_dest_port: "59001" mrm_request_src_ip: "127.0.0.1" - mrm_request_src_port: "9002" + mrm_request_src_port: "59002" election_communication_src_ip: "127.0.0.1" - election_communication_src_port: "9003" + election_communication_src_port: "59003" election_status_src_ip: "127.0.0.1" - election_status_src_port: "9004" + election_status_src_port: "59004" + reset_request_dest_ip: "127.0.0.1" + reset_request_dest_port: "59005" + reset_response_src_ip: "127.0.0.1" + reset_response_src_port: "59006" diff --git a/system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml b/system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml index ca93400e4cee8..ccf510c82a9a3 100644 --- a/system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml +++ b/system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml @@ -7,6 +7,7 @@ + @@ -16,5 +17,6 @@ + diff --git a/system/redundancy_switcher_interface/src/common/converter/log_converter.cpp b/system/redundancy_switcher_interface/src/common/converter/log_converter.cpp index bf672429768d1..508bf393aa49c 100644 --- a/system/redundancy_switcher_interface/src/common/converter/log_converter.cpp +++ b/system/redundancy_switcher_interface/src/common/converter/log_converter.cpp @@ -30,9 +30,8 @@ LogConverter::LogConverter(rclcpp::Node * node) LogConverter::~LogConverter() { is_election_communication_running_ = false; - udp_election_communication_receiver_->~UdpReceiver(); is_election_status_running_ = false; - udp_election_status_receiver_->~UdpReceiver(); + if (udp_election_communication_thread_.joinable()) { udp_election_communication_thread_.join(); } diff --git a/system/redundancy_switcher_interface/src/common/converter/mrm_converter.cpp b/system/redundancy_switcher_interface/src/common/converter/mrm_converter.cpp index 3be4cce6d8d7f..a675495f06c67 100644 --- a/system/redundancy_switcher_interface/src/common/converter/mrm_converter.cpp +++ b/system/redundancy_switcher_interface/src/common/converter/mrm_converter.cpp @@ -29,7 +29,7 @@ MrmConverter::MrmConverter(rclcpp::Node * node) : node_(node), is_udp_receiver_r MrmConverter::~MrmConverter() { is_udp_receiver_running_ = false; - udp_mrm_request_receiver_->~UdpReceiver(); + if (udp_receiver_thread_.joinable()) { udp_receiver_thread_.join(); } diff --git a/system/redundancy_switcher_interface/src/common/converter/reset_converter.cpp b/system/redundancy_switcher_interface/src/common/converter/reset_converter.cpp new file mode 100644 index 0000000000000..acede492447f6 --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/reset_converter.cpp @@ -0,0 +1,84 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "reset_converter.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include + +#include +#include + +namespace redundancy_switcher_interface +{ + +ResetConverter::ResetConverter(rclcpp::Node * node) : node_(node) +{ +} + +void ResetConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port) +{ + udp_reset_request_sender_ = std::make_unique>(dest_ip, dest_port); +} + +void ResetConverter::setUdpReceiver(const std::string & src_ip, const std::string & src_port) +{ + udp_reset_response_receiver_ = std::make_unique>(src_ip, src_port, false); +} + +void ResetConverter::setService() +{ + srv_reset_ = node_->create_service( + "~/service/reset", std::bind(&ResetConverter::onResetRequest, this, std::placeholders::_1, std::placeholders::_2)); +} + +void ResetConverter::onResetRequest( + const autoware_adapi_v1_msgs::srv::RedundancySwitcherReset::Request::SharedPtr, + const autoware_adapi_v1_msgs::srv::RedundancySwitcherReset::Response::SharedPtr response) +{ + auto async_task = std::async(std::launch::async, [this, response]() { + ResetRequest reset_request; + reset_request.request = true; + udp_reset_request_sender_->send(reset_request); + RCLCPP_INFO(node_->get_logger(), "Reset request sent."); + + ResetResponse udp_response; + try { + bool result = udp_reset_response_receiver_->receive(udp_response, 30);; + if (!result) { + response->status.success = false; + response->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::SERVICE_TIMEOUT; + response->status.message = "Timed out waiting for response."; + RCLCPP_WARN(node_->get_logger(), "Timed out waiting for response."); + } else if (!udp_response.response) { + response->status.success = false; + response->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::NO_EFFECT; + response->status.message = "Failed to reset in redundancy switcher."; + RCLCPP_WARN(node_->get_logger(), "Failed to reset in redundancy switcher."); + } else { + response->status.success = true; + response->status.message = "Reset successfully."; + RCLCPP_INFO(node_->get_logger(), "Reset successfully."); + } + } catch (const std::exception &e) { + response->status.success = false; + response->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::TRANSFORM_ERROR; + response->status.message = "Failed to receive UDP response."; + RCLCPP_WARN(node_->get_logger(), "Failed to receive UDP response: %s", e.what()); + } + }); +} + +} // namespace redundancy_switcher_interface diff --git a/system/redundancy_switcher_interface/src/common/converter/reset_converter.hpp b/system/redundancy_switcher_interface/src/common/converter/reset_converter.hpp new file mode 100644 index 0000000000000..4b140884f4692 --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/reset_converter.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 RESET__CONVERTER__RESET_CONVERTER_HPP_ +#define RESET__CONVERTER__RESET_CONVERTER_HPP_ + +#include "udp_sender.hpp" +#include "udp_receiver.hpp" + +#include + +#include + +#include +#include + +namespace redundancy_switcher_interface +{ + +struct ResetRequest +{ + bool request; +}; + +struct ResetResponse +{ + bool response; +}; + +class ResetConverter +{ +public: + explicit ResetConverter(rclcpp::Node * node); + + void setUdpSender(const std::string & dest_ip, const std::string & dest_port); + void setUdpReceiver(const std::string & src_ip, const std::string & src_port); + void setService(); + +private: + rclcpp::Node * node_; + std::unique_ptr> udp_reset_request_sender_; + std::unique_ptr> udp_reset_response_receiver_; + rclcpp::Service::SharedPtr srv_reset_; + + void onResetRequest( + const autoware_adapi_v1_msgs::srv::RedundancySwitcherReset::Request::SharedPtr, + const autoware_adapi_v1_msgs::srv::RedundancySwitcherReset::Response::SharedPtr response); +}; + +} // namespace redundancy_switcher_interface + +#endif // RESET__CONVERTER__RESET_CONVERTER_HPP_ diff --git a/system/redundancy_switcher_interface/src/common/converter/udp_receiver.hpp b/system/redundancy_switcher_interface/src/common/converter/udp_receiver.hpp index 6ad799531b12e..efd9432cc1190 100644 --- a/system/redundancy_switcher_interface/src/common/converter/udp_receiver.hpp +++ b/system/redundancy_switcher_interface/src/common/converter/udp_receiver.hpp @@ -44,7 +44,8 @@ class UdpReceiver const std::string & ip, const std::string & port, bool is_non_blocking, CallbackType callback); ~UdpReceiver(); - bool receive(T & data); // for non callback + bool receive(T & data, int timeout); // for non callback and timeout + bool receive(T & data); // for non callback void receive(); // for callback private: @@ -53,6 +54,7 @@ class UdpReceiver CallbackType callback_; void setCallback(CallbackType callback); + bool has_received_udp_date(int timeout); }; template @@ -123,18 +125,30 @@ void UdpReceiver::setCallback(CallbackType callback) } template -bool UdpReceiver::receive(T & data) +bool UdpReceiver::receive(T & data, int timeout) { struct sockaddr_storage addr; socklen_t addr_len = sizeof(addr); - ssize_t recv_size = recvfrom(socketfd_, &data, sizeof(T), 0, (struct sockaddr *)&addr, &addr_len); - if (recv_size < 0) { - if (errno == EAGAIN || errno == EWOULDBLOCK) { - return false; + memset(&addr, 0, sizeof(addr)); + + if (has_received_udp_date(timeout)) { + ssize_t recv_size = recvfrom(socketfd_, &data, sizeof(T), 0, (struct sockaddr *)&addr, &addr_len); + if (recv_size < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return false; + } + throw std::runtime_error("recvfrom failed"); } - throw std::runtime_error("recvfrom failed"); + return true; + } else { + return false; } - return true; +} + +template +bool UdpReceiver::receive(T & data) +{ + return receive(data, 0); } template @@ -146,6 +160,21 @@ void UdpReceiver::receive() } } +template +bool UdpReceiver::has_received_udp_date(int timeout) +{ + fd_set fds; + FD_ZERO(&fds); + FD_SET(socketfd_, &fds); + + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = timeout; + + int ret = select(socketfd_ + 1, &fds, NULL, NULL, &tv); + return ret > 0; +} + } // namespace redundancy_switcher_interface #endif // COMMON__CONVERTER__UDP_RECEIVER_HPP_ diff --git a/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.cpp b/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.cpp index fea7e796fec7c..8904474b9e0c8 100644 --- a/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.cpp +++ b/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.cpp @@ -23,7 +23,8 @@ RedundancySwitcherInterface::RedundancySwitcherInterface(const rclcpp::NodeOptio : Node("redundancy_switcher_interface", node_options), availability_converter_(this), mrm_converter_(this), - log_converter_(this) + log_converter_(this), + reset_converter_(this) { availability_dest_ip_ = declare_parameter("availability_dest_ip"); availability_dest_port_ = declare_parameter("availability_dest_port"); @@ -36,6 +37,10 @@ RedundancySwitcherInterface::RedundancySwitcherInterface(const rclcpp::NodeOptio declare_parameter("election_communication_src_port"); election_status_src_ip_ = declare_parameter("election_status_src_ip"); election_status_src_port_ = declare_parameter("election_status_src_port"); + reset_request_dest_ip_ = declare_parameter("reset_request_dest_ip"); + reset_request_dest_port_ = declare_parameter("reset_request_dest_port"); + reset_response_src_ip_ = declare_parameter("reset_response_src_ip"); + reset_response_src_port_ = declare_parameter("reset_response_src_port"); // convert udp packets of availability to topics availability_converter_.setUdpSender(availability_dest_ip_, availability_dest_port_); @@ -54,6 +59,11 @@ RedundancySwitcherInterface::RedundancySwitcherInterface(const rclcpp::NodeOptio log_converter_.setUdpElectionCommunicationReceiver( election_communication_src_ip_, election_communication_src_port_); log_converter_.setUdpElectionStatusReceiver(election_status_src_ip_, election_status_src_port_); + + // convert topics of reset request to udp packets + reset_converter_.setUdpSender(reset_request_dest_ip_, reset_request_dest_port_); + reset_converter_.setUdpReceiver(reset_response_src_ip_, reset_response_src_port_); + reset_converter_.setService(); } } // namespace redundancy_switcher_interface diff --git a/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.hpp b/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.hpp index 3b2846387c6d4..52f954ff73997 100644 --- a/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.hpp +++ b/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.hpp @@ -18,6 +18,7 @@ #include "availability_converter.hpp" #include "log_converter.hpp" #include "mrm_converter.hpp" +#include "reset_converter.hpp" #include @@ -43,10 +44,15 @@ class RedundancySwitcherInterface : public rclcpp::Node std::string election_communication_src_port_; std::string election_status_src_ip_; std::string election_status_src_port_; + std::string reset_request_dest_ip_; + std::string reset_request_dest_port_; + std::string reset_response_src_ip_; + std::string reset_response_src_port_; AvailabilityConverter availability_converter_; MrmConverter mrm_converter_; LogConverter log_converter_; + ResetConverter reset_converter_; }; } // namespace redundancy_switcher_interface