feat: add redundancy switcher interface

Signed-off-by: TetsuKawa <[email protected]>

system/redundancy_switcher_interface/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.14)
+project(redundancy_switcher_interface)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(common_converter SHARED
+  src/common/converter/availability_converter.cpp
+  src/common/converter/mrm_converter.cpp
+  src/common/converter/log_converter.cpp
+)
+
+target_include_directories(common_converter PRIVATE
+  src/common/converter
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/node/redundancy_switcher_interface.cpp
+)
+target_include_directories(${PROJECT_NAME} PRIVATE src/common/converter)
+
+target_link_libraries(${PROJECT_NAME} common_converter)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "redundancy_switcher_interface::RedundancySwitcherInterface"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR MultiThreadedExecutor
+)
+
+install(PROGRAMS
+  script/relay_to_sub.py
+  DESTINATION lib/${PROJECT_NAME}
+  PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ
+)
+
+ament_auto_package(INSTALL_TO_SHARE config launch)

system/redundancy_switcher_interface/README.md

@@ -0,0 +1,50 @@
+# redundancy_switcher_interface
+
+## Overview
+
+The redundancy switcher interface node is responsible for relaying UDP packets and ROS2 topics between the redundancy_switcher invoked by systemd and Autoware executed on ROS2.
+
+## availability converter
+
+The availability converter subscribes `/system/operation_mode/availability` and `/vehicle/status/mrm_state`, adds them together into a structure called `Availability` and sends it as a udp packet.
+
+### Interface
+
+| Interface Type | Interface Name                        | Data Type                                          | Description                   |
+| -------------- | ------------------------------------- | -------------------------------------------------- | ----------------------------- |
+| subscriber     | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability`  | Usable behavior of the ego.   |
+| subscriber     | `/vehicle/status/mrm_state`           | `autoware_vehicle_msgs/msg/ControlModeReport` | Ego control mode.             |
+| udp sender     | none                                  | `struct Availability`                              | Combination of the above two. |
+
+## mrm converter
+
+The mrm converter subscribes `/system/fail_safe/mrm_state` into a structure called `MrmState` and sends it as a UDP packet.
+In addition, it receives a udp packet`MrmState` and publish `/system/mrm_request`.
+
+### Interface
+
+| Interface Type | Interface Name                 | Data Type                             | Description              |
+| -------------- | ------------------------------ | ------------------------------------- | ------------------------ |
+| subscriber     | `/system/fail_safe/mrm_state`  | `autoware_adapi_v1_msgs/msg/MrmState` | MRM status of each ECU.  |
+| udp sender     | none                           | `struct MrmState`                     | Same as above.           |
+| publisher      | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior`   | Request of MRM behavior. |
+| udp receiver   | none                           | `struct MrmRequest`                   | Same as above.           |
+
+## log converter
+
+The log　converter receive udp packets into a structure called `ElectionCommunication` and `ElectionStatus`, and publish `/system/election/communication`,
+`/system/election/status`, and `/system/fail_safe/over_all/mrm_state`.
+
+### Interface
+
+| Interface Type | Interface Name                         | Data Type                                     | Description                    |
+| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------ |
+| udp receiver   | none                                   | `struct ElectionCommunication`                | messages among election nodes. |
+| udp receiver   | none                                   | `struct ElectionStatus`                       | Leader Election status.        |
+| publisher      | `/system/election/communication`       | `tier4_system_msgs/msg/ElectionCommunication` | messages among election nodes. |
+| publisher      | `/system/election/status`              | `autoware_adapi_v1_msgs/msg/MrmState`              | Leader Election status.        |
+| publisher      | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state`        | System-wide MRM status.        |
+
+## Parameters
+
+{{ json_to_markdown("system/redundancy_switcher_interface/schema/redundancy_switcher_interface.schema.json") }}

system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml

@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    availability_dest_ip: "127.0.0.1"
+    availability_dest_port: "9000"
+    mrm_state_dest_ip: "127.0.0.1"
+    mrm_state_dest_port: "9001"
+    mrm_request_src_ip: "127.0.0.1"
+    mrm_request_src_port: "9002"
+    election_communication_src_ip: "127.0.0.1"
+    election_communication_src_port: "9003"
+    election_status_src_ip: "127.0.0.1"
+    election_status_src_port: "9004"

system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml

@@ -0,0 +1,20 @@
+<launch>
+  <arg name="param_file" default="$(find-pkg-share redundancy_switcher_interface)/config/redundancy_switcher_interface.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="redundancy_switcher_interface" exec="redundancy_switcher_interface_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/redundancy_switcher_interface/package.xml

@@ -0,0 +1,27 @@
+<?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>redundancy_switcher_interface</name>
+  <version>0.1.0</version>
+  <description>The redundancy switcher interface package</description>
+  <maintainer email="[email protected]">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_planning_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
+  <depend>geometry_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/redundancy_switcher_interface/schema/redundancy_switcher_interface.schema.json

@@ -0,0 +1,89 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for redundancy switcher interface",
+  "type": "object",
+  "definitions": {
+    "redundancy_switcher_interface": {
+      "type": "object",
+      "properties": {
+        "availability_dest_ip": {
+          "type": "string",
+          "description": "IP address of the destination of availability",
+          "default": "127.0.0.1"
+        },
+        "availability_dest_port": {
+          "type": "string",
+          "description": "Port of the destination of availability",
+          "default": "9000"
+        },
+        "mrm_state_dest_ip": {
+          "type": "string",
+          "description": "IP address of the destination of mrm_state",
+          "default": "127.0.0.1"
+        },
+        "mrm_state_dest_port": {
+          "type": "string",
+          "description": "Port of the destination of mrm_state",
+          "default": "9001"
+        },
+        "mrm_request_src_ip": {
+          "type": "string",
+          "description": "IP address of the source of mrm_request",
+          "default": "127.0.0.1"
+        },
+        "mrm_request_src_port": {
+          "type": "string",
+          "description": "Port of the source of mrm_request",
+          "default": "9002"
+        },
+        "election_communication_src_ip": {
+          "type": "string",
+          "description": "IP address of the source of election_communication",
+          "default": "127.0.0.1"
+        },
+        "election_communication_src_port": {
+          "type": "string",
+          "description": "Port of the source of election_communication",
+          "default": "9003"
+        },
+        "election_status_src_ip": {
+          "type": "string",
+          "description": "IP address of the source of election_status",
+          "default": "127.0.0.1"
+        },
+        "election_status_src_port": {
+          "type": "string",
+          "description": "Port of the source of election_status",
+          "default": "9004"
+        }
+      },
+      "required": [
+        "availability_dest_ip",
+        "availability_dest_port",
+        "mrm_state_dest_ip",
+        "mrm_state_dest_port",
+        "mrm_request_src_ip",
+        "mrm_request_src_port",
+        "election_communication_src_ip",
+        "election_communication_src_port",
+        "election_status_src_ip",
+        "election_status_src_port"
+      ],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/redundancy_switcher_interface"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

system/redundancy_switcher_interface/script/relay_to_sub.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+
+from autoware_adapi_v1_msgs.msg import OperationModeState
+from autoware_planning_msgs.msg import Trajectory
+from geometry_msgs.msg import PoseWithCovarianceStamped
+import rclpy
+from rclpy.node import Node
+from tier4_system_msgs.msg import OperationModeAvailability
+
+
+class RemapNode(Node):
+    def __init__(self):
+        super().__init__("remap_node")
+
+        self.create_subscription(
+            OperationModeAvailability,
+            "/system/operation_mode/availability",
+            self.operation_mode_callback,
+            10,
+        )
+        self.create_subscription(
+            OperationModeState,
+            "/system/operation_mode/state",
+            self.operation_mode_state_callback,
+            10,
+        )
+        self.sub_trajectory = self.create_subscription(
+            Trajectory, "/planning/scenario_planning/trajectory", self.trajectory_callback, 10
+        )
+        self.sub_pose_with_covariance = self.create_subscription(
+            PoseWithCovarianceStamped, "/localization/pose_with_covariance", self.pose_callback, 10
+        )
+        # self.sub_initialpose3d = self.create_subscription(PoseWithCovarianceStamped, '/initialpose3d', self.initialpose_callback, 10)
+
+        self.pub_trajectory = self.create_publisher(
+            Trajectory, "/to_sub/planning/scenario_planning/trajectory", 10
+        )
+        self.pub_pose_with_covariance = self.create_publisher(
+            PoseWithCovarianceStamped, "/to_sub/localization/pose_with_covariance", 10
+        )
+        # self.pub_initialpose3d = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/initialpose3d', 10)
+
+        self.autonomous_mode = False
+        self.operation_mode_autonomous_state = False
+        self.get_logger().info(f"Initial autonomous mode: {self.autonomous_mode}")
+        self.tmp_operation_mode_autonomous_state = False
+
+    def operation_mode_callback(self, msg):
+        if msg.autonomous != self.autonomous_mode:
+            self.autonomous_mode = msg.autonomous
+            self.get_logger().info(f"Autonomous mode changed: {self.autonomous_mode}")
+
+    def operation_mode_state_callback(self, msg):
+        self.tmp_operation_mode_autonomous_state = self.operation_mode_autonomous_state
+        if msg.mode == 2:
+            self.operation_mode_autonomous_state = True
+            if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state:
+                self.get_logger().info(f"Operation mode changed: {self.operation_mode_autonomous_state}")
+        else:
+            self.operation_mode_autonomous_state = False
+            if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state:
+                self.get_logger().info(f"Operation mode changed: {self.operation_mode_autonomous_state}")
+
+    def trajectory_callback(self, msg):
+        if self.autonomous_mode or self.operation_mode_autonomous_state == False:
+            self.pub_trajectory.publish(msg)
+
+    def pose_callback(self, msg):
+        if self.autonomous_mode or self.operation_mode_autonomous_state == False:
+            self.pub_pose_with_covariance.publish(msg)
+
+    # def initialpose_callback(self, msg):
+    #     if self.autonomous_mode:
+    #         self.pub_initialpose3d.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RemapNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()