Skip to content

Commit d98e1c8

Browse files
TetsuKawapre-commit-ci[bot]
TetsuKawa
and
pre-commit-ci[bot]
authored
feat: add leader election converter (#1372)
* feat: add control_cmd_switcher Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: add some launch for one host test Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: launch file mistakes Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: add leader_election_converter to launch Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: add rely script Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: add launch for remapping trajectry Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: change MrmState.msg to internal one Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: remove relay_trajectory from psim Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * fix: fix CMakefile Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: fix qos setting Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * style(pre-commit): autofix * modify: invalid variable Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * style(pre-commit): autofix --------- 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 70daa6d commit d98e1c8

File tree

21 files changed

+1005
-80
lines changed

21 files changed

+1005
-80
lines changed

‎dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -34,12 +34,12 @@ DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptio
3434

3535
// Timer
3636
using namespace std::literals::chrono_literals;
37-
timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this));
37+
timer_ = rclcpp::create_timer(
38+
this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this));
3839

3940
// State
4041

4142
// Diagnostics
42-
4343
}
4444

4545
void DummyOperationModePublisher::onTimer()

‎dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp

+5-6
Original file line numberDiff line numberDiff line change
@@ -12,15 +12,14 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_
16-
#define DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_
15+
#ifndef DUMMY_OPERATION_MODE_PUBLISHER_HPP_
16+
#define DUMMY_OPERATION_MODE_PUBLISHER_HPP_
1717

1818
// include
1919
#include <rclcpp/rclcpp.hpp>
2020

2121
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
2222

23-
2423
namespace dummy_operation_mode_publisher
2524
{
2625

@@ -36,7 +35,8 @@ class DummyOperationModePublisher : public rclcpp::Node
3635
// Subscriber
3736

3837
// Publisher
39-
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr pub_operation_mode_state_;
38+
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
39+
pub_operation_mode_state_;
4040

4141
// Service
4242

@@ -50,8 +50,7 @@ class DummyOperationModePublisher : public rclcpp::Node
5050
// State
5151

5252
// Diagnostics
53-
5453
};
5554
} // namespace dummy_operation_mode_publisher
5655

57-
#endif // DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_
56+
#endif // DUMMY_OPERATION_MODE_PUBLISHER_HPP_

‎launch/tier4_control_launch/launch/control.main.mrm-v0.6.tmp.launch.py

+431
Large diffs are not rendered by default.

‎launch/tier4_planning_launch/launch/planning.main.mrm-v0.6.tmp.launch.xml

+47
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
<launch>
2+
<!-- planning module -->
3+
4+
<!-- mission planner -->
5+
<arg name="mission_planner_param_path"/>
6+
<!-- parking -->
7+
<arg name="freespace_planner_param_path"/>
8+
<!-- planning validator -->
9+
<arg name="planning_validator_param_path"/>
10+
<!-- Auto mode setting-->
11+
<arg name="enable_all_modules_auto_mode"/>
12+
<arg name="is_simulation"/>
13+
14+
<group>
15+
<push-ros-namespace namespace="planning"/>
16+
<!-- mission planning module -->
17+
<group>
18+
<push-ros-namespace namespace="mission_planning"/>
19+
<include file="$(find-pkg-share tier4_planning_launch)/launch/mission_planning/mission_planning.launch.xml">
20+
<arg name="mission_planner_param_path" value="$(var mission_planner_param_path)"/>
21+
</include>
22+
</group>
23+
24+
<!-- scenario planning module -->
25+
<group>
26+
<push-ros-namespace namespace="scenario_planning"/>
27+
<include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
28+
<arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
29+
<arg name="is_simulation" value="$(var is_simulation)"/>
30+
</include>
31+
</group>
32+
33+
<!-- planning validator -->
34+
<group>
35+
<include file="$(find-pkg-share planning_validator)/launch/planning_validator.launch.xml">
36+
<arg name="input_trajectory" value="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
37+
<arg name="output_trajectory" value="/tmp/planning/scenario_planning/trajectory"/>
38+
<arg name="planning_validator_param_path" value="$(var planning_validator_param_path)"/>
39+
</include>
40+
</group>
41+
42+
<!-- planning evaluator -->
43+
<group>
44+
<include file="$(find-pkg-share planning_evaluator)/launch/planning_evaluator.launch.xml"/>
45+
</group>
46+
</group>
47+
</launch>

‎launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml

+190
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,190 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<!-- Parameter files -->
4+
<arg name="component_state_monitor_topic_path"/>
5+
<arg name="emergency_handler_param_path"/>
6+
<arg name="duplicated_node_checker_param_path"/>
7+
<arg name="mrm_comfortable_stop_operator_param_path"/>
8+
<arg name="mrm_emergency_stop_operator_param_path"/>
9+
<arg name="hazard_lights_selector_param_path"/>
10+
<arg name="dummy_diag_publisher_param_path"/>
11+
<arg name="system_monitor_cpu_monitor_param_path"/>
12+
<arg name="system_monitor_gpu_monitor_param_path"/>
13+
<arg name="system_monitor_hdd_monitor_param_path"/>
14+
<arg name="system_monitor_mem_monitor_param_path"/>
15+
<arg name="system_monitor_net_monitor_param_path"/>
16+
<arg name="system_monitor_ntp_monitor_param_path"/>
17+
<arg name="system_monitor_process_monitor_param_path"/>
18+
<arg name="system_monitor_voltage_monitor_param_path"/>
19+
20+
<arg name="launch_system_monitor" default="true" description="launch system monitor"/>
21+
<arg name="launch_dummy_diag_publisher" description="launch dummy diag publisher"/>
22+
<arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
23+
<arg name="sensor_model" description="sensor model name"/>
24+
25+
<arg name="use_diagnostic_graph" default="false" description="use diagnostic graph packages"/>
26+
<arg name="mrm_handler_param_path" if="$(var use_diagnostic_graph)"/>
27+
<arg name="diagnostic_graph_aggregator_param_path" if="$(var use_diagnostic_graph)"/>
28+
<arg name="diagnostic_graph_aggregator_graph_path" if="$(var use_diagnostic_graph)"/>
29+
<arg name="diagnostic_graph_aggregator_planning_simulator_graph_path" if="$(var use_diagnostic_graph)"/>
30+
31+
<let name="sensor_launch_pkg" value="$(find-pkg-share $(var sensor_model)_launch)"/>
32+
33+
<!-- Dummy Diag Publisher -->
34+
<arg name="launch_rqt_reconfigure" default="false"/>
35+
<arg name="launch_rqt_runtime_monitor" default="false"/>
36+
<arg name="launch_rqt_robot_monitor" default="false"/>
37+
<arg name="launch_rqt_runtime_monitor_err" default="false"/>
38+
39+
<group>
40+
<push-ros-namespace namespace="/system"/>
41+
42+
<!-- System Monitor -->
43+
<group if="$(var launch_system_monitor)">
44+
<push-ros-namespace namespace="system_monitor"/>
45+
<include file="$(find-pkg-share system_monitor)/launch/system_monitor.launch.xml">
46+
<arg name="cpu_monitor_config_file" value="$(var system_monitor_cpu_monitor_param_path)"/>
47+
<arg name="gpu_monitor_config_file" value="$(var system_monitor_gpu_monitor_param_path)"/>
48+
<arg name="hdd_monitor_config_file" value="$(var system_monitor_hdd_monitor_param_path)"/>
49+
<arg name="mem_monitor_config_file" value="$(var system_monitor_mem_monitor_param_path)"/>
50+
<arg name="net_monitor_config_file" value="$(var system_monitor_net_monitor_param_path)"/>
51+
<arg name="ntp_monitor_config_file" value="$(var system_monitor_ntp_monitor_param_path)"/>
52+
<arg name="process_monitor_config_file" value="$(var system_monitor_process_monitor_param_path)"/>
53+
<arg name="voltage_monitor_config_file" value="$(var system_monitor_voltage_monitor_param_path)"/>
54+
</include>
55+
</group>
56+
57+
<!-- Service Log Checker -->
58+
<group>
59+
<include file="$(find-pkg-share component_interface_tools)/launch/service_log_checker.launch.xml"/>
60+
</group>
61+
62+
<!-- Component State Monitor -->
63+
<group>
64+
<include file="$(find-pkg-share component_state_monitor)/launch/component_state_monitor.launch.py">
65+
<arg name="mode" value="$(var run_mode)"/>
66+
<arg name="file" value="$(var component_state_monitor_topic_path)"/>
67+
</include>
68+
</group>
69+
70+
<!-- Error Monitor -->
71+
<group unless="$(var use_diagnostic_graph)">
72+
<let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
73+
<let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
74+
<let name="config_file" value="$(var system_error_monitor_planning_simulator_param_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
75+
<include file="$(find-pkg-share system_error_monitor)/launch/system_error_monitor.launch.xml">
76+
<arg name="config_file" value="$(var config_file)"/>
77+
<arg name="extra_agg_config_file_sensing" value="$(var sensor_launch_pkg)/config/diagnostic_aggregator/sensor_kit.param.yaml"/>
78+
<arg name="extra_agg_config_file_system" value="$(var diagnostic_aggregator_system_param_path)"/>
79+
<arg name="extra_agg_config_file_vehicle" value="$(var diagnostic_aggregator_vehicle_param_path)"/>
80+
<arg name="use_emergency_hold" value="false"/>
81+
</include>
82+
</group>
83+
84+
<!-- Emergency Handler -->
85+
<group unless="$(var use_diagnostic_graph)">
86+
<include file="$(find-pkg-share emergency_handler)/launch/emergency_handler.launch.xml">
87+
<arg name="config_file" value="$(var emergency_handler_param_path)"/>
88+
</include>
89+
</group>
90+
91+
<group>
92+
<include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
93+
<arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
94+
</include>
95+
</group>
96+
97+
<!-- MRM Operator -->
98+
<group>
99+
<include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
100+
<arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
101+
</include>
102+
</group>
103+
<group>
104+
<include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
105+
<arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
106+
</include>
107+
</group>
108+
<!-- <group>
109+
<include file="$(find-pkg-share mrm_pull_over_manager)/launch/mrm_pull_over_manager.launch.xml"/>
110+
</group>
111+
<group>
112+
<include file="$(find-pkg-share emergency_goal_manager)/launch/emergency_goal_manager.launch.xml"/>
113+
</group> -->
114+
115+
<!-- MRM Handler -->
116+
<group if="$(var use_diagnostic_graph)">
117+
<include file="$(find-pkg-share mrm_handler)/launch/mrm_handler.launch.xml">
118+
<arg name="config_file" value="$(var mrm_handler_param_path)"/>
119+
</include>
120+
</group>
121+
122+
<!-- Diagnostic Graph Aggregator -->
123+
<group if="$(var use_diagnostic_graph)">
124+
<let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
125+
<let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
126+
<let name="graph_file" value="$(var diagnostic_graph_aggregator_planning_simulator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
127+
<include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
128+
<arg name="param_file" value="$(var diagnostic_graph_aggregator_param_path)"/>
129+
<arg name="graph_file" value="$(var graph_file)"/>
130+
</include>
131+
</group>
132+
133+
<!-- Hazard Status Converter -->
134+
<group if="$(var use_diagnostic_graph)">
135+
<include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
136+
</group>
137+
</group>
138+
139+
<!-- hazard selector -->
140+
<group>
141+
<include file="$(find-pkg-share hazard_lights_selector)/launch/hazard_lights_selector.launch.xml">
142+
<arg name="param_path" value="$(var hazard_lights_selector_param_path)"/>
143+
</include>
144+
</group>
145+
146+
<!-- Dummy Diag Publisher -->
147+
<group if="$(var launch_dummy_diag_publisher)">
148+
<include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher.launch.xml">
149+
<arg name="config_file" value="$(var dummy_diag_publisher_param_path)"/>
150+
<arg name="extra_config_file_sensor" value="$(var sensor_launch_pkg)/config/dummy_diag_publisher/sensor_kit.param.yaml"/>
151+
<arg name="launch_rqt_reconfigure" value="$(var launch_rqt_reconfigure)"/>
152+
<arg name="launch_rqt_runtime_monitor" value="$(var launch_rqt_runtime_monitor)"/>
153+
<arg name="launch_rqt_robot_monitor" value="$(var launch_rqt_robot_monitor)"/>
154+
<arg name="launch_rqt_runtime_monitor_err" value="$(var launch_rqt_runtime_monitor_err)"/>
155+
</include>
156+
</group>
157+
158+
<!-- Control Cmd Switcher for MRMv0.6 one host test-->
159+
<group>
160+
<include file="$(find-pkg-share control_cmd_switcher)/launch/control_cmd_switcher.launch.xml"/>
161+
</group>
162+
163+
<!-- Leader Election Converter-->
164+
<group>
165+
<include file="$(find-pkg-share leader_election_converter)/launch/leader_election_converter.launch.xml"/>
166+
</group>
167+
168+
<!-- topic state montor for MRMv0.6 one host test-->
169+
<group>
170+
<include file="$(find-pkg-share topic_state_monitor)/launch/topic_state_monitor.launch.xml">
171+
<arg name="node_name_suffix" value="scenario_planning_trajectry"/>
172+
<arg name="topic" value="/planning/scenario_planning/trajectory"/>
173+
<arg name="topic_type" value="autoware_auto_planning_msgs/msg/Trajectory"/>
174+
<arg name="diag_name" value="publish_to_sub_status"/>
175+
<arg name="warn_rate" value="5.0"/>
176+
<arg name="error_rate" value="1.0"/>
177+
<arg name="timeout" value="1.0"/>
178+
</include>
179+
180+
<!-- <include file="$(find-pkg-share topic_state_monitor)/launch/topic_state_monitor.launch.xml">
181+
<arg name="node_name_suffix" value="pose_estimator_pose_with_covariance"/>
182+
<arg name="topic" value=" /localization/pose_estimator/pose_with_covariance"/>
183+
<arg name="topic_type" value="geometry_msgs/msg/PoseWithCovarianceStamped"/>
184+
<arg name="diag_name" value="publish_to_sub_status"/>
185+
<arg name="warn_rate" value=""/>
186+
<arg name="error_rate" value=""/>
187+
<arg name="timeout" value=""/>
188+
</include> -->
189+
</group>
190+
</launch>

‎launch/tier4_system_launch/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,9 @@
1212
<buildtool_depend>autoware_cmake</buildtool_depend>
1313

1414
<exec_depend>component_state_monitor</exec_depend>
15+
<exec_depend>control_cmd_switcher</exec_depend>
1516
<exec_depend>emergency_handler</exec_depend>
17+
<exec_depend>leader_election_converter</exec_depend>
1618
<exec_depend>system_error_monitor</exec_depend>
1719
<exec_depend>system_monitor</exec_depend>
1820

‎system/control_cmd_switcher/CMakeLists.txt

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(control_cmd_switcher)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(${PROJECT_NAME} SHARED
8+
src/control_cmd_switcher/control_cmd_switcher.cpp
9+
)
10+
11+
rclcpp_components_register_node(${PROJECT_NAME}
12+
PLUGIN "ControlCmdSwitcher"
13+
EXECUTABLE ${PROJECT_NAME}_node
14+
)
15+
16+
install(PROGRAMS
17+
tool/rely_trajectory.py
18+
DESTINATION lib/${PROJECT_NAME}
19+
PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ
20+
)
21+
22+
ament_auto_package(INSTALL_TO_SHARE
23+
launch
24+
config
25+
)

‎system/control_cmd_switcher/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
# control_cmd_switcher

‎system/control_cmd_switcher/config/control_cmd_switcher.yaml

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
# Default configuration for mrm handler
2+
---
3+
/**:
4+
ros__parameters:

‎system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
<launch>
2+
<arg name="input_main_control_cmd" default="/main/control/command/control_cmd"/>
3+
<arg name="input_sub_control_cmd" default="/sub/control/command/control_cmd"/>
4+
<arg name="input_election_status" default="/system/election/status"/>
5+
<arg name="output_control_cmd" default="/control/command/control_cmd"/>
6+
7+
<arg name="config_file" default="$(find-pkg-share control_cmd_switcher)/config/control_cmd_switcher.yaml"/>
8+
9+
<!-- mrm_handler -->
10+
<node pkg="control_cmd_switcher" exec="control_cmd_switcher_node" name="control_cmd_switcher" output="screen">
11+
<remap from="~/input/main/control_cmd" to="$(var input_main_control_cmd)"/>
12+
<remap from="~/input/sub/control_cmd" to="$(var input_sub_control_cmd)"/>
13+
<remap from="~/input/election/status" to="$(var input_election_status)"/>
14+
<remap from="~/output/control_cmd" to="$(var output_control_cmd)"/>
15+
</node>
16+
</launch>

‎system/control_cmd_switcher/package.xml

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
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>control_cmd_switcher</name>
5+
<version>0.1.0</version>
6+
<description>The control_cmd_switcher ROS 2 package</description>
7+
8+
<maintainer email="tetsuhiro.kawaguchi@tier4.jp">Tetsuhiro Kawaguchi</maintainer>
9+
<license>Apache License 2.0</license>
10+
11+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
12+
<buildtool_depend>autoware_cmake</buildtool_depend>
13+
14+
<depend>autoware_auto_control_msgs</depend>
15+
<depend>rclcpp</depend>
16+
<depend>rclcpp_components</depend>
17+
<depend>tier4_system_msgs</depend>
18+
19+
<test_depend>ament_lint_auto</test_depend>
20+
<test_depend>autoware_lint_common</test_depend>
21+
22+
<export>
23+
<build_type>ament_cmake</build_type>
24+
</export>
25+
</package>

0 commit comments

Comments
 (0)