From 80930c45c0b38b205c1aa12ebc622a546ce5f610 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 25 Jun 2024 01:27:34 +0900 Subject: [PATCH 01/14] feat: add leader election converter Signed-off-by: TetsuKawa --- .../leader_election_converter/CMakeLists.txt | 30 ++++ system/leader_election_converter/README.md | 52 ++++++ .../leader_election_converter.param.yaml | 12 ++ .../leader_election_converter.launch.xml | 20 +++ system/leader_election_converter/package.xml | 25 +++ .../leader_election_converter.schema.json | 89 +++++++++++ .../converter/availability_converter.cpp | 73 +++++++++ .../converter/availability_converter.hpp | 65 ++++++++ .../src/common/converter/log_converter.cpp | 125 +++++++++++++++ .../src/common/converter/log_converter.hpp | 87 ++++++++++ .../src/common/converter/mrm_converter.cpp | 94 +++++++++++ .../src/common/converter/mrm_converter.hpp | 72 +++++++++ .../src/common/converter/udp_receiver.hpp | 148 ++++++++++++++++++ .../src/common/converter/udp_sender.hpp | 82 ++++++++++ .../src/node/leader_election_converter.cpp | 59 +++++++ .../src/node/leader_election_converter.hpp | 51 ++++++ 16 files changed, 1084 insertions(+) create mode 100644 system/leader_election_converter/CMakeLists.txt create mode 100644 system/leader_election_converter/README.md create mode 100644 system/leader_election_converter/config/leader_election_converter.param.yaml create mode 100644 system/leader_election_converter/launch/leader_election_converter.launch.xml create mode 100644 system/leader_election_converter/package.xml create mode 100644 system/leader_election_converter/schema/leader_election_converter.schema.json create mode 100644 system/leader_election_converter/src/common/converter/availability_converter.cpp create mode 100644 system/leader_election_converter/src/common/converter/availability_converter.hpp create mode 100644 system/leader_election_converter/src/common/converter/log_converter.cpp create mode 100644 system/leader_election_converter/src/common/converter/log_converter.hpp create mode 100644 system/leader_election_converter/src/common/converter/mrm_converter.cpp create mode 100644 system/leader_election_converter/src/common/converter/mrm_converter.hpp create mode 100644 system/leader_election_converter/src/common/converter/udp_receiver.hpp create mode 100644 system/leader_election_converter/src/common/converter/udp_sender.hpp create mode 100644 system/leader_election_converter/src/node/leader_election_converter.cpp create mode 100644 system/leader_election_converter/src/node/leader_election_converter.hpp diff --git a/system/leader_election_converter/CMakeLists.txt b/system/leader_election_converter/CMakeLists.txt new file mode 100644 index 0000000000000..697b9a3b74d13 --- /dev/null +++ b/system/leader_election_converter/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(leader_election_converter) + +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/leader_election_converter.cpp +) +target_include_directories(${PROJECT_NAME} PRIVATE src/common/converter) + +target_link_libraries(${PROJECT_NAME} common_converter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "leader_election_converter::LeaderElectionConverter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/leader_election_converter/README.md b/system/leader_election_converter/README.md new file mode 100644 index 0000000000000..75058f2c91036 --- /dev/null +++ b/system/leader_election_converter/README.md @@ -0,0 +1,52 @@ +# leader_election_converter + +## Overview + +The leader election converter node is responsible for relaying UDP packets and ROS2 topics between the leader_election 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_auto_vehicle_msgs/msg/ControlModeReport`| Ego control mode. | +| udp sender | none | `struct Availability` | Combination of the above two. | + + +## mrm converte +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` | `tier4_system_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 amoung election nodes. | +| udp receiver | none | `struct ElectionStatus` | Leader Election status. | +| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages amoung election nodes. | +| publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. | +| publisher | `/system/fail_safe/over_all/mrm_state`| `autoware_adapi_v1_msgs/msg/mrm_state` | System-wode MRM status. | + +## Parameters + +{{ json_to_markdown("system/leader_election_converter/schema/leader_election_converter.schema.json") }} + diff --git a/system/leader_election_converter/config/leader_election_converter.param.yaml b/system/leader_election_converter/config/leader_election_converter.param.yaml new file mode 100644 index 0000000000000..5269a558cd107 --- /dev/null +++ b/system/leader_election_converter/config/leader_election_converter.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/leader_election_converter/launch/leader_election_converter.launch.xml b/system/leader_election_converter/launch/leader_election_converter.launch.xml new file mode 100644 index 0000000000000..d3737bd3eba6b --- /dev/null +++ b/system/leader_election_converter/launch/leader_election_converter.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/system/leader_election_converter/package.xml b/system/leader_election_converter/package.xml new file mode 100644 index 0000000000000..1a86e5339a2ee --- /dev/null +++ b/system/leader_election_converter/package.xml @@ -0,0 +1,25 @@ + + + + leader_election_converter + 0.1.0 + The leader election converter package + TetsuKawa + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp + rclcpp_components + tier4_system_msgs + autoware_auto_vehicle_msgs + autoware_adapi_v1_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/leader_election_converter/schema/leader_election_converter.schema.json b/system/leader_election_converter/schema/leader_election_converter.schema.json new file mode 100644 index 0000000000000..c96f97b294f77 --- /dev/null +++ b/system/leader_election_converter/schema/leader_election_converter.schema.json @@ -0,0 +1,89 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for leader election converter", + "type": "object", + "definitions": { + "leader_election_converter": { + "type": "object", + "properties": { + "availability_dest_ip": { + "type": "string", + "description": "IP address of the destination of avaialability", + "default": "127.0.0.1" + }, + "availability_dest_port": { + "type": "string", + "description": "Port of the destination of avaialability", + "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 electioin_communication", + "default": "127.0.0.1" + }, + "election_communication_src_port": { + "type": "string", + "description": "Port of the source of electioin_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/leader_election_converter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/system/leader_election_converter/src/common/converter/availability_converter.cpp b/system/leader_election_converter/src/common/converter/availability_converter.cpp new file mode 100644 index 0000000000000..ad822e86e0192 --- /dev/null +++ b/system/leader_election_converter/src/common/converter/availability_converter.cpp @@ -0,0 +1,73 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "availability_converter.hpp" + +namespace leader_election_converter +{ + +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).transient_local(); + 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_auto_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(node_->get_logger(), "Failed to take control mode report"); + } +} + +} // namespace leader_election_converter diff --git a/system/leader_election_converter/src/common/converter/availability_converter.hpp b/system/leader_election_converter/src/common/converter/availability_converter.hpp new file mode 100644 index 0000000000000..edac136d9bd6c --- /dev/null +++ b/system/leader_election_converter/src/common/converter/availability_converter.hpp @@ -0,0 +1,65 @@ +// 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__AVAILABILITY_CONVERTER_HPP_ +#define COMMON__AVAILABILITY_CONVERTER_HPP_ + +#include +#include + +#include + +#include "udp_sender.hpp" + + +namespace leader_election_converter +{ + +struct Availability +{ + autoware_auto_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: + AvailabilityConverter(rclcpp::Node * node); + ~AvailabilityConverter() = default; + + 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 leader_election_converter + +#endif // COMMON__AVAILABILITY_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp new file mode 100644 index 0000000000000..51071dc638d5f --- /dev/null +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -0,0 +1,125 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "log_converter.hpp" + +namespace leader_election_converter +{ + +LogConverter::LogConverter(rclcpp::Node * node) +: node_(node), +is_election_comunication_running_(true), +is_election_status_running_(true) +{} + +LogConverter::~LogConverter() +{ + is_election_comunication_running_ = false; + udp_election_comunication_receiver_->~UdpReceiver(); + is_election_status_running_ = false; + udp_election_status_receiver_->~UdpReceiver(); + if (udp_election_comunication_thread_.joinable()) { + udp_election_comunication_thread_.join(); + } + if (udp_election_status_thread_.joinable()) { + udp_election_status_thread_.join(); + } +} + +void LogConverter::setUdpElectionCommunicatioinReceiver(const std::string & src_ip, const std::string & src_port) +{ + udp_election_comunication_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_comunication_receiver_ = std::make_unique>(src_ip, src_port, std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1)); + while (is_election_comunication_running_) { + udp_election_comunication_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_comunication_ = 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.link = (node_msg.msg >> 24) & 0xFF; + msg.heartbeat = (node_msg.msg >> 56) & 0x0F; + msg.checksum = (node_msg.msg >> 60) & 0x0F; + pub_election_comunication_->publish(msg); +} + +void LogConverter::convertElectionStatusToTopic(const ElectionStatus & status) +{ + tier4_system_msgs::msg::ElectionStatus election_status; + autoware_adapi_v1_msgs::msg::MrmState mrm_status; + + 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.state; + election_status.mrm_state.behavior.type = status.behavior; + election_status.election_start_count = status.election_start_count; + election_status.in_election = status.in_election; + election_status.is_received_availability = status.is_received_availability; + election_status.is_received_mrm_state = status.is_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_status.stamp = node_->now(); + mrm_status.state = status.state; + mrm_status.behavior = status.behavior; + pub_over_all_mrm_state_->publish(mrm_status); +} + +} // namespace leader_election_converter diff --git a/system/leader_election_converter/src/common/converter/log_converter.hpp b/system/leader_election_converter/src/common/converter/log_converter.hpp new file mode 100644 index 0000000000000..12adb5ab35ecb --- /dev/null +++ b/system/leader_election_converter/src/common/converter/log_converter.hpp @@ -0,0 +1,87 @@ +// 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__LOG_CONVERTER_HPP_ +#define COMMON__LOG_CONVERTER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include "udp_sender.hpp" +#include "udp_receiver.hpp" + + +namespace leader_election_converter +{ + +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; + tier4_system_msgs::msg::MrmState::_state_type state; + tier4_system_msgs::msg::MrmBehavior::_type_type 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::_is_received_availability_type is_received_availability; + tier4_system_msgs::msg::ElectionStatus::_is_received_mrm_state_type is_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: + LogConverter(rclcpp::Node * node); + ~LogConverter(); + + void setUdpElectionCommunicatioinReceiver(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_comunication_receiver_; + std::unique_ptr> udp_election_status_receiver_; + rclcpp::Publisher::SharedPtr pub_election_comunication_; + rclcpp::Publisher::SharedPtr pub_election_status_; + rclcpp::Publisher::SharedPtr pub_over_all_mrm_state_; + + std::thread udp_election_comunication_thread_; + std::thread udp_election_status_thread_; + std::atomic is_election_comunication_running_; + std::atomic is_election_status_running_; +}; + +} // namespace leader_election_converter + +#endif // COMMON__LOG_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.cpp b/system/leader_election_converter/src/common/converter/mrm_converter.cpp new file mode 100644 index 0000000000000..72d6fbe51a747 --- /dev/null +++ b/system/leader_election_converter/src/common/converter/mrm_converter.cpp @@ -0,0 +1,94 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "mrm_converter.hpp" + +namespace leader_election_converter +{ + +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).transient_local(); + 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 tier4_system_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg) +{ + MrmState mrm_state; + mrm_state.state = mrm_state_msg->state; + mrm_state.behavior = mrm_state_msg->behavior.type; + + 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 leader_election_converter diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.hpp b/system/leader_election_converter/src/common/converter/mrm_converter.hpp new file mode 100644 index 0000000000000..18aea9d3006fd --- /dev/null +++ b/system/leader_election_converter/src/common/converter/mrm_converter.hpp @@ -0,0 +1,72 @@ +// 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__MRM_CONVERTER_HPP_ +#define COMMON__MRM_CONVERTER_HPP_ + +#include +#include + +#include +#include +#include + +#include "udp_sender.hpp" +#include "udp_receiver.hpp" + + +namespace leader_election_converter +{ + +typedef struct MrmState +{ + tier4_system_msgs::msg::MrmState::_state_type state; + tier4_system_msgs::msg::MrmBehavior::_type_type behavior; +} MrmState; + +typedef struct MrmRequest +{ + tier4_system_msgs::msg::MrmBehavior::_type_type behavior; +} MrmRequest; + +class MrmConverter +{ +public: + 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 tier4_system_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 leader_election_converter + +#endif // COMMON__MRM_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/udp_receiver.hpp b/system/leader_election_converter/src/common/converter/udp_receiver.hpp new file mode 100644 index 0000000000000..834e285e74efc --- /dev/null +++ b/system/leader_election_converter/src/common/converter/udp_receiver.hpp @@ -0,0 +1,148 @@ + +// 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__UDP_RECEIVER_HPP_ +#define COMMON__UDP_RECEIVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace leader_election_converter +{ + +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 leader_election_converter + +#endif // COMMON__UDP_RECEIVER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/udp_sender.hpp b/system/leader_election_converter/src/common/converter/udp_sender.hpp new file mode 100644 index 0000000000000..be06e152dfce8 --- /dev/null +++ b/system/leader_election_converter/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__UDP_SENDER_HPP_ +#define COMMON__UDP_SENDER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + + +namespace leader_election_converter +{ + +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 leader_election_converterl + + +#endif // COMMON__UDP_SENDER_HPP_ diff --git a/system/leader_election_converter/src/node/leader_election_converter.cpp b/system/leader_election_converter/src/node/leader_election_converter.cpp new file mode 100644 index 0000000000000..32eb845f38f61 --- /dev/null +++ b/system/leader_election_converter/src/node/leader_election_converter.cpp @@ -0,0 +1,59 @@ +// 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 "leader_election_converter.hpp" + +namespace leader_election_converter +{ + +LeaderElectionConverter::LeaderElectionConverter(const rclcpp::NodeOptions & node_options) +: Node("leader_election_converter", 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_.setUdpElectionCommunicatioinReceiver(election_communication_src_ip_, election_communication_src_port_); + log_converter_.setUdpElectionStatusReceiver(election_status_src_ip_, election_status_src_port_); +} + +} // namespace leader_election_converter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(leader_election_converter::LeaderElectionConverter) diff --git a/system/leader_election_converter/src/node/leader_election_converter.hpp b/system/leader_election_converter/src/node/leader_election_converter.hpp new file mode 100644 index 0000000000000..de35afd905544 --- /dev/null +++ b/system/leader_election_converter/src/node/leader_election_converter.hpp @@ -0,0 +1,51 @@ +// 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__LEADER_ELECTION_CONVERTER_HPP_ +#define NODE__LEADER_ELECTION_CONVERTER_HPP_ + +#include +#include +#include "availability_converter.hpp" +#include "mrm_converter.hpp" +#include "log_converter.hpp" + +namespace leader_election_converter +{ + +class LeaderElectionConverter : public rclcpp::Node +{ +public: + explicit LeaderElectionConverter(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 leader_election_converter + +#endif // NODE__LEADER_ELECTION_CONVERTER_HPP_ From 4abddaac7a361c4f87dfb837ac1964132cae56f7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jun 2024 16:39:07 +0000 Subject: [PATCH 02/14] style(pre-commit): autofix --- system/leader_election_converter/README.md | 44 +++++++------- .../leader_election_converter.launch.xml | 2 +- system/leader_election_converter/package.xml | 4 +- .../converter/availability_converter.cpp | 32 ++++++----- .../converter/availability_converter.hpp | 32 +++++------ .../src/common/converter/log_converter.cpp | 41 ++++++++----- .../src/common/converter/log_converter.hpp | 29 +++++----- .../src/common/converter/mrm_converter.cpp | 17 +++--- .../src/common/converter/mrm_converter.hpp | 18 +++--- .../src/common/converter/udp_receiver.hpp | 56 +++++++++--------- .../src/common/converter/udp_sender.hpp | 31 +++++----- .../src/node/leader_election_converter.cpp | 57 ++++++++++--------- .../src/node/leader_election_converter.hpp | 10 ++-- 13 files changed, 198 insertions(+), 175 deletions(-) diff --git a/system/leader_election_converter/README.md b/system/leader_election_converter/README.md index 75058f2c91036..5d6dd0a5bf1f4 100644 --- a/system/leader_election_converter/README.md +++ b/system/leader_election_converter/README.md @@ -8,45 +8,43 @@ The leader election converter node is responsible for relaying UDP packets and R 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_auto_vehicle_msgs/msg/ControlModeReport`| Ego control mode. | -| udp sender | none | `struct Availability` | Combination of the above two. | - +| 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_auto_vehicle_msgs/msg/ControlModeReport` | Ego control mode. | +| udp sender | none | `struct Availability` | Combination of the above two. | ## mrm converte + 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` | `tier4_system_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. | - +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------ | ----------------------------------- | ------------------------ | +| subscriber | `/system/fail_safe/mrm_state` | `tier4_system_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`, + +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 amoung election nodes. | -| udp receiver | none | `struct ElectionStatus` | Leader Election status. | -| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages amoung election nodes. | -| publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. | -| publisher | `/system/fail_safe/over_all/mrm_state`| `autoware_adapi_v1_msgs/msg/mrm_state` | System-wode MRM status. | +| Interface Type | Interface Name | Data Type | Description | +| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------- | +| udp receiver | none | `struct ElectionCommunication` | messages amoung election nodes. | +| udp receiver | none | `struct ElectionStatus` | Leader Election status. | +| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages amoung election nodes. | +| publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. | +| publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wode MRM status. | ## Parameters {{ json_to_markdown("system/leader_election_converter/schema/leader_election_converter.schema.json") }} - diff --git a/system/leader_election_converter/launch/leader_election_converter.launch.xml b/system/leader_election_converter/launch/leader_election_converter.launch.xml index d3737bd3eba6b..34ab3a8212336 100644 --- a/system/leader_election_converter/launch/leader_election_converter.launch.xml +++ b/system/leader_election_converter/launch/leader_election_converter.launch.xml @@ -1,4 +1,4 @@ - + diff --git a/system/leader_election_converter/package.xml b/system/leader_election_converter/package.xml index 1a86e5339a2ee..d9341c8fe2df3 100644 --- a/system/leader_election_converter/package.xml +++ b/system/leader_election_converter/package.xml @@ -10,11 +10,11 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs + autoware_auto_vehicle_msgs rclcpp rclcpp_components tier4_system_msgs - autoware_auto_vehicle_msgs - autoware_adapi_v1_msgs ament_lint_auto autoware_lint_common diff --git a/system/leader_election_converter/src/common/converter/availability_converter.cpp b/system/leader_election_converter/src/common/converter/availability_converter.cpp index ad822e86e0192..6edb0f89e51d1 100644 --- a/system/leader_election_converter/src/common/converter/availability_converter.cpp +++ b/system/leader_election_converter/src/common/converter/availability_converter.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "availability_converter.hpp" #include "rclcpp/rclcpp.hpp" -#include "availability_converter.hpp" namespace leader_election_converter { -AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node) -: node_(node) {} +AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node) : node_(node) +{ +} void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port) { @@ -30,22 +31,27 @@ void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std: void AvailabilityConverter::setSubscriber() { const auto qos = rclcpp::QoS(1).transient_local(); - availability_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + 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); + 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_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) {}; + control_mode_options.callback_group = control_mode_callback_group_; + auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_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_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); - + sub_control_mode_ = + node_->create_subscription( + "~/input/control_mode", qos, not_executed_callback, control_mode_options); } void AvailabilityConverter::convertToUdp( diff --git a/system/leader_election_converter/src/common/converter/availability_converter.hpp b/system/leader_election_converter/src/common/converter/availability_converter.hpp index edac136d9bd6c..2f4434cd008b2 100644 --- a/system/leader_election_converter/src/common/converter/availability_converter.hpp +++ b/system/leader_election_converter/src/common/converter/availability_converter.hpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMMON__AVAILABILITY_CONVERTER_HPP_ -#define COMMON__AVAILABILITY_CONVERTER_HPP_ +#ifndef COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ +#define COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ -#include -#include +#include "udp_sender.hpp" #include -#include "udp_sender.hpp" - +#include +#include namespace leader_election_converter { @@ -41,25 +40,26 @@ struct Availability class AvailabilityConverter { - public: AvailabilityConverter(rclcpp::Node * node); ~AvailabilityConverter() = default; 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); + 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_; - + 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 leader_election_converter -#endif // COMMON__AVAILABILITY_CONVERTER_HPP_ +#endif // COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp index 51071dc638d5f..656322469d009 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.cpp +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/rclcpp.hpp" #include "log_converter.hpp" +#include "rclcpp/rclcpp.hpp" + namespace leader_election_converter { LogConverter::LogConverter(rclcpp::Node * node) -: node_(node), -is_election_comunication_running_(true), -is_election_status_running_(true) -{} +: node_(node), is_election_comunication_running_(true), is_election_status_running_(true) +{ +} LogConverter::~LogConverter() { @@ -38,15 +38,20 @@ LogConverter::~LogConverter() } } -void LogConverter::setUdpElectionCommunicatioinReceiver(const std::string & src_ip, const std::string & src_port) +void LogConverter::setUdpElectionCommunicatioinReceiver( + const std::string & src_ip, const std::string & src_port) { - udp_election_comunication_thread_ = std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port); + udp_election_comunication_thread_ = + std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port); } -void LogConverter::startUdpElectionCommunicationReceiver(const std::string & src_ip, const std::string & src_port) +void LogConverter::startUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port) { try { - udp_election_comunication_receiver_ = std::make_unique>(src_ip, src_port, std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1)); + udp_election_comunication_receiver_ = std::make_unique>( + src_ip, src_port, + std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1)); while (is_election_comunication_running_) { udp_election_comunication_receiver_->receive(); } @@ -55,15 +60,20 @@ void LogConverter::startUdpElectionCommunicationReceiver(const std::string & src } } -void LogConverter::setUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port) +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); + 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) +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)); + 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(); } @@ -74,8 +84,9 @@ void LogConverter::startUdpElectionStatusReceiver(const std::string & src_ip, co void LogConverter::setPublisher() { - pub_election_comunication_ = node_->create_publisher( - "~/output/election_communication", rclcpp::QoS{1}); + pub_election_comunication_ = + 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( diff --git a/system/leader_election_converter/src/common/converter/log_converter.hpp b/system/leader_election_converter/src/common/converter/log_converter.hpp index 12adb5ab35ecb..3f83ffd0c2074 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.hpp +++ b/system/leader_election_converter/src/common/converter/log_converter.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMMON__LOG_CONVERTER_HPP_ -#define COMMON__LOG_CONVERTER_HPP_ +#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 #include - -#include "udp_sender.hpp" -#include "udp_receiver.hpp" - +#include namespace leader_election_converter { @@ -59,12 +59,14 @@ class LogConverter LogConverter(rclcpp::Node * node); ~LogConverter(); - void setUdpElectionCommunicatioinReceiver(const std::string & src_ip, const std::string & src_port); + void setUdpElectionCommunicatioinReceiver( + 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 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); @@ -72,7 +74,8 @@ class LogConverter rclcpp::Node * node_; std::unique_ptr> udp_election_comunication_receiver_; std::unique_ptr> udp_election_status_receiver_; - rclcpp::Publisher::SharedPtr pub_election_comunication_; + rclcpp::Publisher::SharedPtr + pub_election_comunication_; rclcpp::Publisher::SharedPtr pub_election_status_; rclcpp::Publisher::SharedPtr pub_over_all_mrm_state_; @@ -84,4 +87,4 @@ class LogConverter } // namespace leader_election_converter -#endif // COMMON__LOG_CONVERTER_HPP_ +#endif // COMMON__CONVERTER__LOG_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.cpp b/system/leader_election_converter/src/common/converter/mrm_converter.cpp index 72d6fbe51a747..4f873f85d4696 100644 --- a/system/leader_election_converter/src/common/converter/mrm_converter.cpp +++ b/system/leader_election_converter/src/common/converter/mrm_converter.cpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/rclcpp.hpp" #include "mrm_converter.hpp" +#include "rclcpp/rclcpp.hpp" + namespace leader_election_converter { -MrmConverter::MrmConverter(rclcpp::Node * node) -: node_(node), -is_udp_receiver_running_(true) +MrmConverter::MrmConverter(rclcpp::Node * node) : node_(node), is_udp_receiver_running_(true) { } @@ -46,7 +45,8 @@ void MrmConverter::setUdpReceiver(const std::string & src_ip, const std::string 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)); + 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(); } @@ -63,8 +63,8 @@ void MrmConverter::setSubscriber() options.callback_group = callback_group_; sub_mrm_state_ = node_->create_subscription( - "~/input/mrm_state", qos, - std::bind(&MrmConverter::convertToUdp, this, std::placeholders::_1), options); + "~/input/mrm_state", qos, std::bind(&MrmConverter::convertToUdp, this, std::placeholders::_1), + options); } void MrmConverter::setPublisher() @@ -73,7 +73,8 @@ void MrmConverter::setPublisher() "~/output/mrm_request", rclcpp::QoS{1}); } -void MrmConverter::convertToUdp(const tier4_system_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg) +void MrmConverter::convertToUdp( + const tier4_system_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg) { MrmState mrm_state; mrm_state.state = mrm_state_msg->state; diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.hpp b/system/leader_election_converter/src/common/converter/mrm_converter.hpp index 18aea9d3006fd..2f38f15e0cd66 100644 --- a/system/leader_election_converter/src/common/converter/mrm_converter.hpp +++ b/system/leader_election_converter/src/common/converter/mrm_converter.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMMON__MRM_CONVERTER_HPP_ -#define COMMON__MRM_CONVERTER_HPP_ +#ifndef COMMON__CONVERTER__MRM_CONVERTER_HPP_ +#define COMMON__CONVERTER__MRM_CONVERTER_HPP_ -#include -#include +#include "udp_receiver.hpp" +#include "udp_sender.hpp" #include -#include -#include -#include "udp_sender.hpp" -#include "udp_receiver.hpp" +#include +#include +#include +#include namespace leader_election_converter { @@ -69,4 +69,4 @@ class MrmConverter } // namespace leader_election_converter -#endif // COMMON__MRM_CONVERTER_HPP_ +#endif // COMMON__CONVERTER__MRM_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/udp_receiver.hpp b/system/leader_election_converter/src/common/converter/udp_receiver.hpp index 834e285e74efc..39abce0812029 100644 --- a/system/leader_election_converter/src/common/converter/udp_receiver.hpp +++ b/system/leader_election_converter/src/common/converter/udp_receiver.hpp @@ -13,47 +13,48 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMMON__UDP_RECEIVER_HPP_ -#define COMMON__UDP_RECEIVER_HPP_ +#ifndef COMMON__CONVERTER__UDP_RECEIVER_HPP_ +#define COMMON__CONVERTER__UDP_RECEIVER_HPP_ -#include -#include -#include -#include +#include +#include #include -#include +#include +#include #include -#include -#include -#include +#include +#include +#include +#include namespace leader_election_converter { - -template + +template class UdpReceiver { public: - using CallbackType = std::function; + 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( + 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 + bool receive(T & data); // for non callback + void receive(); // for callback private: int socketfd_; - struct addrinfo *res_; + struct addrinfo * res_; CallbackType callback_; void setCallback(CallbackType callback); }; -template +template UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port) { struct addrinfo hints; @@ -78,14 +79,14 @@ UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port) } } -template +template UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port, CallbackType callback) : UdpReceiver(ip, port) { setCallback(callback); } -template +template UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port, bool is_non_blocking) : UdpReceiver(ip, port) { @@ -98,14 +99,15 @@ UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port, bo } } -template -UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port, bool is_non_blocking, CallbackType callback) +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 +template UdpReceiver::~UdpReceiver() { shutdown(socketfd_, SHUT_RDWR); @@ -113,13 +115,13 @@ UdpReceiver::~UdpReceiver() close(socketfd_); } -template +template void UdpReceiver::setCallback(CallbackType callback) { callback_ = callback; } -template +template bool UdpReceiver::receive(T & data) { struct sockaddr_storage addr; @@ -134,7 +136,7 @@ bool UdpReceiver::receive(T & data) return true; } -template +template void UdpReceiver::receive() { T data; @@ -145,4 +147,4 @@ void UdpReceiver::receive() } // namespace leader_election_converter -#endif // COMMON__UDP_RECEIVER_HPP_ +#endif // COMMON__CONVERTER__UDP_RECEIVER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/udp_sender.hpp b/system/leader_election_converter/src/common/converter/udp_sender.hpp index be06e152dfce8..866fd1b7fcfb9 100644 --- a/system/leader_election_converter/src/common/converter/udp_sender.hpp +++ b/system/leader_election_converter/src/common/converter/udp_sender.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMMON__UDP_SENDER_HPP_ -#define COMMON__UDP_SENDER_HPP_ +#ifndef COMMON__CONVERTER__UDP_SENDER_HPP_ +#define COMMON__CONVERTER__UDP_SENDER_HPP_ -#include -#include -#include -#include #include -#include +#include +#include #include +#include +#include +#include namespace leader_election_converter { - -template + +template class UdpSender { public: @@ -38,10 +38,10 @@ class UdpSender private: int socketfd_; - struct addrinfo *res_; + struct addrinfo * res_; }; -template +template UdpSender::UdpSender(const std::string & ip, const std::string & port) { struct addrinfo hints; @@ -60,7 +60,7 @@ UdpSender::UdpSender(const std::string & ip, const std::string & port) } } -template +template UdpSender::~UdpSender() { shutdown(socketfd_, SHUT_RDWR); @@ -68,7 +68,7 @@ UdpSender::~UdpSender() close(socketfd_); } -template +template void UdpSender::send(const T & data) { if (sendto(socketfd_, &data, sizeof(T), 0, res_->ai_addr, res_->ai_addrlen) < 0) { @@ -76,7 +76,6 @@ void UdpSender::send(const T & data) } } -} // namespace leader_election_converterl - +} // namespace leader_election_converter -#endif // COMMON__UDP_SENDER_HPP_ +#endif // COMMON__CONVERTER__UDP_SENDER_HPP_ diff --git a/system/leader_election_converter/src/node/leader_election_converter.cpp b/system/leader_election_converter/src/node/leader_election_converter.cpp index 32eb845f38f61..a90af167688de 100644 --- a/system/leader_election_converter/src/node/leader_election_converter.cpp +++ b/system/leader_election_converter/src/node/leader_election_converter.cpp @@ -23,34 +23,35 @@ LeaderElectionConverter::LeaderElectionConverter(const rclcpp::NodeOptions & nod 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_.setUdpElectionCommunicatioinReceiver(election_communication_src_ip_, election_communication_src_port_); - log_converter_.setUdpElectionStatusReceiver(election_status_src_ip_, election_status_src_port_); + 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_.setUdpElectionCommunicatioinReceiver( + election_communication_src_ip_, election_communication_src_port_); + log_converter_.setUdpElectionStatusReceiver(election_status_src_ip_, election_status_src_port_); } } // namespace leader_election_converter diff --git a/system/leader_election_converter/src/node/leader_election_converter.hpp b/system/leader_election_converter/src/node/leader_election_converter.hpp index de35afd905544..f33ba60bb7b24 100644 --- a/system/leader_election_converter/src/node/leader_election_converter.hpp +++ b/system/leader_election_converter/src/node/leader_election_converter.hpp @@ -15,11 +15,13 @@ #ifndef NODE__LEADER_ELECTION_CONVERTER_HPP_ #define NODE__LEADER_ELECTION_CONVERTER_HPP_ -#include -#include #include "availability_converter.hpp" -#include "mrm_converter.hpp" #include "log_converter.hpp" +#include "mrm_converter.hpp" + +#include + +#include namespace leader_election_converter { @@ -33,7 +35,7 @@ class LeaderElectionConverter : public rclcpp::Node 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_state_dest_port_; std::string mrm_request_src_ip_; std::string mrm_request_src_port_; std::string election_communication_src_ip_; From 8cf97b3019914d51592154116348eb20b6bfb043 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 25 Jun 2024 01:59:35 +0900 Subject: [PATCH 03/14] modify Signed-off-by: TetsuKawa --- .../src/common/converter/availability_converter.cpp | 2 ++ .../src/common/converter/availability_converter.hpp | 6 +++--- .../src/common/converter/log_converter.cpp | 3 +++ .../src/common/converter/log_converter.hpp | 4 +++- .../src/common/converter/mrm_converter.cpp | 3 +++ .../src/common/converter/mrm_converter.hpp | 4 +++- .../src/common/converter/udp_receiver.hpp | 1 + .../src/common/converter/udp_sender.hpp | 1 + .../src/node/leader_election_converter.cpp | 2 ++ .../src/node/leader_election_converter.hpp | 2 ++ 10 files changed, 23 insertions(+), 5 deletions(-) diff --git a/system/leader_election_converter/src/common/converter/availability_converter.cpp b/system/leader_election_converter/src/common/converter/availability_converter.cpp index ad822e86e0192..1398cd7fa160b 100644 --- a/system/leader_election_converter/src/common/converter/availability_converter.cpp +++ b/system/leader_election_converter/src/common/converter/availability_converter.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include "rclcpp/rclcpp.hpp" #include "availability_converter.hpp" diff --git a/system/leader_election_converter/src/common/converter/availability_converter.hpp b/system/leader_election_converter/src/common/converter/availability_converter.hpp index edac136d9bd6c..59c634cca9dd6 100644 --- a/system/leader_election_converter/src/common/converter/availability_converter.hpp +++ b/system/leader_election_converter/src/common/converter/availability_converter.hpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include "udp_sender.hpp" @@ -41,10 +43,8 @@ struct Availability class AvailabilityConverter { - public: - AvailabilityConverter(rclcpp::Node * node); - ~AvailabilityConverter() = default; + explicit AvailabilityConverter(rclcpp::Node * node); void setUdpSender(const std::string & dest_ip, const std::string & dest_port); void setSubscriber(); diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp index 51071dc638d5f..12635c275913b 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.cpp +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include "rclcpp/rclcpp.hpp" #include "log_converter.hpp" diff --git a/system/leader_election_converter/src/common/converter/log_converter.hpp b/system/leader_election_converter/src/common/converter/log_converter.hpp index 12adb5ab35ecb..ba3d3c409cff3 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.hpp +++ b/system/leader_election_converter/src/common/converter/log_converter.hpp @@ -23,6 +23,8 @@ #include #include #include +#include +#include #include "udp_sender.hpp" #include "udp_receiver.hpp" @@ -56,7 +58,7 @@ typedef struct ElectionStatus class LogConverter { public: - LogConverter(rclcpp::Node * node); + explicit LogConverter(rclcpp::Node * node); ~LogConverter(); void setUdpElectionCommunicatioinReceiver(const std::string & src_ip, const std::string & src_port); diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.cpp b/system/leader_election_converter/src/common/converter/mrm_converter.cpp index 72d6fbe51a747..6f8dfdfc76c24 100644 --- a/system/leader_election_converter/src/common/converter/mrm_converter.cpp +++ b/system/leader_election_converter/src/common/converter/mrm_converter.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include "rclcpp/rclcpp.hpp" #include "mrm_converter.hpp" diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.hpp b/system/leader_election_converter/src/common/converter/mrm_converter.hpp index 18aea9d3006fd..d1dffa18eae5e 100644 --- a/system/leader_election_converter/src/common/converter/mrm_converter.hpp +++ b/system/leader_election_converter/src/common/converter/mrm_converter.hpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include "udp_sender.hpp" #include "udp_receiver.hpp" @@ -43,7 +45,7 @@ typedef struct MrmRequest class MrmConverter { public: - MrmConverter(rclcpp::Node * node); + explicit MrmConverter(rclcpp::Node * node); ~MrmConverter(); void setUdpSender(const std::string & dest_ip, const std::string & dest_port); diff --git a/system/leader_election_converter/src/common/converter/udp_receiver.hpp b/system/leader_election_converter/src/common/converter/udp_receiver.hpp index 834e285e74efc..8d1f9f54aa6de 100644 --- a/system/leader_election_converter/src/common/converter/udp_receiver.hpp +++ b/system/leader_election_converter/src/common/converter/udp_receiver.hpp @@ -26,6 +26,7 @@ #include #include #include +#include namespace leader_election_converter diff --git a/system/leader_election_converter/src/common/converter/udp_sender.hpp b/system/leader_election_converter/src/common/converter/udp_sender.hpp index be06e152dfce8..251587f066d3f 100644 --- a/system/leader_election_converter/src/common/converter/udp_sender.hpp +++ b/system/leader_election_converter/src/common/converter/udp_sender.hpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace leader_election_converter diff --git a/system/leader_election_converter/src/node/leader_election_converter.cpp b/system/leader_election_converter/src/node/leader_election_converter.cpp index 32eb845f38f61..7ce92564b4d2b 100644 --- a/system/leader_election_converter/src/node/leader_election_converter.cpp +++ b/system/leader_election_converter/src/node/leader_election_converter.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "leader_election_converter.hpp" namespace leader_election_converter diff --git a/system/leader_election_converter/src/node/leader_election_converter.hpp b/system/leader_election_converter/src/node/leader_election_converter.hpp index de35afd905544..d6cbcf603f634 100644 --- a/system/leader_election_converter/src/node/leader_election_converter.hpp +++ b/system/leader_election_converter/src/node/leader_election_converter.hpp @@ -17,6 +17,8 @@ #include #include +#include + #include "availability_converter.hpp" #include "mrm_converter.hpp" #include "log_converter.hpp" From c7da2cde087888832d1178443e8484f492e8657f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jun 2024 17:07:18 +0000 Subject: [PATCH 04/14] style(pre-commit): autofix --- .../common/converter/availability_converter.cpp | 5 +++-- .../common/converter/availability_converter.hpp | 14 +++++++------- .../src/common/converter/log_converter.cpp | 7 +++---- .../src/common/converter/log_converter.hpp | 3 --- .../src/common/converter/mrm_converter.cpp | 7 +++---- .../src/common/converter/mrm_converter.hpp | 3 --- .../src/common/converter/udp_receiver.hpp | 2 +- .../src/common/converter/udp_sender.hpp | 2 +- .../src/node/leader_election_converter.cpp | 4 ++-- .../src/node/leader_election_converter.hpp | 5 +---- 10 files changed, 21 insertions(+), 31 deletions(-) diff --git a/system/leader_election_converter/src/common/converter/availability_converter.cpp b/system/leader_election_converter/src/common/converter/availability_converter.cpp index a213ffe0a529c..f4bba88fe0581 100644 --- a/system/leader_election_converter/src/common/converter/availability_converter.cpp +++ b/system/leader_election_converter/src/common/converter/availability_converter.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include "availability_converter.hpp" #include "rclcpp/rclcpp.hpp" +#include +#include + namespace leader_election_converter { diff --git a/system/leader_election_converter/src/common/converter/availability_converter.hpp b/system/leader_election_converter/src/common/converter/availability_converter.hpp index f7031cc37399e..a0dd5ebb422bb 100644 --- a/system/leader_election_converter/src/common/converter/availability_converter.hpp +++ b/system/leader_election_converter/src/common/converter/availability_converter.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMMON__AVAILABILITY_CONVERTER_HPP_ -#define COMMON__AVAILABILITY_CONVERTER_HPP_ +#ifndef COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ +#define COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ -#include -#include +#include "udp_sender.hpp" #include -#include -#include -#include "udp_sender.hpp" +#include +#include +#include +#include namespace leader_election_converter { diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp index 36015cf188aef..cd914df814248 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.cpp +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - -#include "rclcpp/rclcpp.hpp" #include "log_converter.hpp" #include "rclcpp/rclcpp.hpp" +#include +#include + namespace leader_election_converter { diff --git a/system/leader_election_converter/src/common/converter/log_converter.hpp b/system/leader_election_converter/src/common/converter/log_converter.hpp index a3fc56b141f24..dc03a3a27ef5d 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.hpp +++ b/system/leader_election_converter/src/common/converter/log_converter.hpp @@ -30,9 +30,6 @@ #include #include -#include "udp_sender.hpp" -#include "udp_receiver.hpp" - namespace leader_election_converter { diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.cpp b/system/leader_election_converter/src/common/converter/mrm_converter.cpp index bd4ae0346b902..d18583d1b617b 100644 --- a/system/leader_election_converter/src/common/converter/mrm_converter.cpp +++ b/system/leader_election_converter/src/common/converter/mrm_converter.cpp @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - -#include "rclcpp/rclcpp.hpp" #include "mrm_converter.hpp" #include "rclcpp/rclcpp.hpp" +#include +#include + namespace leader_election_converter { diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.hpp b/system/leader_election_converter/src/common/converter/mrm_converter.hpp index f645804a4f8ec..347a816029ecc 100644 --- a/system/leader_election_converter/src/common/converter/mrm_converter.hpp +++ b/system/leader_election_converter/src/common/converter/mrm_converter.hpp @@ -28,9 +28,6 @@ #include #include -#include "udp_sender.hpp" -#include "udp_receiver.hpp" - namespace leader_election_converter { diff --git a/system/leader_election_converter/src/common/converter/udp_receiver.hpp b/system/leader_election_converter/src/common/converter/udp_receiver.hpp index 137a13cd811a2..3a3e9f7075004 100644 --- a/system/leader_election_converter/src/common/converter/udp_receiver.hpp +++ b/system/leader_election_converter/src/common/converter/udp_receiver.hpp @@ -17,7 +17,6 @@ #define COMMON__CONVERTER__UDP_RECEIVER_HPP_ #include -#include #include #include #include @@ -28,6 +27,7 @@ #include #include #include +#include namespace leader_election_converter { diff --git a/system/leader_election_converter/src/common/converter/udp_sender.hpp b/system/leader_election_converter/src/common/converter/udp_sender.hpp index 87ca1790bac33..15cb16774a24e 100644 --- a/system/leader_election_converter/src/common/converter/udp_sender.hpp +++ b/system/leader_election_converter/src/common/converter/udp_sender.hpp @@ -19,11 +19,11 @@ #include #include #include -#include #include #include #include +#include namespace leader_election_converter { diff --git a/system/leader_election_converter/src/node/leader_election_converter.cpp b/system/leader_election_converter/src/node/leader_election_converter.cpp index 5fbc3acd2886b..d8934caf0951d 100644 --- a/system/leader_election_converter/src/node/leader_election_converter.cpp +++ b/system/leader_election_converter/src/node/leader_election_converter.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "leader_election_converter.hpp" +#include + namespace leader_election_converter { diff --git a/system/leader_election_converter/src/node/leader_election_converter.hpp b/system/leader_election_converter/src/node/leader_election_converter.hpp index e4dfb0a35b625..87381b2968097 100644 --- a/system/leader_election_converter/src/node/leader_election_converter.hpp +++ b/system/leader_election_converter/src/node/leader_election_converter.hpp @@ -15,10 +15,6 @@ #ifndef NODE__LEADER_ELECTION_CONVERTER_HPP_ #define NODE__LEADER_ELECTION_CONVERTER_HPP_ -#include -#include -#include - #include "availability_converter.hpp" #include "log_converter.hpp" #include "mrm_converter.hpp" @@ -26,6 +22,7 @@ #include #include +#include namespace leader_election_converter { From 4a0bd0ef2d2ec6ef1f1aa7e872f8a9c4c2513c8a Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 25 Jun 2024 02:16:19 +0900 Subject: [PATCH 05/14] modify: spellcheck Signed-off-by: TetsuKawa --- system/leader_election_converter/README.md | 6 ++--- .../leader_election_converter.launch.xml | 2 +- .../leader_election_converter.schema.json | 4 ++-- .../src/common/converter/log_converter.cpp | 22 +++++++++---------- .../src/common/converter/log_converter.hpp | 10 ++++----- .../src/node/leader_election_converter.cpp | 2 +- 6 files changed, 23 insertions(+), 23 deletions(-) diff --git a/system/leader_election_converter/README.md b/system/leader_election_converter/README.md index 5d6dd0a5bf1f4..945ead3092328 100644 --- a/system/leader_election_converter/README.md +++ b/system/leader_election_converter/README.md @@ -39,11 +39,11 @@ The log　converter receive udp packets into a structure called `ElectionCommuni | Interface Type | Interface Name | Data Type | Description | | -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------- | -| udp receiver | none | `struct ElectionCommunication` | messages amoung election nodes. | +| 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 amoung election nodes. | +| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages among election nodes. | | publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. | -| publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wode MRM status. | +| publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wide MRM status. | ## Parameters diff --git a/system/leader_election_converter/launch/leader_election_converter.launch.xml b/system/leader_election_converter/launch/leader_election_converter.launch.xml index 34ab3a8212336..901bd499ad2c6 100644 --- a/system/leader_election_converter/launch/leader_election_converter.launch.xml +++ b/system/leader_election_converter/launch/leader_election_converter.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/system/leader_election_converter/schema/leader_election_converter.schema.json b/system/leader_election_converter/schema/leader_election_converter.schema.json index c96f97b294f77..e6e480ebe2c7d 100644 --- a/system/leader_election_converter/schema/leader_election_converter.schema.json +++ b/system/leader_election_converter/schema/leader_election_converter.schema.json @@ -8,12 +8,12 @@ "properties": { "availability_dest_ip": { "type": "string", - "description": "IP address of the destination of avaialability", + "description": "IP address of the destination of availability", "default": "127.0.0.1" }, "availability_dest_port": { "type": "string", - "description": "Port of the destination of avaialability", + "description": "Port of the destination of availability", "default": "9000" }, "mrm_state_dest_ip": { diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp index 36015cf188aef..f2a65092e8f98 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.cpp +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -24,18 +24,18 @@ namespace leader_election_converter { LogConverter::LogConverter(rclcpp::Node * node) -: node_(node), is_election_comunication_running_(true), is_election_status_running_(true) +: node_(node), is_election_communication_running_(true), is_election_status_running_(true) { } LogConverter::~LogConverter() { - is_election_comunication_running_ = false; - udp_election_comunication_receiver_->~UdpReceiver(); + is_election_communication_running_ = false; + udp_election_communication_receiver_->~UdpReceiver(); is_election_status_running_ = false; udp_election_status_receiver_->~UdpReceiver(); - if (udp_election_comunication_thread_.joinable()) { - udp_election_comunication_thread_.join(); + if (udp_election_communication_thread_.joinable()) { + udp_election_communication_thread_.join(); } if (udp_election_status_thread_.joinable()) { udp_election_status_thread_.join(); @@ -45,7 +45,7 @@ LogConverter::~LogConverter() void LogConverter::setUdpElectionCommunicatioinReceiver( const std::string & src_ip, const std::string & src_port) { - udp_election_comunication_thread_ = + udp_election_communication_thread_ = std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port); } @@ -53,11 +53,11 @@ void LogConverter::startUdpElectionCommunicationReceiver( const std::string & src_ip, const std::string & src_port) { try { - udp_election_comunication_receiver_ = std::make_unique>( + udp_election_communication_receiver_ = std::make_unique>( src_ip, src_port, std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1)); - while (is_election_comunication_running_) { - udp_election_comunication_receiver_->receive(); + 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()); @@ -88,7 +88,7 @@ void LogConverter::startUdpElectionStatusReceiver( void LogConverter::setPublisher() { - pub_election_comunication_ = + pub_election_communication_ = node_->create_publisher( "~/output/election_communication", rclcpp::QoS{1}); pub_election_status_ = node_->create_publisher( @@ -107,7 +107,7 @@ void LogConverter::convertElectionCommunicationToTopic(const ElectionCommunicati msg.link = (node_msg.msg >> 24) & 0xFF; msg.heartbeat = (node_msg.msg >> 56) & 0x0F; msg.checksum = (node_msg.msg >> 60) & 0x0F; - pub_election_comunication_->publish(msg); + pub_election_communication_->publish(msg); } void LogConverter::convertElectionStatusToTopic(const ElectionStatus & status) diff --git a/system/leader_election_converter/src/common/converter/log_converter.hpp b/system/leader_election_converter/src/common/converter/log_converter.hpp index a3fc56b141f24..d465fb28d7187 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.hpp +++ b/system/leader_election_converter/src/common/converter/log_converter.hpp @@ -64,7 +64,7 @@ class LogConverter explicit LogConverter(rclcpp::Node * node); ~LogConverter(); - void setUdpElectionCommunicatioinReceiver( + 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(); @@ -77,16 +77,16 @@ class LogConverter void convertElectionStatusToTopic(const ElectionStatus & status); rclcpp::Node * node_; - std::unique_ptr> udp_election_comunication_receiver_; + std::unique_ptr> udp_election_communication_receiver_; std::unique_ptr> udp_election_status_receiver_; rclcpp::Publisher::SharedPtr - pub_election_comunication_; + pub_election_communication_; rclcpp::Publisher::SharedPtr pub_election_status_; rclcpp::Publisher::SharedPtr pub_over_all_mrm_state_; - std::thread udp_election_comunication_thread_; + std::thread udp_election_communication_thread_; std::thread udp_election_status_thread_; - std::atomic is_election_comunication_running_; + std::atomic is_election_communication_running_; std::atomic is_election_status_running_; }; diff --git a/system/leader_election_converter/src/node/leader_election_converter.cpp b/system/leader_election_converter/src/node/leader_election_converter.cpp index 5fbc3acd2886b..e289fab5c4a4b 100644 --- a/system/leader_election_converter/src/node/leader_election_converter.cpp +++ b/system/leader_election_converter/src/node/leader_election_converter.cpp @@ -51,7 +51,7 @@ LeaderElectionConverter::LeaderElectionConverter(const rclcpp::NodeOptions & nod // convert udp packets of election info to topics log_converter_.setPublisher(); - log_converter_.setUdpElectionCommunicatioinReceiver( + log_converter_.setUdpElectionCommunicationReceiver( election_communication_src_ip_, election_communication_src_port_); log_converter_.setUdpElectionStatusReceiver(election_status_src_ip_, election_status_src_port_); } From 5e0a902302bb12f2a8bf8aab598daf7b4b3e1557 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jun 2024 17:18:39 +0000 Subject: [PATCH 06/14] style(pre-commit): autofix --- system/leader_election_converter/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/system/leader_election_converter/README.md b/system/leader_election_converter/README.md index 945ead3092328..691f21d481f88 100644 --- a/system/leader_election_converter/README.md +++ b/system/leader_election_converter/README.md @@ -37,13 +37,13 @@ The log　converter receive udp packets into a structure called `ElectionCommuni ### Interface -| Interface Type | Interface Name | Data Type | Description | -| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------- | +| Interface Type | Interface Name | Data Type | Description | +| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------ | | udp receiver | none | `struct ElectionCommunication` | messages among election nodes. | -| udp receiver | none | `struct ElectionStatus` | Leader Election status. | +| 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` | `tier4_system_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. | +| publisher | `/system/election/status` | `tier4_system_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 From 11fc59e7b6830dba7526b1224efb84792d7af417 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 25 Jun 2024 02:22:19 +0900 Subject: [PATCH 07/14] modify: spellcheck Signed-off-by: TetsuKawa --- system/leader_election_converter/README.md | 2 +- .../schema/leader_election_converter.schema.json | 4 ++-- .../src/common/converter/log_converter.cpp | 9 +++++---- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/system/leader_election_converter/README.md b/system/leader_election_converter/README.md index 945ead3092328..4f1a4d75979c1 100644 --- a/system/leader_election_converter/README.md +++ b/system/leader_election_converter/README.md @@ -16,7 +16,7 @@ The availability converter subscribes `/system/operation_mode/availability` and | subscriber | `/vehicle/status/mrm_state` | `autoware_auto_vehicle_msgs/msg/ControlModeReport` | Ego control mode. | | udp sender | none | `struct Availability` | Combination of the above two. | -## mrm converte +## 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`. diff --git a/system/leader_election_converter/schema/leader_election_converter.schema.json b/system/leader_election_converter/schema/leader_election_converter.schema.json index e6e480ebe2c7d..c54fdf2210221 100644 --- a/system/leader_election_converter/schema/leader_election_converter.schema.json +++ b/system/leader_election_converter/schema/leader_election_converter.schema.json @@ -38,12 +38,12 @@ }, "election_communication_src_ip": { "type": "string", - "description": "IP address of the source of electioin_communication", + "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 electioin_communication", + "description": "Port of the source of election_communication", "default": "9003" }, "election_status_src_ip": { diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp index 9945bd27fc6bc..9c779b83dde8b 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.cpp +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "log_converter.hpp" +#include +#include #include "rclcpp/rclcpp.hpp" +#include "log_converter.hpp" -#include -#include +#include "rclcpp/rclcpp.hpp" namespace leader_election_converter { @@ -41,7 +42,7 @@ LogConverter::~LogConverter() } } -void LogConverter::setUdpElectionCommunicatioinReceiver( +void LogConverter::setUdpElectionCommunicationReceiver( const std::string & src_ip, const std::string & src_port) { udp_election_communication_thread_ = From 66c9d45e6fa84c2c2b16922a5306ea013493dd35 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jun 2024 17:25:10 +0000 Subject: [PATCH 08/14] style(pre-commit): autofix --- .../src/common/converter/log_converter.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp index 9c779b83dde8b..5987f635d4f59 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.cpp +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - -#include "rclcpp/rclcpp.hpp" #include "log_converter.hpp" #include "rclcpp/rclcpp.hpp" +#include +#include + namespace leader_election_converter { From dc817fa87de26529fc9fd4f105fa55d3f0019fbe Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 27 Jun 2024 16:25:56 +0900 Subject: [PATCH 09/14] modify: refactor Signed-off-by: TetsuKawa --- .../src/common/converter/log_converter.cpp | 4 ++-- .../src/common/converter/log_converter.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp index 5987f635d4f59..a1e9ee6cd89dd 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.cpp +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -121,8 +121,8 @@ void LogConverter::convertElectionStatusToTopic(const ElectionStatus & status) election_status.mrm_state.behavior.type = status.behavior; election_status.election_start_count = status.election_start_count; election_status.in_election = status.in_election; - election_status.is_received_availability = status.is_received_availability; - election_status.is_received_mrm_state = status.is_received_mrm_state; + 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; diff --git a/system/leader_election_converter/src/common/converter/log_converter.hpp b/system/leader_election_converter/src/common/converter/log_converter.hpp index aaf27513759d8..af0113544bf7f 100644 --- a/system/leader_election_converter/src/common/converter/log_converter.hpp +++ b/system/leader_election_converter/src/common/converter/log_converter.hpp @@ -46,8 +46,8 @@ typedef struct ElectionStatus tier4_system_msgs::msg::MrmBehavior::_type_type 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::_is_received_availability_type is_received_availability; - tier4_system_msgs::msg::ElectionStatus::_is_received_mrm_state_type is_received_mrm_state; + 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; From 3bf52e32e1cbf4798691d0d34a0224dbb7b93c90 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 27 Jun 2024 08:24:34 +0000 Subject: [PATCH 10/14] style(pre-commit): autofix --- .../src/trajectory_repeater.cpp | 15 +++++++++------ .../src/trajectory_repeater.hpp | 9 ++++----- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/planning/trajectory_repeater/src/trajectory_repeater.cpp b/planning/trajectory_repeater/src/trajectory_repeater.cpp index af6852c70398a..0b1b7880f9a17 100644 --- a/planning/trajectory_repeater/src/trajectory_repeater.cpp +++ b/planning/trajectory_repeater/src/trajectory_repeater.cpp @@ -24,10 +24,12 @@ TrajectoryRepeater::TrajectoryRepeater(const rclcpp::NodeOptions & node_options) // Subscriber sub_trajectory_ = create_subscription( - "~/input/trajectory", 10, std::bind(&TrajectoryRepeater::onTrajectory, this, std::placeholders::_1)); + "~/input/trajectory", 10, + std::bind(&TrajectoryRepeater::onTrajectory, this, std::placeholders::_1)); // Publisher - pub_trajectory_ = create_publisher("~/output/trajectory", 10); + pub_trajectory_ = + create_publisher("~/output/trajectory", 10); // Service @@ -35,15 +37,16 @@ TrajectoryRepeater::TrajectoryRepeater(const rclcpp::NodeOptions & node_options) // Timer using namespace std::literals::chrono_literals; - timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrajectoryRepeater::onTimer, this)); + timer_ = + rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrajectoryRepeater::onTimer, this)); // State // Diagnostics - } -void TrajectoryRepeater::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +void TrajectoryRepeater::onTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { last_trajectory_ = msg; } @@ -54,7 +57,7 @@ void TrajectoryRepeater::onTimer() RCLCPP_DEBUG(get_logger(), "No trajectory received"); return; } - + pub_trajectory_->publish(*last_trajectory_); } diff --git a/planning/trajectory_repeater/src/trajectory_repeater.hpp b/planning/trajectory_repeater/src/trajectory_repeater.hpp index 4b6d47823818b..83e9b396dae81 100644 --- a/planning/trajectory_repeater/src/trajectory_repeater.hpp +++ b/planning/trajectory_repeater/src/trajectory_repeater.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ -#define TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ +#ifndef TRAJECTORY_REPEATER_HPP_ +#define TRAJECTORY_REPEATER_HPP_ // include #include @@ -46,15 +46,14 @@ class TrajectoryRepeater : public rclcpp::Node // Timer rclcpp::TimerBase::SharedPtr timer_; - + void onTimer(); // State autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_trajectory_; // Diagnostics - }; } // namespace trajectory_repeater -#endif // TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ \ No newline at end of file +#endif // TRAJECTORY_REPEATER_HPP_ From 0eaad5efe9745e14f513960e9987f92deed62437 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 28 Jun 2024 01:33:55 +0900 Subject: [PATCH 11/14] modify: topic name Signed-off-by: TetsuKawa --- .../launch/leader_election_converter.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/leader_election_converter/launch/leader_election_converter.launch.xml b/system/leader_election_converter/launch/leader_election_converter.launch.xml index 901bd499ad2c6..2078fc7fd7d42 100644 --- a/system/leader_election_converter/launch/leader_election_converter.launch.xml +++ b/system/leader_election_converter/launch/leader_election_converter.launch.xml @@ -5,8 +5,8 @@ - - + + From c8885e5b71bc70e11a12b1b16171d289c854a72f Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 28 Jun 2024 13:46:38 +0900 Subject: [PATCH 12/14] feat(dummy_operation_mode_publisher): add dummy operation mode publisher Signed-off-by: Makoto Kurihara --- .../CMakeLists.txt | 21 +++++++ .../dummy_operation_mode_publisher/README.md | 23 +++++++ .../dummy_operation_mode_publisher.param.yaml | 2 + .../dummy_operation_mode_publisher.launch.xml | 7 +++ .../package.xml | 25 ++++++++ .../src/dummy_operation_mode_publisher.cpp | 63 +++++++++++++++++++ .../src/dummy_operation_mode_publisher.hpp | 57 +++++++++++++++++ 7 files changed, 198 insertions(+) create mode 100644 dummy/dummy_operation_mode_publisher/CMakeLists.txt create mode 100644 dummy/dummy_operation_mode_publisher/README.md create mode 100644 dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml create mode 100644 dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml create mode 100644 dummy/dummy_operation_mode_publisher/package.xml create mode 100644 dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp create mode 100644 dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp diff --git a/dummy/dummy_operation_mode_publisher/CMakeLists.txt b/dummy/dummy_operation_mode_publisher/CMakeLists.txt new file mode 100644 index 0000000000000..d682cd8198e4e --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.14) +project(dummy_operation_mode_publisher) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(dummy_operation_mode_publisher SHARED + src/dummy_operation_mode_publisher.cpp +) +ament_target_dependencies(dummy_operation_mode_publisher) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "dummy_operation_mode_publisher::DummyOperationModePublisher" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/dummy/dummy_operation_mode_publisher/README.md b/dummy/dummy_operation_mode_publisher/README.md new file mode 100644 index 0000000000000..e67018a331dc5 --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/README.md @@ -0,0 +1,23 @@ +# Dummy Operation Mode Publisher + +## Purpose + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +### Output + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml b/dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml new file mode 100644 index 0000000000000..a20dbd7ffd3d9 --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml @@ -0,0 +1,2 @@ +/**: + ros__parameters: diff --git a/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml b/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml new file mode 100644 index 0000000000000..ba1525699940b --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/dummy/dummy_operation_mode_publisher/package.xml b/dummy/dummy_operation_mode_publisher/package.xml new file mode 100644 index 0000000000000..5cc8fc63c70e8 --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/package.xml @@ -0,0 +1,25 @@ + + + + dummy_operation_mode_publisher + 0.1.0 + The dummy_operation_mode_publisher package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + autoware_adapi_v1_msgs + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp new file mode 100644 index 0000000000000..bb5c5ba377083 --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp @@ -0,0 +1,63 @@ +// Copyright 2024 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 "dummy_operation_mode_publisher.hpp" + +namespace dummy_operation_mode_publisher +{ + +DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptions & node_options) +: Node("dummy_operation_mode_publisher", node_options) +{ + // Parameter + + // Subscriber + + // Publisher + pub_operation_mode_state_ = create_publisher( + "~/output/operation_mode_state", 10); + + // Service + + // Client + + // Timer + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this)); + + // State + + // Diagnostics + +} + +void DummyOperationModePublisher::onTimer() +{ + autoware_adapi_v1_msgs::msg::OperationModeState msg; + msg.stamp = this->now(); + msg.mode = autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS; + msg.is_autonomous_mode_available = true; + msg.is_in_transition = false; + msg.is_stop_mode_available = true; + msg.is_autonomous_mode_available = true; + msg.is_local_mode_available = true; + msg.is_remote_mode_available = true; + + pub_operation_mode_state_->publish(msg); +} + +} // namespace dummy_operation_mode_publisher + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(dummy_operation_mode_publisher::DummyOperationModePublisher) diff --git a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp new file mode 100644 index 0000000000000..70020d88a762e --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 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 DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ +#define DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ + +// include +#include + +#include + + +namespace dummy_operation_mode_publisher +{ + +class DummyOperationModePublisher : public rclcpp::Node +{ +public: + explicit DummyOperationModePublisher(const rclcpp::NodeOptions & node_options); + ~DummyOperationModePublisher() = default; + +private: + // Parameter + + // Subscriber + + // Publisher + rclcpp::Publisher::SharedPtr pub_operation_mode_state_; + + // Service + + // Client + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + void onTimer(); + + // State + + // Diagnostics + +}; +} // namespace dummy_operation_mode_publisher + +#endif // DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ \ No newline at end of file From ce6dc8735acf8aec358d91bf4b4beca21402c83a Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 28 Jun 2024 13:47:00 +0900 Subject: [PATCH 13/14] chore(trajectory_repeater): change remap future work: parameterize Signed-off-by: Makoto Kurihara --- .../trajectory_repeater/launch/trajectory_repeater.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml b/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml index 7f4a4711e85c3..ebbde691c32d4 100644 --- a/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml +++ b/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml @@ -3,6 +3,6 @@ - + From 70daa6de7c110cbed120c54fb562898907333ecc Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 1 Jul 2024 16:22:36 +0900 Subject: [PATCH 14/14] feat(mrm_stop_operator): add mrm stop operator Signed-off-by: Makoto Kurihara --- system/mrm_stop_operator/CMakeLists.txt | 21 +++++ system/mrm_stop_operator/README.md | 23 ++++++ .../config/mrm_stop_operator.param.yaml | 6 ++ .../launch/mrm_stop_operator.launch.xml | 11 +++ system/mrm_stop_operator/package.xml | 26 ++++++ .../src/mrm_stop_operator.cpp | 80 +++++++++++++++++++ .../src/mrm_stop_operator.hpp | 72 +++++++++++++++++ 7 files changed, 239 insertions(+) create mode 100644 system/mrm_stop_operator/CMakeLists.txt create mode 100644 system/mrm_stop_operator/README.md create mode 100644 system/mrm_stop_operator/config/mrm_stop_operator.param.yaml create mode 100644 system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml create mode 100644 system/mrm_stop_operator/package.xml create mode 100644 system/mrm_stop_operator/src/mrm_stop_operator.cpp create mode 100644 system/mrm_stop_operator/src/mrm_stop_operator.hpp diff --git a/system/mrm_stop_operator/CMakeLists.txt b/system/mrm_stop_operator/CMakeLists.txt new file mode 100644 index 0000000000000..f66488c659806 --- /dev/null +++ b/system/mrm_stop_operator/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.14) +project(mrm_stop_operator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(mrm_stop_operator SHARED + src/mrm_stop_operator.cpp +) +ament_target_dependencies(mrm_stop_operator) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "mrm_stop_operator::MrmStopOperator" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/system/mrm_stop_operator/README.md b/system/mrm_stop_operator/README.md new file mode 100644 index 0000000000000..4407161d510bd --- /dev/null +++ b/system/mrm_stop_operator/README.md @@ -0,0 +1,23 @@ +# Mrm Stop Operator + +## Purpose + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +### Output + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml new file mode 100644 index 0000000000000..5dfbb2956d668 --- /dev/null +++ b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + min_acceleration: -4.0 # min acceleration for sub ecu mrm stop [m/s^2] + max_jerk: 5.0 # max jerk for sub ecu mrm stop [m/s^3] + min_jerk: -5.0 # min jerk for sub ecu mrm stop [m/s^3] + diff --git a/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml new file mode 100644 index 0000000000000..9d5226cf99f19 --- /dev/null +++ b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/system/mrm_stop_operator/package.xml b/system/mrm_stop_operator/package.xml new file mode 100644 index 0000000000000..f3822268496b0 --- /dev/null +++ b/system/mrm_stop_operator/package.xml @@ -0,0 +1,26 @@ + + + + mrm_stop_operator + 0.1.0 + The mrm_stop_operator package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + rclcpp + rclcpp_components + tier4_planning_msgs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.cpp b/system/mrm_stop_operator/src/mrm_stop_operator.cpp new file mode 100644 index 0000000000000..d403e886ed7a1 --- /dev/null +++ b/system/mrm_stop_operator/src/mrm_stop_operator.cpp @@ -0,0 +1,80 @@ +// Copyright 2024 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 "mrm_stop_operator.hpp" + +namespace mrm_stop_operator +{ + +MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options) +: Node("mrm_stop_operator", node_options) +{ + // Parameter + params_.min_acceleration = declare_parameter("min_acceleration", -4.0); + params_.max_jerk = declare_parameter("max_jerk", 5.0); + params_.min_jerk = declare_parameter("min_jerk", -5.0); + + // Subscriber + sub_mrm_request_ = create_subscription( + "~/input/mrm_request", 10, + std::bind(&MrmStopOperator::onMrmRequest, this, std::placeholders::_1)); + + // Publisher + pub_velocity_limit_ = create_publisher( + "~/output/velocity_limit", rclcpp::QoS{10}.transient_local()); + pub_velocity_limit_clear_command_ = create_publisher( + "~/output/velocity_limit_clear_command", rclcpp::QoS{10}.transient_local()); + + // Timer + + // Service + + // Client + + // Timer + + // State + initState(); + + // Diagnostics + +} + +void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg) +{ + if (msg->type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP && + last_mrm_request_.type != tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) { + tier4_planning_msgs::msg::VelocityLimit velocity_limit; + velocity_limit.stamp = this->now(); + velocity_limit.max_velocity = 0.0; + velocity_limit.use_constraints = true; + velocity_limit.constraints.min_acceleration = params_.min_acceleration; + velocity_limit.constraints.max_jerk = params_.max_jerk; + velocity_limit.constraints.min_jerk = params_.min_jerk; + velocity_limit.sender = "mrm_stop_operator"; + pub_velocity_limit_->publish(velocity_limit); + + last_mrm_request_ = *msg; + } +} + +void MrmStopOperator::initState() +{ + last_mrm_request_.type = tier4_system_msgs::msg::MrmBehavior::NONE; +} + +} // namespace mrm_stop_operator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mrm_stop_operator::MrmStopOperator) diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.hpp b/system/mrm_stop_operator/src/mrm_stop_operator.hpp new file mode 100644 index 0000000000000..750c343f13062 --- /dev/null +++ b/system/mrm_stop_operator/src/mrm_stop_operator.hpp @@ -0,0 +1,72 @@ +// Copyright 2024 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 MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ +#define MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ + +// include +#include + +#include +#include +#include + +namespace mrm_stop_operator +{ + +struct Parameters +{ + double min_acceleration; + double max_jerk; + double min_jerk; +}; + +class MrmStopOperator : public rclcpp::Node +{ +public: + explicit MrmStopOperator(const rclcpp::NodeOptions & node_options); + ~MrmStopOperator() = default; + +private: + // Parameter + Parameters params_; + + // Subscriber + rclcpp::Subscription::SharedPtr sub_mrm_request_; + + void onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg); + + // Service + + // Publisher + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_clear_command_; + + // Service + + // Client + + // Timer + + // State + tier4_system_msgs::msg::MrmBehavior last_mrm_request_; + + void initState(); + + // Diagnostics + +}; +} // namespace mrm_stop_operator + +#endif // MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ \ No newline at end of file