feat(redundancy_switcher_interface): add reset_converter

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

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

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"

system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml

@@ -7,6 +7,7 @@
   <arg name="output_over_all_mrm_state" default="/system/fail_safe/over_all/mrm_state"/>
   <arg name="output_election_communication" default="/system/election/communication"/>
   <arg name="output_election_status" default="/system/election/status"/>
+  <arg name="service_reset" default="/system/election/reset"/>
   <node pkg="redundancy_switcher_interface" exec="redundancy_switcher_interface_node">
     <param from="$(var param_file)"/>
     <remap from="~/input/control_mode" to="$(var input_control_mode)"/>
@@ -16,5 +17,6 @@
     <remap from="~/output/over_all_mrm_state" to="$(var output_over_all_mrm_state)"/>
     <remap from="~/output/election_communication" to="$(var output_election_communication)"/>
     <remap from="~/output/election_status" to="$(var output_election_status)"/>
+    <remap from="~/service/reset" to="$(var service_reset)"/>
   </node>
 </launch>

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();
   }

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();
   }

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 <autoware_adapi_v1_msgs/msg/response_status.hpp>
+
+#include <memory>
+#include <string>
+
+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<UdpSender<ResetRequest>>(dest_ip, dest_port);
+}
+
+void ResetConverter::setUdpReceiver(const std::string & src_ip, const std::string & src_port)
+{
+  udp_reset_response_receiver_ = std::make_unique<UdpReceiver<ResetResponse>>(src_ip, src_port, false);
+}
+
+void ResetConverter::setService()
+{
+  srv_reset_ = node_->create_service<autoware_adapi_v1_msgs::srv::RedundancySwitcherReset>(
+    "~/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

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/srv/redundancy_switcher_reset.hpp>
+
+#include <memory>
+#include <string>
+
+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<UdpSender<ResetRequest>> udp_reset_request_sender_;
+  std::unique_ptr<UdpReceiver<ResetResponse>> udp_reset_response_receiver_;
+  rclcpp::Service<autoware_adapi_v1_msgs::srv::RedundancySwitcherReset>::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_

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 <typename T>
@@ -123,18 +125,30 @@ void UdpReceiver<T>::setCallback(CallbackType callback)
 }
 
 template <typename T>
-bool UdpReceiver<T>::receive(T & data)
+bool UdpReceiver<T>::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 <typename T>
+bool UdpReceiver<T>::receive(T & data)
+{
+  return receive(data, 0);
 }
 
 template <typename T>
@@ -146,6 +160,21 @@ void UdpReceiver<T>::receive()
   }
 }
 
+template <typename T>
+bool UdpReceiver<T>::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_

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<std::string>("availability_dest_ip");
   availability_dest_port_ = declare_parameter<std::string>("availability_dest_port");
@@ -36,6 +37,10 @@ RedundancySwitcherInterface::RedundancySwitcherInterface(const rclcpp::NodeOptio
     declare_parameter<std::string>("election_communication_src_port");
   election_status_src_ip_ = declare_parameter<std::string>("election_status_src_ip");
   election_status_src_port_ = declare_parameter<std::string>("election_status_src_port");
+  reset_request_dest_ip_ = declare_parameter<std::string>("reset_request_dest_ip");
+  reset_request_dest_port_ = declare_parameter<std::string>("reset_request_dest_port");
+  reset_response_src_ip_ = declare_parameter<std::string>("reset_response_src_ip");
+  reset_response_src_port_ = declare_parameter<std::string>("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

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 <rclcpp/rclcpp.hpp>
 
@@ -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