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