feat(leader_election_converter): mrm v0.6 add leader election convert…

…er based on x2 v3.0.0 (#1388)

* feat: add leader election converter node

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* fix: remove transient local setting from mrm state subscriber

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

---------

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

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)

system/leader_election_converter/README.md

@@ -0,0 +1,50 @@
+# 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 converter
+
+The mrm converter subscribes `/system/fail_safe/mrm_state` into a structure called `MrmState` and sends it as a UDP packet.
+In addition, it receives a udp packet`MrmState` and publish `/system/mrm_request`.
+
+### Interface
+
+| Interface Type | Interface Name                 | Data Type                           | Description              |
+| -------------- | ------------------------------ | ----------------------------------- | ------------------------ |
+| subscriber     | `/system/fail_safe/mrm_state`  | `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 among election nodes. |
+| udp receiver   | none                                   | `struct ElectionStatus`                       | Leader Election status.        |
+| publisher      | `/system/election/communication`       | `tier4_system_msgs/msg/ElectionCommunication` | messages among election nodes. |
+| publisher      | `/system/election/status`              | `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
+
+{{ json_to_markdown("system/leader_election_converter/schema/leader_election_converter.schema.json") }}

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"

system/leader_election_converter/launch/leader_election_converter.launch.xml

@@ -0,0 +1,20 @@
+<launch>
+  <arg name="param_file" default="$(find-pkg-share leader_election_converter)/config/leader_election_converter.param.yaml"/>
+  <arg name="input_control_mode" default="/vehicle/status/control_mode"/>
+  <arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>
+  <arg name="input_mrm_state" default="/system/fail_safe/mrm_state"/>
+  <arg name="output_mrm_request" default="/system/mrm_request"/>
+  <arg name="output_over_all_mrm_state" default="/system/fail_safe/over_all/mrm_state"/>
+  <arg name="output_election_communication" default="/system/election/communication"/>
+  <arg name="output_election_status" default="/system/election/status"/>
+  <node pkg="leader_election_converter" exec="leader_election_converter_node">
+    <param from="$(var param_file)"/>
+    <remap from="~/input/control_mode" to="$(var input_control_mode)"/>
+    <remap from="~/input/operation_mode_availability" to="$(var input_operation_mode_availability)"/>
+    <remap from="~/input/mrm_state" to="$(var input_mrm_state)"/>
+    <remap from="~/output/mrm_request" to="$(var output_mrm_request)"/>
+    <remap from="~/output/over_all_mrm_state" to="$(var output_over_all_mrm_state)"/>
+    <remap from="~/output/election_communication" to="$(var output_election_communication)"/>
+    <remap from="~/output/election_status" to="$(var output_election_status)"/>
+  </node>
+</launch>

system/leader_election_converter/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>leader_election_converter</name>
+  <version>0.1.0</version>
+  <description>The leader election converter package</description>
+  <maintainer email="tetsuhiro.kawaguchi@tier4.jp">TetsuKawa</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_system_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 availability",
+          "default": "127.0.0.1"
+        },
+        "availability_dest_port": {
+          "type": "string",
+          "description": "Port of the destination of availability",
+          "default": "9000"
+        },
+        "mrm_state_dest_ip": {
+          "type": "string",
+          "description": "IP address of the destination of mrm_state",
+          "default": "127.0.0.1"
+        },
+        "mrm_state_dest_port": {
+          "type": "string",
+          "description": "Port of the destination of mrm_state",
+          "default": "9001"
+        },
+        "mrm_request_src_ip": {
+          "type": "string",
+          "description": "IP address of the source of mrm_request",
+          "default": "127.0.0.1"
+        },
+        "mrm_request_src_port": {
+          "type": "string",
+          "description": "Port of the source of mrm_request",
+          "default": "9002"
+        },
+        "election_communication_src_ip": {
+          "type": "string",
+          "description": "IP address of the source of election_communication",
+          "default": "127.0.0.1"
+        },
+        "election_communication_src_port": {
+          "type": "string",
+          "description": "Port of the source of election_communication",
+          "default": "9003"
+        },
+        "election_status_src_ip": {
+          "type": "string",
+          "description": "IP address of the source of election_status",
+          "default": "127.0.0.1"
+        },
+        "election_status_src_port": {
+          "type": "string",
+          "description": "Port of the source of election_status",
+          "default": "9004"
+        }
+      },
+      "required": [
+        "availability_dest_ip",
+        "availability_dest_port",
+        "mrm_state_dest_ip",
+        "mrm_state_dest_port",
+        "mrm_request_src_ip",
+        "mrm_request_src_port",
+        "election_communication_src_ip",
+        "election_communication_src_port",
+        "election_status_src_ip",
+        "election_status_src_port"
+      ],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/leader_election_converter"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

system/leader_election_converter/src/common/converter/availability_converter.cpp

@@ -0,0 +1,83 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "availability_converter.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <memory>
+#include <string>
+
+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<UdpSender<Availability>>(dest_ip, dest_port);
+}
+
+void AvailabilityConverter::setSubscriber()
+{
+  const auto qos = rclcpp::QoS(1);
+  availability_callback_group_ =
+    node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  rclcpp::SubscriptionOptions availability_options = rclcpp::SubscriptionOptions();
+  availability_options.callback_group = availability_callback_group_;
+
+  control_mode_callback_group_ =
+    node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  rclcpp::SubscriptionOptions control_mode_options = rclcpp::SubscriptionOptions();
+  control_mode_options.callback_group = control_mode_callback_group_;
+  auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg::
+                                    ControlModeReport::ConstSharedPtr msg) {};
+
+  sub_operation_mode_availability_ =
+    node_->create_subscription<tier4_system_msgs::msg::OperationModeAvailability>(
+      "~/input/operation_mode_availability", qos,
+      std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1),
+      availability_options);
+
+  sub_control_mode_ =
+    node_->create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
+      "~/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<autoware_auto_vehicle_msgs::msg::ControlModeReport>();
+  rclcpp::MessageInfo message_info;
+  const bool success = sub_control_mode_->take(*control_mode_report, message_info);
+  if (success) {
+    Availability availability;
+    availability.mode = control_mode_report->mode;
+    availability.stop = availability_msg->stop;
+    availability.autonomous = availability_msg->autonomous;
+    availability.local = availability_msg->local;
+    availability.remote = availability_msg->remote;
+    availability.emergency_stop = availability_msg->emergency_stop;
+    availability.comfortable_stop = availability_msg->comfortable_stop;
+    availability.pull_over = availability_msg->pull_over;
+    udp_availability_sender_->send(availability);
+  } else {
+    RCLCPP_ERROR_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 5000, "Failed to take control mode report");
+  }
+}
+
+}  // namespace leader_election_converter

system/leader_election_converter/src/common/converter/availability_converter.hpp

@@ -0,0 +1,67 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_
+#define COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_
+
+#include "udp_sender.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
+#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
+
+#include <memory>
+#include <string>
+
+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:
+  explicit AvailabilityConverter(rclcpp::Node * node);
+
+  void setUdpSender(const std::string & dest_ip, const std::string & dest_port);
+  void setSubscriber();
+  void convertToUdp(
+    const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg);
+
+private:
+  rclcpp::Node * node_;
+  std::unique_ptr<UdpSender<Availability>> udp_availability_sender_;
+  rclcpp::CallbackGroup::SharedPtr availability_callback_group_;
+  rclcpp::CallbackGroup::SharedPtr control_mode_callback_group_;
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
+    sub_control_mode_;
+  rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
+    sub_operation_mode_availability_;
+};
+
+}  // namespace leader_election_converter
+
+#endif  // COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_