Merge branch 'feat/add-mrm-v0.6-launch-based-on-v3.0.0' into feat/add…

…-control-cmd-switcher

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
+)

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

dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml

@@ -0,0 +1,2 @@
+/**:
+  ros__parameters:

dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="dummy_operation_mode_publisher_param_path" default="$(find-pkg-share dummy_operation_mode_publisher)/config/dummy_operation_mode_publisher.param.yaml"/>
+
+  <node pkg="dummy_operation_mode_publisher" exec="dummy_operation_mode_publisher_node" name="dummy_operation_mode_publisher" output="screen">
+    <remap from="~/output/operation_mode_state" to="/system/operation_mode/state"/>
+  </node>
+</launch>

dummy/dummy_operation_mode_publisher/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>dummy_operation_mode_publisher</name>
+  <version>0.1.0</version>
+  <description>The dummy_operation_mode_publisher package</description>
+  <maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <!-- depend -->
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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<autoware_adapi_v1_msgs::msg::OperationModeState>(
+    "~/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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(dummy_operation_mode_publisher::DummyOperationModePublisher)

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+
+
+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<autoware_adapi_v1_msgs::msg::OperationModeState>::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_

planning/trajectory_repeater/launch/trajectory_repeater.launch.xml

@@ -3,6 +3,6 @@
 
   <node pkg="trajectory_repeater" exec="trajectory_repeater_node" name="trajectory_repeater" output="screen">
     <remap from="~/input/trajectory" to="/main/planning/scenario_planning/trajectory"/>
-    <remap from="~/output/trajectory" to="/planning/scenario_planning/trajectory"/>
+    <remap from="~/output/trajectory" to="/planning/trajectory_repeater/trajectory"/>
   </node>
 </launch>

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"