Merge pull request autowarefoundation#1350 from tier4/feat/add-leader…

…-election-converter

planning/trajectory_repeater/src/trajectory_repeater.cpp

@@ -24,26 +24,29 @@ TrajectoryRepeater::TrajectoryRepeater(const rclcpp::NodeOptions & node_options)
 
   // Subscriber
   sub_trajectory_ = create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
-    "~/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<autoware_auto_planning_msgs::msg::Trajectory>("~/output/trajectory", 10);
+  pub_trajectory_ =
+    create_publisher<autoware_auto_planning_msgs::msg::Trajectory>("~/output/trajectory", 10);
 
   // Service
 
   // Client
 
   // 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_);
 }
 

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 <rclcpp/rclcpp.hpp>
@@ -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_
+#endif  // TRAJECTORY_REPEATER_HPP_

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,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.
+
+#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).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<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(node_->get_logger(), "Failed to take control mode report");
+  }
+}
+
+}  // namespace leader_election_converter