Skip to content

Commit 8501fcd

Browse files
TetsuKawapre-commit-ci[bot]
TetsuKawa
and
pre-commit-ci[bot]
authored
feat: add redundancy switcher interface mrm v0.6 for gen2 (#1756)
* feat: add redundancy switcher interface Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * style(pre-commit): autofix * feat(redundancy_switcher_interface): add reset_converter Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * style(pre-commit): autofix * style(pre-commit): autofix * modify: python grammer Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: response to spell check Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: modify schema Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> --------- Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 204d745 commit 8501fcd

19 files changed

+1451
-0
lines changed

‎system/redundancy_switcher_interface/CMakeLists.txt

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(redundancy_switcher_interface)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(common_converter SHARED
8+
src/common/converter/availability_converter.cpp
9+
src/common/converter/mrm_converter.cpp
10+
src/common/converter/log_converter.cpp
11+
src/common/converter/reset_converter.cpp
12+
)
13+
14+
target_include_directories(common_converter PRIVATE
15+
src/common/converter
16+
)
17+
18+
ament_auto_add_library(${PROJECT_NAME} SHARED
19+
src/node/redundancy_switcher_interface.cpp
20+
)
21+
target_include_directories(${PROJECT_NAME} PRIVATE src/common/converter)
22+
23+
target_link_libraries(${PROJECT_NAME} common_converter)
24+
25+
rclcpp_components_register_node(${PROJECT_NAME}
26+
PLUGIN "redundancy_switcher_interface::RedundancySwitcherInterface"
27+
EXECUTABLE ${PROJECT_NAME}_node
28+
EXECUTOR MultiThreadedExecutor
29+
)
30+
31+
install(PROGRAMS
32+
script/relay_to_sub.py
33+
DESTINATION lib/${PROJECT_NAME}
34+
PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ
35+
)
36+
37+
ament_auto_package(INSTALL_TO_SHARE config launch)

‎system/redundancy_switcher_interface/README.md

+50
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
# redundancy_switcher_interface
2+
3+
## Overview
4+
5+
The redundancy switcher interface node is responsible for relaying UDP packets and ros topics between the redundancy_switcher invoked by systemd and Autoware executed on ros.
6+
7+
## availability converter
8+
9+
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.
10+
11+
### Interface
12+
13+
| Interface Type | Interface Name | Data Type | Description |
14+
| -------------- | ------------------------------------- | ------------------------------------------------- | ----------------------------- |
15+
| subscriber | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability` | Usable behavior of the ego. |
16+
| subscriber | `/vehicle/status/mrm_state` | `autoware_vehicle_msgs/msg/ControlModeReport` | Ego control mode. |
17+
| udp sender | none | `struct Availability` | Combination of the above two. |
18+
19+
## mrm converter
20+
21+
The mrm converter subscribes `/system/fail_safe/mrm_state` into a structure called `MrmState` and sends it as a UDP packet.
22+
In addition, it receives a udp packet`MrmState` and publish `/system/mrm_request`.
23+
24+
### Interface
25+
26+
| Interface Type | Interface Name | Data Type | Description |
27+
| -------------- | ------------------------------ | ------------------------------------- | ------------------------ |
28+
| subscriber | `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs/msg/MrmState` | MRM status of each ECU. |
29+
| udp sender | none | `struct MrmState` | Same as above. |
30+
| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. |
31+
| udp receiver | none | `struct MrmRequest` | Same as above. |
32+
33+
## log converter
34+
35+
The log　converter receive udp packets into a structure called `ElectionCommunication` and `ElectionStatus`, and publish `/system/election/communication`,
36+
`/system/election/status`, and `/system/fail_safe/over_all/mrm_state`.
37+
38+
### Interface
39+
40+
| Interface Type | Interface Name | Data Type | Description |
41+
| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------ |
42+
| udp receiver | none | `struct ElectionCommunication` | messages among election nodes. |
43+
| udp receiver | none | `struct ElectionStatus` | Leader Election status. |
44+
| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages among election nodes. |
45+
| publisher | `/system/election/status` | `autoware_adapi_v1_msgs/msg/MrmState` | Leader Election status. |
46+
| publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wide MRM status. |
47+
48+
## Parameters
49+
50+
{{ json_to_markdown("system/redundancy_switcher_interface/schema/redundancy_switcher_interface.schema.json") }}

‎system/redundancy_switcher_interface/config/redundancy_switcher_interface.param.yaml

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
/**:
2+
ros__parameters:
3+
availability_dest_ip: "127.0.0.1"
4+
availability_dest_port: "59000"
5+
mrm_state_dest_ip: "127.0.0.1"
6+
mrm_state_dest_port: "59001"
7+
mrm_request_src_ip: "127.0.0.1"
8+
mrm_request_src_port: "59002"
9+
election_communication_src_ip: "127.0.0.1"
10+
election_communication_src_port: "59003"
11+
election_status_src_ip: "127.0.0.1"
12+
election_status_src_port: "59004"
13+
reset_request_dest_ip: "127.0.0.1"
14+
reset_request_dest_port: "59005"
15+
reset_response_src_ip: "127.0.0.1"
16+
reset_response_src_port: "59006"

‎system/redundancy_switcher_interface/launch/redundancy_switcher_interface.launch.xml

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<launch>
2+
<arg name="param_file" default="$(find-pkg-share redundancy_switcher_interface)/config/redundancy_switcher_interface.param.yaml"/>
3+
<arg name="input_control_mode" default="/vehicle/status/control_mode"/>
4+
<arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>
5+
<arg name="input_mrm_state" default="/system/fail_safe/mrm_state"/>
6+
<arg name="output_mrm_request" default="/system/mrm_request"/>
7+
<arg name="output_over_all_mrm_state" default="/system/fail_safe/over_all/mrm_state"/>
8+
<arg name="output_election_communication" default="/system/election/communication"/>
9+
<arg name="output_election_status" default="/system/election/status"/>
10+
<arg name="service_reset" default="/system/election/reset"/>
11+
<node pkg="redundancy_switcher_interface" exec="redundancy_switcher_interface_node">
12+
<param from="$(var param_file)"/>
13+
<remap from="~/input/control_mode" to="$(var input_control_mode)"/>
14+
<remap from="~/input/operation_mode_availability" to="$(var input_operation_mode_availability)"/>
15+
<remap from="~/input/mrm_state" to="$(var input_mrm_state)"/>
16+
<remap from="~/output/mrm_request" to="$(var output_mrm_request)"/>
17+
<remap from="~/output/over_all_mrm_state" to="$(var output_over_all_mrm_state)"/>
18+
<remap from="~/output/election_communication" to="$(var output_election_communication)"/>
19+
<remap from="~/output/election_status" to="$(var output_election_status)"/>
20+
<remap from="~/service/reset" to="$(var service_reset)"/>
21+
</node>
22+
</launch>

‎system/redundancy_switcher_interface/package.xml

+27
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>redundancy_switcher_interface</name>
5+
<version>0.1.0</version>
6+
<description>The redundancy switcher interface package</description>
7+
<maintainer email="tetsuhiro.kawaguchi@tier4.jp">TetsuKawa</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
11+
<buildtool_depend>autoware_cmake</buildtool_depend>
12+
13+
<depend>autoware_adapi_v1_msgs</depend>
14+
<depend>autoware_planning_msgs</depend>
15+
<depend>autoware_vehicle_msgs</depend>
16+
<depend>geometry_msgs</depend>
17+
<depend>rclcpp</depend>
18+
<depend>rclcpp_components</depend>
19+
<depend>tier4_system_msgs</depend>
20+
21+
<test_depend>ament_lint_auto</test_depend>
22+
<test_depend>autoware_lint_common</test_depend>
23+
24+
<export>
25+
<build_type>ament_cmake</build_type>
26+
</export>
27+
</package>

‎system/redundancy_switcher_interface/schema/redundancy_switcher_interface.schema.json

+113
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for redundancy switcher interface",
4+
"type": "object",
5+
"definitions": {
6+
"redundancy_switcher_interface": {
7+
"type": "object",
8+
"properties": {
9+
"availability_dest_ip": {
10+
"type": "string",
11+
"description": "IP address of the destination of availability",
12+
"default": "127.0.0.1"
13+
},
14+
"availability_dest_port": {
15+
"type": "string",
16+
"description": "Port of the destination of availability",
17+
"default": "59000"
18+
},
19+
"mrm_state_dest_ip": {
20+
"type": "string",
21+
"description": "IP address of the destination of mrm_state",
22+
"default": "127.0.0.1"
23+
},
24+
"mrm_state_dest_port": {
25+
"type": "string",
26+
"description": "Port of the destination of mrm_state",
27+
"default": "59001"
28+
},
29+
"mrm_request_src_ip": {
30+
"type": "string",
31+
"description": "IP address of the source of mrm_request",
32+
"default": "127.0.0.1"
33+
},
34+
"mrm_request_src_port": {
35+
"type": "string",
36+
"description": "Port of the source of mrm_request",
37+
"default": "59002"
38+
},
39+
"election_communication_src_ip": {
40+
"type": "string",
41+
"description": "IP address of the source of election_communication",
42+
"default": "127.0.0.1"
43+
},
44+
"election_communication_src_port": {
45+
"type": "string",
46+
"description": "Port of the source of election_communication",
47+
"default": "59003"
48+
},
49+
"election_status_src_ip": {
50+
"type": "string",
51+
"description": "IP address of the source of election_status",
52+
"default": "127.0.0.1"
53+
},
54+
"election_status_src_port": {
55+
"type": "string",
56+
"description": "Port of the source of election_status",
57+
"default": "59004"
58+
},
59+
"reset_request_dest_ip": {
60+
"type": "string",
61+
"description": "IP address of the destination of reset_request",
62+
"default": "127.0.0.1"
63+
},
64+
"reset_request_dest_port": {
65+
"type": "string",
66+
"description": "Port of the destination of reset_request",
67+
"default": "59005"
68+
},
69+
"reset_response_src_ip": {
70+
"type": "string",
71+
"description": "IP address of the source of reset_response",
72+
"default": "127.0.0.1"
73+
},
74+
"reset_response_src_port": {
75+
"type": "string",
76+
"description": "Port of the source of reset_response",
77+
"default": "59006"
78+
}
79+
},
80+
"required": [
81+
"availability_dest_ip",
82+
"availability_dest_port",
83+
"mrm_state_dest_ip",
84+
"mrm_state_dest_port",
85+
"mrm_request_src_ip",
86+
"mrm_request_src_port",
87+
"election_communication_src_ip",
88+
"election_communication_src_port",
89+
"election_status_src_ip",
90+
"election_status_src_port",
91+
"reset_request_dest_ip",
92+
"reset_request_dest_port",
93+
"reset_response_src_ip",
94+
"reset_response_src_port"
95+
],
96+
"additionalProperties": false
97+
}
98+
},
99+
"properties": {
100+
"/**": {
101+
"type": "object",
102+
"properties": {
103+
"ros__parameters": {
104+
"$ref": "#/definitions/redundancy_switcher_interface"
105+
}
106+
},
107+
"required": ["ros__parameters"],
108+
"additionalProperties": false
109+
}
110+
},
111+
"required": ["/**"],
112+
"additionalProperties": false
113+
}

‎system/redundancy_switcher_interface/script/relay_to_sub.py

+90
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
#!/usr/bin/env python3
2+
3+
from autoware_adapi_v1_msgs.msg import OperationModeState
4+
from autoware_planning_msgs.msg import Trajectory
5+
from geometry_msgs.msg import PoseWithCovarianceStamped
6+
import rclpy
7+
from rclpy.node import Node
8+
from tier4_system_msgs.msg import OperationModeAvailability
9+
10+
11+
class RemapNode(Node):
12+
def __init__(self):
13+
super().__init__("remap_node")
14+
15+
self.create_subscription(
16+
OperationModeAvailability,
17+
"/system/operation_mode/availability",
18+
self.operation_mode_callback,
19+
10,
20+
)
21+
self.create_subscription(
22+
OperationModeState,
23+
"/system/operation_mode/state",
24+
self.operation_mode_state_callback,
25+
10,
26+
)
27+
self.sub_trajectory = self.create_subscription(
28+
Trajectory, "/planning/scenario_planning/trajectory", self.trajectory_callback, 10
29+
)
30+
self.sub_pose_with_covariance = self.create_subscription(
31+
PoseWithCovarianceStamped, "/localization/pose_with_covariance", self.pose_callback, 10
32+
)
33+
# self.sub_initialpose3d = self.create_subscription(PoseWithCovarianceStamped, '/initialpose3d', self.initialpose_callback, 10)
34+
35+
self.pub_trajectory = self.create_publisher(
36+
Trajectory, "/to_sub/planning/scenario_planning/trajectory", 10
37+
)
38+
self.pub_pose_with_covariance = self.create_publisher(
39+
PoseWithCovarianceStamped, "/to_sub/localization/pose_with_covariance", 10
40+
)
41+
# self.pub_initialpose3d = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/initialpose3d', 10)
42+
43+
self.autonomous_mode = False
44+
self.operation_mode_autonomous_state = False
45+
self.get_logger().info(f"Initial autonomous mode: {self.autonomous_mode}")
46+
self.tmp_operation_mode_autonomous_state = False
47+
48+
def operation_mode_callback(self, msg):
49+
if msg.autonomous != self.autonomous_mode:
50+
self.autonomous_mode = msg.autonomous
51+
self.get_logger().info(f"Autonomous mode changed: {self.autonomous_mode}")
52+
53+
def operation_mode_state_callback(self, msg):
54+
self.tmp_operation_mode_autonomous_state = self.operation_mode_autonomous_state
55+
if msg.mode == 2:
56+
self.operation_mode_autonomous_state = True
57+
if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state:
58+
self.get_logger().info(
59+
f"Operation mode changed: {self.operation_mode_autonomous_state}"
60+
)
61+
else:
62+
self.operation_mode_autonomous_state = False
63+
if self.tmp_operation_mode_autonomous_state != self.operation_mode_autonomous_state:
64+
self.get_logger().info(
65+
f"Operation mode changed: {self.operation_mode_autonomous_state}"
66+
)
67+
68+
def trajectory_callback(self, msg):
69+
if not self.autonomous_mode or self.operation_mode_autonomous_state:
70+
self.pub_trajectory.publish(msg)
71+
72+
def pose_callback(self, msg):
73+
if not self.autonomous_mode or self.operation_mode_autonomous_state:
74+
self.pub_pose_with_covariance.publish(msg)
75+
76+
# def initialpose_callback(self, msg):
77+
# if self.autonomous_mode:
78+
# self.pub_initialpose3d.publish(msg)
79+
80+
81+
def main(args=None):
82+
rclpy.init(args=args)
83+
node = RemapNode()
84+
rclpy.spin(node)
85+
node.destroy_node()
86+
rclpy.shutdown()
87+
88+
89+
if __name__ == "__main__":
90+
main()

0 commit comments

Comments
 (0)