diff --git a/system/redundancy_switcher_interface/CMakeLists.txt b/system/redundancy_switcher_interface/CMakeLists.txt new file mode 100644 index 0000000000000..708165efb5366 --- /dev/null +++ b/system/redundancy_switcher_interface/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) +project(redundancy_switcher_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +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 +) + +target_include_directories(common_converter PRIVATE + src/common/converter +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/node/redundancy_switcher_interface.cpp +) +target_include_directories(${PROJECT_NAME} PRIVATE src/common/converter) + +target_link_libraries(${PROJECT_NAME} common_converter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "redundancy_switcher_interface::RedundancySwitcherInterface" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + +install(PROGRAMS + script/relay_to_sub.py + DESTINATION lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ +) + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/redundancy_switcher_interface/README.md b/system/redundancy_switcher_interface/README.md new file mode 100644 index 0000000000000..87d8aa3c9c6b2 --- /dev/null +++ b/system/redundancy_switcher_interface/README.md @@ -0,0 +1,50 @@ +# redundancy_switcher_interface + +## Overview + +The redundancy switcher interface node is responsible for relaying UDP packets and ROS2 topics between the redundancy_switcher invoked by systemd and Autoware executed on ROS2. + +## availability converter + +The availability converter subscribes `/system/operation_mode/availability` and `/vehicle/status/mrm_state`, adds them together into a structure called `Availability` and sends it as a udp packet. + +### Interface + +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------------- | ------------------------------------------------- | ----------------------------- | +| subscriber | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability` | Usable behavior of the ego. | +| subscriber | `/vehicle/status/mrm_state` | `autoware_vehicle_msgs/msg/ControlModeReport` | Ego control mode. | +| udp sender | none | `struct Availability` | Combination of the above two. | + +## mrm converter + +The mrm converter subscribes `/system/fail_safe/mrm_state` into a structure called `MrmState` and sends it as a UDP packet. +In addition, it receives a udp packet`MrmState` and publish `/system/mrm_request`. + +### Interface + +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------ | ------------------------------------- | ------------------------ | +| subscriber | `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs/msg/MrmState` | MRM status of each ECU. | +| udp sender | none | `struct MrmState` | Same as above. | +| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. | +| udp receiver | none | `struct MrmRequest` | Same as above. | + +## log converter + +The log　converter receive udp packets into a structure called `ElectionCommunication` and `ElectionStatus`, and publish `/system/election/communication`, +`/system/election/status`, and `/system/fail_safe/over_all/mrm_state`. + +### Interface + +| Interface Type | Interface Name | Data Type | Description | +| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------ | +| udp receiver | none | `struct ElectionCommunication` | messages among election nodes. | +| udp receiver | none | `struct ElectionStatus` | Leader Election status. | +| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages among election nodes. | +| publisher | `/system/election/status` | `autoware_adapi_v1_msgs/msg/MrmState` | Leader Election status. | +| publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wide MRM status. | + +## Parameters + +{{ json_to_markdown("system/redundancy_switcher_interface/schema/redundancy_switcher_interface.schema.json") }} diff --git a/system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml b/system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml new file mode 100644 index 0000000000000..5269a558cd107 --- /dev/null +++ b/system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + availability_dest_ip: "127.0.0.1" + availability_dest_port: "9000" + mrm_state_dest_ip: "127.0.0.1" + mrm_state_dest_port: "9001" + mrm_request_src_ip: "127.0.0.1" + mrm_request_src_port: "9002" + election_communication_src_ip: "127.0.0.1" + election_communication_src_port: "9003" + election_status_src_ip: "127.0.0.1" + election_status_src_port: "9004" diff --git a/system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml b/system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml new file mode 100644 index 0000000000000..ca93400e4cee8 --- /dev/null +++ b/system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/system/redundancy_switcher_interface/package.xml b/system/redundancy_switcher_interface/package.xml new file mode 100644 index 0000000000000..095bc11194b0f --- /dev/null +++ b/system/redundancy_switcher_interface/package.xml @@ -0,0 +1,27 @@ + + + + redundancy_switcher_interface + 0.1.0 + The redundancy switcher interface package + TetsuKawa + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_planning_msgs + autoware_vehicle_msgs + geometry_msgs + rclcpp + rclcpp_components + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/redundancy_switcher_interface/schema/redundancy_switcher_interface.schema.json b/system/redundancy_switcher_interface/schema/redundancy_switcher_interface.schema.json new file mode 100644 index 0000000000000..065e9fdc65346 --- /dev/null +++ b/system/redundancy_switcher_interface/schema/redundancy_switcher_interface.schema.json @@ -0,0 +1,89 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for redundancy switcher interface", + "type": "object", + "definitions": { + "redundancy_switcher_interface": { + "type": "object", + "properties": { + "availability_dest_ip": { + "type": "string", + "description": "IP address of the destination of availability", + "default": "127.0.0.1" + }, + "availability_dest_port": { + "type": "string", + "description": "Port of the destination of availability", + "default": "9000" + }, + "mrm_state_dest_ip": { + "type": "string", + "description": "IP address of the destination of mrm_state", + "default": "127.0.0.1" + }, + "mrm_state_dest_port": { + "type": "string", + "description": "Port of the destination of mrm_state", + "default": "9001" + }, + "mrm_request_src_ip": { + "type": "string", + "description": "IP address of the source of mrm_request", + "default": "127.0.0.1" + }, + "mrm_request_src_port": { + "type": "string", + "description": "Port of the source of mrm_request", + "default": "9002" + }, + "election_communication_src_ip": { + "type": "string", + "description": "IP address of the source of election_communication", + "default": "127.0.0.1" + }, + "election_communication_src_port": { + "type": "string", + "description": "Port of the source of election_communication", + "default": "9003" + }, + "election_status_src_ip": { + "type": "string", + "description": "IP address of the source of election_status", + "default": "127.0.0.1" + }, + "election_status_src_port": { + "type": "string", + "description": "Port of the source of election_status", + "default": "9004" + } + }, + "required": [ + "availability_dest_ip", + "availability_dest_port", + "mrm_state_dest_ip", + "mrm_state_dest_port", + "mrm_request_src_ip", + "mrm_request_src_port", + "election_communication_src_ip", + "election_communication_src_port", + "election_status_src_ip", + "election_status_src_port" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/redundancy_switcher_interface" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/system/redundancy_switcher_interface/script/relay_to_sub.py b/system/redundancy_switcher_interface/script/relay_to_sub.py new file mode 100644 index 0000000000000..f0ee9c8b05083 --- /dev/null +++ b/system/redundancy_switcher_interface/script/relay_to_sub.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 + +from autoware_adapi_v1_msgs.msg import OperationModeState +from autoware_planning_msgs.msg import Trajectory +from geometry_msgs.msg import PoseWithCovarianceStamped +import rclpy +from rclpy.node import Node +from tier4_system_msgs.msg import OperationModeAvailability + + +class RemapNode(Node): + def __init__(self): + super().__init__("remap_node") + + self.create_subscription( + OperationModeAvailability, + "/system/operation_mode/availability", + self.operation_mode_callback, + 10, + ) + self.create_subscription( + OperationModeState, + "/system/operation_mode/state", + self.operation_mode_state_callback, + 10, + ) + self.sub_trajectory = self.create_subscription( + Trajectory, "/planning/scenario_planning/trajectory", self.trajectory_callback, 10 + ) + self.sub_pose_with_covariance = self.create_subscription( + PoseWithCovarianceStamped, "/localization/pose_with_covariance", self.pose_callback, 10 + ) + # self.sub_initialpose3d = self.create_subscription(PoseWithCovarianceStamped, '/initialpose3d', self.initialpose_callback, 10) + + self.pub_trajectory = self.create_publisher( + Trajectory, "/to_sub/planning/scenario_planning/trajectory", 10 + ) + self.pub_pose_with_covariance = self.create_publisher( + PoseWithCovarianceStamped, "/to_sub/localization/pose_with_covariance", 10 + ) + # self.pub_initialpose3d = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/initialpose3d', 10) + + self.autonomous_mode = False + self.operation_mode_autonomous_state = False + self.get_logger().info(f"Initial autonomous mode: {self.autonomous_mode}") + self.tmp_operation_mode_autonomous_state = False + + def operation_mode_callback(self, msg): + if msg.autonomous != self.autonomous_mode: + self.autonomous_mode = msg.autonomous + self.get_logger().info(f"Autonomous mode changed: {self.autonomous_mode}") + + def operation_mode_state_callback(self, msg): + self.tmp_operation_mode_autonomous_state = self.operation_mode_autonomous_state + if msg.mode == 2: + self.operation_mode_autonomous_state = True + if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state: + self.get_logger().info( + f"Operation mode changed: {self.operation_mode_autonomous_state}" + ) + else: + self.operation_mode_autonomous_state = False + if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state: + self.get_logger().info( + f"Operation mode changed: {self.operation_mode_autonomous_state}" + ) + + def trajectory_callback(self, msg): + if self.autonomous_mode or self.operation_mode_autonomous_state == False: + self.pub_trajectory.publish(msg) + + def pose_callback(self, msg): + if self.autonomous_mode or self.operation_mode_autonomous_state == False: + self.pub_pose_with_covariance.publish(msg) + + # def initialpose_callback(self, msg): + # if self.autonomous_mode: + # self.pub_initialpose3d.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = RemapNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/system/redundancy_switcher_interface/src/common/converter/availability_converter.cpp b/system/redundancy_switcher_interface/src/common/converter/availability_converter.cpp new file mode 100644 index 0000000000000..bf68cc133be66 --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/availability_converter.cpp @@ -0,0 +1,83 @@ +// 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 "availability_converter.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace redundancy_switcher_interface +{ + +AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node) : node_(node) +{ +} + +void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port) +{ + udp_availability_sender_ = std::make_unique>(dest_ip, dest_port); +} + +void AvailabilityConverter::setSubscriber() +{ + const auto qos = rclcpp::QoS(1); + availability_callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions availability_options = rclcpp::SubscriptionOptions(); + availability_options.callback_group = availability_callback_group_; + + control_mode_callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::SubscriptionOptions control_mode_options = rclcpp::SubscriptionOptions(); + control_mode_options.callback_group = control_mode_callback_group_; + auto not_executed_callback = + []([[maybe_unused]] const typename autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr + msg) {}; + + sub_operation_mode_availability_ = + node_->create_subscription( + "~/input/operation_mode_availability", qos, + std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1), + availability_options); + + sub_control_mode_ = node_->create_subscription( + "~/input/control_mode", qos, not_executed_callback, control_mode_options); +} + +void AvailabilityConverter::convertToUdp( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg) +{ + auto control_mode_report = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool success = sub_control_mode_->take(*control_mode_report, message_info); + if (success) { + Availability availability; + availability.mode = control_mode_report->mode; + availability.stop = availability_msg->stop; + availability.autonomous = availability_msg->autonomous; + availability.local = availability_msg->local; + availability.remote = availability_msg->remote; + availability.emergency_stop = availability_msg->emergency_stop; + availability.comfortable_stop = availability_msg->comfortable_stop; + availability.pull_over = availability_msg->pull_over; + udp_availability_sender_->send(availability); + } else { + RCLCPP_ERROR_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "Failed to take control mode report"); + } +} + +} // namespace redundancy_switcher_interface diff --git a/system/redundancy_switcher_interface/src/common/converter/availability_converter.hpp b/system/redundancy_switcher_interface/src/common/converter/availability_converter.hpp new file mode 100644 index 0000000000000..e4f473088db04 --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/availability_converter.hpp @@ -0,0 +1,66 @@ +// 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 COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ +#define COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ + +#include "udp_sender.hpp" + +#include + +#include +#include + +#include +#include + +namespace redundancy_switcher_interface +{ + +struct Availability +{ + autoware_vehicle_msgs::msg::ControlModeReport::_mode_type mode; + tier4_system_msgs::msg::OperationModeAvailability::_stop_type stop; + tier4_system_msgs::msg::OperationModeAvailability::_autonomous_type autonomous; + tier4_system_msgs::msg::OperationModeAvailability::_local_type local; + tier4_system_msgs::msg::OperationModeAvailability::_remote_type remote; + tier4_system_msgs::msg::OperationModeAvailability::_emergency_stop_type emergency_stop; + tier4_system_msgs::msg::OperationModeAvailability::_comfortable_stop_type comfortable_stop; + tier4_system_msgs::msg::OperationModeAvailability::_pull_over_type pull_over; + // tier4_system_msgs::msg::OperationModeAvailability::_stop_next_bus_stop_type stop_next_bus_stop; +}; + +class AvailabilityConverter +{ +public: + explicit AvailabilityConverter(rclcpp::Node * node); + + void setUdpSender(const std::string & dest_ip, const std::string & dest_port); + void setSubscriber(); + void convertToUdp( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg); + +private: + rclcpp::Node * node_; + std::unique_ptr> udp_availability_sender_; + rclcpp::CallbackGroup::SharedPtr availability_callback_group_; + rclcpp::CallbackGroup::SharedPtr control_mode_callback_group_; + rclcpp::Subscription::SharedPtr sub_control_mode_; + rclcpp::Subscription::SharedPtr + sub_operation_mode_availability_; +}; + +} // namespace redundancy_switcher_interface + +#endif // COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ diff --git a/system/redundancy_switcher_interface/src/common/converter/log_converter.cpp b/system/redundancy_switcher_interface/src/common/converter/log_converter.cpp new file mode 100644 index 0000000000000..bf672429768d1 --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/log_converter.cpp @@ -0,0 +1,140 @@ +// 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 "log_converter.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace redundancy_switcher_interface +{ + +LogConverter::LogConverter(rclcpp::Node * node) +: node_(node), is_election_communication_running_(true), is_election_status_running_(true) +{ +} + +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(); + } + if (udp_election_status_thread_.joinable()) { + udp_election_status_thread_.join(); + } +} + +void LogConverter::setUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port) +{ + udp_election_communication_thread_ = + std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port); +} + +void LogConverter::startUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port) +{ + try { + udp_election_communication_receiver_ = std::make_unique>( + src_ip, src_port, + std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1)); + while (is_election_communication_running_) { + udp_election_communication_receiver_->receive(); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Error in UDP receiver thread: %s", e.what()); + } +} + +void LogConverter::setUdpElectionStatusReceiver( + const std::string & src_ip, const std::string & src_port) +{ + udp_election_status_thread_ = + std::thread(&LogConverter::startUdpElectionStatusReceiver, this, src_ip, src_port); +} + +void LogConverter::startUdpElectionStatusReceiver( + const std::string & src_ip, const std::string & src_port) +{ + try { + udp_election_status_receiver_ = std::make_unique>( + src_ip, src_port, + std::bind(&LogConverter::convertElectionStatusToTopic, this, std::placeholders::_1)); + while (is_election_status_running_) { + udp_election_status_receiver_->receive(); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Error in UDP receiver thread: %s", e.what()); + } +} + +void LogConverter::setPublisher() +{ + pub_election_communication_ = + node_->create_publisher( + "~/output/election_communication", rclcpp::QoS{1}); + pub_election_status_ = node_->create_publisher( + "~/output/election_status", rclcpp::QoS{1}); + pub_over_all_mrm_state_ = node_->create_publisher( + "~/output/over_all_mrm_state", rclcpp::QoS{1}); +} + +void LogConverter::convertElectionCommunicationToTopic(const ElectionCommunication & node_msg) +{ + tier4_system_msgs::msg::ElectionCommunication msg; + msg.stamp = node_->now(); + msg.node_id = (node_msg.msg >> 8) & 0xFF; + msg.type = node_msg.msg & 0xFF; + msg.term = (node_msg.msg >> 16) & 0xFF; + msg.path = (node_msg.msg >> 24) & 0x0F; + msg.link = (node_msg.msg >> 28) & 0x0F; + msg.heartbeat = (node_msg.msg >> 56) & 0x0F; + msg.checksum = (node_msg.msg >> 60) & 0x0F; + pub_election_communication_->publish(msg); +} + +void LogConverter::convertElectionStatusToTopic(const ElectionStatus & status) +{ + tier4_system_msgs::msg::ElectionStatus election_status; + autoware_adapi_v1_msgs::msg::MrmState mrm_state; + + election_status.stamp = node_->now(); + election_status.leader_id = status.leader_id; + election_status.path_info = status.path_info; + election_status.mrm_state.state = status.mrm_state; + election_status.mrm_state.behavior = status.mrm_behavior; + election_status.election_start_count = status.election_start_count; + election_status.in_election = status.in_election; + election_status.has_received_availability = status.has_received_availability; + election_status.has_received_mrm_state = status.has_received_mrm_state; + election_status.is_autoware_emergency = status.is_autoware_emergency; + election_status.is_main_ecu_connected = status.is_main_ecu_connected; + election_status.is_sub_ecu_connected = status.is_sub_ecu_connected; + election_status.is_main_vcu_connected = status.is_main_vcu_connected; + election_status.is_sub_vcu_connected = status.is_sub_vcu_connected; + pub_election_status_->publish(election_status); + + mrm_state.stamp = node_->now(); + mrm_state.state = status.mrm_state; + mrm_state.behavior = status.mrm_behavior; + pub_over_all_mrm_state_->publish(mrm_state); +} + +} // namespace redundancy_switcher_interface diff --git a/system/redundancy_switcher_interface/src/common/converter/log_converter.hpp b/system/redundancy_switcher_interface/src/common/converter/log_converter.hpp new file mode 100644 index 0000000000000..14c31c313e6c1 --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/log_converter.hpp @@ -0,0 +1,91 @@ +// 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 COMMON__CONVERTER__LOG_CONVERTER_HPP_ +#define COMMON__CONVERTER__LOG_CONVERTER_HPP_ + +#include "udp_receiver.hpp" +#include "udp_sender.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace redundancy_switcher_interface +{ + +typedef struct ElectionCommunication +{ + uint64_t msg; +} ElectionCommunication; + +typedef struct ElectionStatus +{ + tier4_system_msgs::msg::ElectionStatus::_leader_id_type leader_id; + tier4_system_msgs::msg::ElectionStatus::_path_info_type path_info; + autoware_adapi_v1_msgs::msg::MrmState::_state_type mrm_state; + autoware_adapi_v1_msgs::msg::MrmState::_behavior_type mrm_behavior; + tier4_system_msgs::msg::ElectionStatus::_election_start_count_type election_start_count; + tier4_system_msgs::msg::ElectionStatus::_in_election_type in_election; + tier4_system_msgs::msg::ElectionStatus::_has_received_availability_type has_received_availability; + tier4_system_msgs::msg::ElectionStatus::_has_received_mrm_state_type has_received_mrm_state; + tier4_system_msgs::msg::ElectionStatus::_is_autoware_emergency_type is_autoware_emergency; + tier4_system_msgs::msg::ElectionStatus::_is_main_ecu_connected_type is_main_ecu_connected; + tier4_system_msgs::msg::ElectionStatus::_is_sub_ecu_connected_type is_sub_ecu_connected; + tier4_system_msgs::msg::ElectionStatus::_is_main_vcu_connected_type is_main_vcu_connected; + tier4_system_msgs::msg::ElectionStatus::_is_sub_vcu_connected_type is_sub_vcu_connected; +} ElectionStatus; + +class LogConverter +{ +public: + explicit LogConverter(rclcpp::Node * node); + ~LogConverter(); + + void setUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port); + void setUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port); + void setPublisher(); + +private: + void startUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port); + void startUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port); + void convertElectionCommunicationToTopic(const ElectionCommunication & node_msg); + void convertElectionStatusToTopic(const ElectionStatus & status); + + rclcpp::Node * node_; + std::unique_ptr> udp_election_communication_receiver_; + std::unique_ptr> udp_election_status_receiver_; + rclcpp::Publisher::SharedPtr + pub_election_communication_; + rclcpp::Publisher::SharedPtr pub_election_status_; + rclcpp::Publisher::SharedPtr pub_over_all_mrm_state_; + + std::thread udp_election_communication_thread_; + std::thread udp_election_status_thread_; + std::atomic is_election_communication_running_; + std::atomic is_election_status_running_; +}; + +} // namespace redundancy_switcher_interface + +#endif // COMMON__CONVERTER__LOG_CONVERTER_HPP_ diff --git a/system/redundancy_switcher_interface/src/common/converter/mrm_converter.cpp b/system/redundancy_switcher_interface/src/common/converter/mrm_converter.cpp new file mode 100644 index 0000000000000..3be4cce6d8d7f --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/mrm_converter.cpp @@ -0,0 +1,98 @@ +// 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 "mrm_converter.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace redundancy_switcher_interface +{ + +MrmConverter::MrmConverter(rclcpp::Node * node) : node_(node), is_udp_receiver_running_(true) +{ +} + +MrmConverter::~MrmConverter() +{ + is_udp_receiver_running_ = false; + udp_mrm_request_receiver_->~UdpReceiver(); + if (udp_receiver_thread_.joinable()) { + udp_receiver_thread_.join(); + } +} + +void MrmConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port) +{ + udp_mrm_state_sender_ = std::make_unique>(dest_ip, dest_port); +} + +void MrmConverter::setUdpReceiver(const std::string & src_ip, const std::string & src_port) +{ + udp_receiver_thread_ = std::thread(&MrmConverter::startUdpReceiver, this, src_ip, src_port); +} + +void MrmConverter::startUdpReceiver(const std::string & src_ip, const std::string & src_port) +{ + try { + udp_mrm_request_receiver_ = std::make_unique>( + src_ip, src_port, std::bind(&MrmConverter::convertToTopic, this, std::placeholders::_1)); + while (is_udp_receiver_running_) { + udp_mrm_request_receiver_->receive(); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Error in UDP receiver thread: %s", e.what()); + } +} + +void MrmConverter::setSubscriber() +{ + const auto qos = rclcpp::QoS(1); + callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions options; + options.callback_group = callback_group_; + + sub_mrm_state_ = node_->create_subscription( + "~/input/mrm_state", qos, std::bind(&MrmConverter::convertToUdp, this, std::placeholders::_1), + options); +} + +void MrmConverter::setPublisher() +{ + pub_mrm_request_ = node_->create_publisher( + "~/output/mrm_request", rclcpp::QoS{1}); +} + +void MrmConverter::convertToUdp( + const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg) +{ + MrmState mrm_state; + mrm_state.state = mrm_state_msg->state; + mrm_state.behavior = mrm_state_msg->behavior; + + udp_mrm_state_sender_->send(mrm_state); +} + +void MrmConverter::convertToTopic(const MrmRequest & mrm_request) +{ + tier4_system_msgs::msg::MrmBehavior mrm_request_msg; + mrm_request_msg.stamp = node_->now(); + mrm_request_msg.type = mrm_request.behavior; + + pub_mrm_request_->publish(mrm_request_msg); +} + +} // namespace redundancy_switcher_interface diff --git a/system/redundancy_switcher_interface/src/common/converter/mrm_converter.hpp b/system/redundancy_switcher_interface/src/common/converter/mrm_converter.hpp new file mode 100644 index 0000000000000..71b168d0cdb6d --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/mrm_converter.hpp @@ -0,0 +1,74 @@ +// 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 COMMON__CONVERTER__MRM_CONVERTER_HPP_ +#define COMMON__CONVERTER__MRM_CONVERTER_HPP_ + +#include "udp_receiver.hpp" +#include "udp_sender.hpp" + +#include + +#include +#include + +#include +#include +#include +#include + +namespace redundancy_switcher_interface +{ + +typedef struct MrmState +{ + autoware_adapi_v1_msgs::msg::MrmState::_state_type state; + autoware_adapi_v1_msgs::msg::MrmState::_behavior_type behavior; +} MrmState; + +typedef struct MrmRequest +{ + tier4_system_msgs::msg::MrmBehavior::_type_type behavior; +} MrmRequest; + +class MrmConverter +{ +public: + explicit MrmConverter(rclcpp::Node * node); + ~MrmConverter(); + + 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 setSubscriber(); + void setPublisher(); + +private: + void startUdpReceiver(const std::string & src_ip, const std::string & src_port); + void convertToUdp(const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg); + void convertToTopic(const MrmRequest & mrm_request); + + rclcpp::Node * node_; + std::unique_ptr> udp_mrm_state_sender_; + std::unique_ptr> udp_mrm_request_receiver_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; + rclcpp::Publisher::SharedPtr pub_mrm_request_; + + std::thread udp_receiver_thread_; + std::atomic is_udp_receiver_running_; +}; + +} // namespace redundancy_switcher_interface + +#endif // COMMON__CONVERTER__MRM_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 new file mode 100644 index 0000000000000..6ad799531b12e --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/udp_receiver.hpp @@ -0,0 +1,151 @@ + +// 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 COMMON__CONVERTER__UDP_RECEIVER_HPP_ +#define COMMON__CONVERTER__UDP_RECEIVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace redundancy_switcher_interface +{ + +template +class UdpReceiver +{ +public: + using CallbackType = std::function; + UdpReceiver(const std::string & ip, const std::string & port); + UdpReceiver(const std::string & ip, const std::string & port, CallbackType callback); + UdpReceiver(const std::string & ip, const std::string & port, bool is_non_blocking); + UdpReceiver( + const std::string & ip, const std::string & port, bool is_non_blocking, CallbackType callback); + ~UdpReceiver(); + + bool receive(T & data); // for non callback + void receive(); // for callback + +private: + int socketfd_; + struct addrinfo * res_; + CallbackType callback_; + + void setCallback(CallbackType callback); +}; + +template +UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port) +{ + struct addrinfo hints; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + + if (getaddrinfo(ip.c_str(), port.c_str(), &hints, &res_) != 0) { + throw std::runtime_error("getaddrinfo failed"); + } + + socketfd_ = socket(res_->ai_family, res_->ai_socktype, res_->ai_protocol); + if (socketfd_ < 0) { + freeaddrinfo(res_); + throw std::runtime_error("socket failed"); + } + + if (bind(socketfd_, res_->ai_addr, res_->ai_addrlen) < 0) { + freeaddrinfo(res_); + close(socketfd_); + throw std::runtime_error("bind failed"); + } +} + +template +UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port, CallbackType callback) +: UdpReceiver(ip, port) +{ + setCallback(callback); +} + +template +UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port, bool is_non_blocking) +: UdpReceiver(ip, port) +{ + if (is_non_blocking) { + if (fcntl(socketfd_, F_SETFL, O_NONBLOCK) < 0) { + freeaddrinfo(res_); + close(socketfd_); + throw std::runtime_error("fcntl failed"); + } + } +} + +template +UdpReceiver::UdpReceiver( + const std::string & ip, const std::string & port, bool is_non_blocking, CallbackType callback) +: UdpReceiver(ip, port, is_non_blocking) +{ + setCallback(callback); +} + +template +UdpReceiver::~UdpReceiver() +{ + shutdown(socketfd_, SHUT_RDWR); + freeaddrinfo(res_); + close(socketfd_); +} + +template +void UdpReceiver::setCallback(CallbackType callback) +{ + callback_ = callback; +} + +template +bool UdpReceiver::receive(T & data) +{ + 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; + } + throw std::runtime_error("recvfrom failed"); + } + return true; +} + +template +void UdpReceiver::receive() +{ + T data; + if (receive(data) && callback_) { + callback_(data); + } +} + +} // namespace redundancy_switcher_interface + +#endif // COMMON__CONVERTER__UDP_RECEIVER_HPP_ diff --git a/system/redundancy_switcher_interface/src/common/converter/udp_sender.hpp b/system/redundancy_switcher_interface/src/common/converter/udp_sender.hpp new file mode 100644 index 0000000000000..2e8877330a217 --- /dev/null +++ b/system/redundancy_switcher_interface/src/common/converter/udp_sender.hpp @@ -0,0 +1,82 @@ +// 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 COMMON__CONVERTER__UDP_SENDER_HPP_ +#define COMMON__CONVERTER__UDP_SENDER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace redundancy_switcher_interface +{ + +template +class UdpSender +{ +public: + UdpSender(const std::string & ip, const std::string & port); + ~UdpSender(); + + void send(const T & data); + +private: + int socketfd_; + struct addrinfo * res_; +}; + +template +UdpSender::UdpSender(const std::string & ip, const std::string & port) +{ + struct addrinfo hints; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + + if (getaddrinfo(ip.c_str(), port.c_str(), &hints, &res_) != 0) { + throw std::runtime_error("getaddrinfo failed"); + } + + socketfd_ = socket(res_->ai_family, res_->ai_socktype, res_->ai_protocol); + if (socketfd_ < 0) { + freeaddrinfo(res_); + throw std::runtime_error("socket failed"); + } +} + +template +UdpSender::~UdpSender() +{ + shutdown(socketfd_, SHUT_RDWR); + freeaddrinfo(res_); + close(socketfd_); +} + +template +void UdpSender::send(const T & data) +{ + if (sendto(socketfd_, &data, sizeof(T), 0, res_->ai_addr, res_->ai_addrlen) < 0) { + throw std::runtime_error("sendto failed"); + } +} + +} // namespace redundancy_switcher_interface + +#endif // COMMON__CONVERTER__UDP_SENDER_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 new file mode 100644 index 0000000000000..fea7e796fec7c --- /dev/null +++ b/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.cpp @@ -0,0 +1,62 @@ +// 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 "redundancy_switcher_interface.hpp" + +#include + +namespace redundancy_switcher_interface +{ + +RedundancySwitcherInterface::RedundancySwitcherInterface(const rclcpp::NodeOptions & node_options) +: Node("redundancy_switcher_interface", node_options), + availability_converter_(this), + mrm_converter_(this), + log_converter_(this) +{ + availability_dest_ip_ = declare_parameter("availability_dest_ip"); + availability_dest_port_ = declare_parameter("availability_dest_port"); + mrm_state_dest_ip_ = declare_parameter("mrm_state_dest_ip"); + mrm_state_dest_port_ = declare_parameter("mrm_state_dest_port"); + mrm_request_src_ip_ = declare_parameter("mrm_request_src_ip"); + mrm_request_src_port_ = declare_parameter("mrm_request_src_port"); + election_communication_src_ip_ = declare_parameter("election_communication_src_ip"); + election_communication_src_port_ = + 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"); + + // convert udp packets of availability to topics + availability_converter_.setUdpSender(availability_dest_ip_, availability_dest_port_); + availability_converter_.setSubscriber(); + + // convert topics of mrm state to udp packets + mrm_converter_.setUdpSender(mrm_state_dest_ip_, mrm_state_dest_port_); + mrm_converter_.setSubscriber(); + + // convert udp packets of mrm request to topics + mrm_converter_.setPublisher(); + mrm_converter_.setUdpReceiver(mrm_request_src_ip_, mrm_request_src_port_); + + // convert udp packets of election info to topics + log_converter_.setPublisher(); + log_converter_.setUdpElectionCommunicationReceiver( + election_communication_src_ip_, election_communication_src_port_); + log_converter_.setUdpElectionStatusReceiver(election_status_src_ip_, election_status_src_port_); +} + +} // namespace redundancy_switcher_interface + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(redundancy_switcher_interface::RedundancySwitcherInterface) diff --git a/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.hpp b/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.hpp new file mode 100644 index 0000000000000..3b2846387c6d4 --- /dev/null +++ b/system/redundancy_switcher_interface/src/node/redundancy_switcher_interface.hpp @@ -0,0 +1,54 @@ +// 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 NODE__REDUNDANCY_SWITCHER_INTERFACE_HPP_ +#define NODE__REDUNDANCY_SWITCHER_INTERFACE_HPP_ + +#include "availability_converter.hpp" +#include "log_converter.hpp" +#include "mrm_converter.hpp" + +#include + +#include +#include + +namespace redundancy_switcher_interface +{ + +class RedundancySwitcherInterface : public rclcpp::Node +{ +public: + explicit RedundancySwitcherInterface(const rclcpp::NodeOptions & node_options); + +private: + std::string availability_dest_ip_; + std::string availability_dest_port_; + std::string mrm_state_dest_ip_; + std::string mrm_state_dest_port_; + std::string mrm_request_src_ip_; + std::string mrm_request_src_port_; + std::string election_communication_src_ip_; + std::string election_communication_src_port_; + std::string election_status_src_ip_; + std::string election_status_src_port_; + + AvailabilityConverter availability_converter_; + MrmConverter mrm_converter_; + LogConverter log_converter_; +}; + +} // namespace redundancy_switcher_interface + +#endif // NODE__REDUNDANCY_SWITCHER_INTERFACE_HPP_