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>
tier4 · Jul 2, 2024 · d98e1c8 · d98e1c8
1 parent 70daa6d
commit d98e1c8
Show file tree

Hide file tree

Showing 21 changed files with 1,005 additions and 80 deletions.
diff --git a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp
@@ -34,12 +34,12 @@ DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptio
 
   // Timer
   using namespace std::literals::chrono_literals;
-  timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this));
+  timer_ = rclcpp::create_timer(
+    this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this));
 
   // State
 
   // Diagnostics
-
 }
 
 void DummyOperationModePublisher::onTimer()

diff --git a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp
@@ -12,15 +12,14 @@
 // 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_
+#ifndef DUMMY_OPERATION_MODE_PUBLISHER_HPP_
+#define 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
 {
 
@@ -36,7 +35,8 @@ class DummyOperationModePublisher : public rclcpp::Node
   // Subscriber
 
   // Publisher
-  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr pub_operation_mode_state_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
+    pub_operation_mode_state_;
 
   // Service
 
@@ -50,8 +50,7 @@ class DummyOperationModePublisher : public rclcpp::Node
   // State
 
   // Diagnostics
-
 };
 }  // namespace dummy_operation_mode_publisher
 
-#endif  // DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_
+#endif  // DUMMY_OPERATION_MODE_PUBLISHER_HPP_
diff --git a/launch/tier4_control_launch/launch/control.main.mrm-v0.6.tmp.launch.py b/launch/tier4_control_launch/launch/control.main.mrm-v0.6.tmp.launch.py
diff --git a/launch/tier4_planning_launch/launch/planning.main.mrm-v0.6.tmp.launch.xml b/launch/tier4_planning_launch/launch/planning.main.mrm-v0.6.tmp.launch.xml
@@ -0,0 +1,47 @@
+<launch>
+  <!-- planning module -->
+
+  <!-- mission planner -->
+  <arg name="mission_planner_param_path"/>
+  <!-- parking -->
+  <arg name="freespace_planner_param_path"/>
+  <!-- planning validator -->
+  <arg name="planning_validator_param_path"/>
+  <!-- Auto mode setting-->
+  <arg name="enable_all_modules_auto_mode"/>
+  <arg name="is_simulation"/>
+
+  <group>
+    <push-ros-namespace namespace="planning"/>
+    <!-- mission planning module -->
+    <group>
+      <push-ros-namespace namespace="mission_planning"/>
+      <include file="$(find-pkg-share tier4_planning_launch)/launch/mission_planning/mission_planning.launch.xml">
+        <arg name="mission_planner_param_path" value="$(var mission_planner_param_path)"/>
+      </include>
+    </group>
+
+    <!-- scenario planning module -->
+    <group>
+      <push-ros-namespace namespace="scenario_planning"/>
+      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
+        <arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
+        <arg name="is_simulation" value="$(var is_simulation)"/>
+      </include>
+    </group>
+
+    <!-- planning validator -->
+    <group>
+      <include file="$(find-pkg-share planning_validator)/launch/planning_validator.launch.xml">
+        <arg name="input_trajectory" value="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
+        <arg name="output_trajectory" value="/tmp/planning/scenario_planning/trajectory"/>
+        <arg name="planning_validator_param_path" value="$(var planning_validator_param_path)"/>
+      </include>
+    </group>
+
+    <!-- planning evaluator -->
+    <group>
+      <include file="$(find-pkg-share planning_evaluator)/launch/planning_evaluator.launch.xml"/>
+    </group>
+  </group>
+</launch>
diff --git a/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml b/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml
@@ -0,0 +1,190 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <!-- Parameter files -->
+  <arg name="component_state_monitor_topic_path"/>
+  <arg name="emergency_handler_param_path"/>
+  <arg name="duplicated_node_checker_param_path"/>
+  <arg name="mrm_comfortable_stop_operator_param_path"/>
+  <arg name="mrm_emergency_stop_operator_param_path"/>
+  <arg name="hazard_lights_selector_param_path"/>
+  <arg name="dummy_diag_publisher_param_path"/>
+  <arg name="system_monitor_cpu_monitor_param_path"/>
+  <arg name="system_monitor_gpu_monitor_param_path"/>
+  <arg name="system_monitor_hdd_monitor_param_path"/>
+  <arg name="system_monitor_mem_monitor_param_path"/>
+  <arg name="system_monitor_net_monitor_param_path"/>
+  <arg name="system_monitor_ntp_monitor_param_path"/>
+  <arg name="system_monitor_process_monitor_param_path"/>
+  <arg name="system_monitor_voltage_monitor_param_path"/>
+
+  <arg name="launch_system_monitor" default="true" description="launch system monitor"/>
+  <arg name="launch_dummy_diag_publisher" description="launch dummy diag publisher"/>
+  <arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
+  <arg name="sensor_model" description="sensor model name"/>
+
+  <arg name="use_diagnostic_graph" default="false" description="use diagnostic graph packages"/>
+  <arg name="mrm_handler_param_path" if="$(var use_diagnostic_graph)"/>
+  <arg name="diagnostic_graph_aggregator_param_path" if="$(var use_diagnostic_graph)"/>
+  <arg name="diagnostic_graph_aggregator_graph_path" if="$(var use_diagnostic_graph)"/>
+  <arg name="diagnostic_graph_aggregator_planning_simulator_graph_path" if="$(var use_diagnostic_graph)"/>
+
+  <let name="sensor_launch_pkg" value="$(find-pkg-share $(var sensor_model)_launch)"/>
+
+  <!-- Dummy Diag Publisher -->
+  <arg name="launch_rqt_reconfigure" default="false"/>
+  <arg name="launch_rqt_runtime_monitor" default="false"/>
+  <arg name="launch_rqt_robot_monitor" default="false"/>
+  <arg name="launch_rqt_runtime_monitor_err" default="false"/>
+
+  <group>
+    <push-ros-namespace namespace="/system"/>
+
+    <!-- System Monitor -->
+    <group if="$(var launch_system_monitor)">
+      <push-ros-namespace namespace="system_monitor"/>
+      <include file="$(find-pkg-share system_monitor)/launch/system_monitor.launch.xml">
+        <arg name="cpu_monitor_config_file" value="$(var system_monitor_cpu_monitor_param_path)"/>
+        <arg name="gpu_monitor_config_file" value="$(var system_monitor_gpu_monitor_param_path)"/>
+        <arg name="hdd_monitor_config_file" value="$(var system_monitor_hdd_monitor_param_path)"/>
+        <arg name="mem_monitor_config_file" value="$(var system_monitor_mem_monitor_param_path)"/>
+        <arg name="net_monitor_config_file" value="$(var system_monitor_net_monitor_param_path)"/>
+        <arg name="ntp_monitor_config_file" value="$(var system_monitor_ntp_monitor_param_path)"/>
+        <arg name="process_monitor_config_file" value="$(var system_monitor_process_monitor_param_path)"/>
+        <arg name="voltage_monitor_config_file" value="$(var system_monitor_voltage_monitor_param_path)"/>
+      </include>
+    </group>
+
+    <!-- Service Log Checker -->
+    <group>
+      <include file="$(find-pkg-share component_interface_tools)/launch/service_log_checker.launch.xml"/>
+    </group>
+
+    <!-- Component State Monitor -->
+    <group>
+      <include file="$(find-pkg-share component_state_monitor)/launch/component_state_monitor.launch.py">
+        <arg name="mode" value="$(var run_mode)"/>
+        <arg name="file" value="$(var component_state_monitor_topic_path)"/>
+      </include>
+    </group>
+
+    <!-- Error Monitor -->
+    <group unless="$(var use_diagnostic_graph)">
+      <let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
+      <let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
+      <let name="config_file" value="$(var system_error_monitor_planning_simulator_param_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
+      <include file="$(find-pkg-share system_error_monitor)/launch/system_error_monitor.launch.xml">
+        <arg name="config_file" value="$(var config_file)"/>
+        <arg name="extra_agg_config_file_sensing" value="$(var sensor_launch_pkg)/config/diagnostic_aggregator/sensor_kit.param.yaml"/>
+        <arg name="extra_agg_config_file_system" value="$(var diagnostic_aggregator_system_param_path)"/>
+        <arg name="extra_agg_config_file_vehicle" value="$(var diagnostic_aggregator_vehicle_param_path)"/>
+        <arg name="use_emergency_hold" value="false"/>
+      </include>
+    </group>
+
+    <!-- Emergency Handler -->
+    <group unless="$(var use_diagnostic_graph)">
+      <include file="$(find-pkg-share emergency_handler)/launch/emergency_handler.launch.xml">
+        <arg name="config_file" value="$(var emergency_handler_param_path)"/>
+      </include>
+    </group>
+
+    <group>
+      <include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
+        <arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
+      </include>
+    </group>
+
+    <!-- MRM Operator -->
+    <group>
+      <include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
+        <arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
+        <arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
+      </include>
+    </group>
+    <!-- <group>
+      <include file="$(find-pkg-share mrm_pull_over_manager)/launch/mrm_pull_over_manager.launch.xml"/>
+    </group>
+    <group>
+      <include file="$(find-pkg-share emergency_goal_manager)/launch/emergency_goal_manager.launch.xml"/>
+    </group> -->
+
+    <!-- MRM Handler -->
+    <group if="$(var use_diagnostic_graph)">
+      <include file="$(find-pkg-share mrm_handler)/launch/mrm_handler.launch.xml">
+        <arg name="config_file" value="$(var mrm_handler_param_path)"/>
+      </include>
+    </group>
+
+    <!-- Diagnostic Graph Aggregator -->
+    <group if="$(var use_diagnostic_graph)">
+      <let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
+      <let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
+      <let name="graph_file" value="$(var diagnostic_graph_aggregator_planning_simulator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
+      <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
+        <arg name="param_file" value="$(var diagnostic_graph_aggregator_param_path)"/>
+        <arg name="graph_file" value="$(var graph_file)"/>
+      </include>
+    </group>
+
+    <!-- Hazard Status Converter -->
+    <group if="$(var use_diagnostic_graph)">
+      <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
+    </group>
+  </group>
+
+  <!-- hazard selector -->
+  <group>
+    <include file="$(find-pkg-share hazard_lights_selector)/launch/hazard_lights_selector.launch.xml">
+      <arg name="param_path" value="$(var hazard_lights_selector_param_path)"/>
+    </include>
+  </group>
+
+  <!-- Dummy Diag Publisher -->
+  <group if="$(var launch_dummy_diag_publisher)">
+    <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher.launch.xml">
+      <arg name="config_file" value="$(var dummy_diag_publisher_param_path)"/>
+      <arg name="extra_config_file_sensor" value="$(var sensor_launch_pkg)/config/dummy_diag_publisher/sensor_kit.param.yaml"/>
+      <arg name="launch_rqt_reconfigure" value="$(var launch_rqt_reconfigure)"/>
+      <arg name="launch_rqt_runtime_monitor" value="$(var launch_rqt_runtime_monitor)"/>
+      <arg name="launch_rqt_robot_monitor" value="$(var launch_rqt_robot_monitor)"/>
+      <arg name="launch_rqt_runtime_monitor_err" value="$(var launch_rqt_runtime_monitor_err)"/>
+    </include>
+  </group>
+
+  <!-- Control Cmd Switcher for MRMv0.6 one host test-->
+  <group>
+    <include file="$(find-pkg-share control_cmd_switcher)/launch/control_cmd_switcher.launch.xml"/>
+  </group>
+
+  <!-- Leader Election Converter-->
+  <group>
+    <include file="$(find-pkg-share leader_election_converter)/launch/leader_election_converter.launch.xml"/>
+  </group>
+
+  <!-- topic state montor for MRMv0.6 one host test-->
+  <group>
+    <include file="$(find-pkg-share topic_state_monitor)/launch/topic_state_monitor.launch.xml">
+      <arg name="node_name_suffix" value="scenario_planning_trajectry"/>
+      <arg name="topic" value="/planning/scenario_planning/trajectory"/>
+      <arg name="topic_type" value="autoware_auto_planning_msgs/msg/Trajectory"/>
+      <arg name="diag_name" value="publish_to_sub_status"/>
+      <arg name="warn_rate" value="5.0"/>
+      <arg name="error_rate" value="1.0"/>
+      <arg name="timeout" value="1.0"/>
+    </include>
+
+    <!-- <include file="$(find-pkg-share topic_state_monitor)/launch/topic_state_monitor.launch.xml">
+      <arg name="node_name_suffix" value="pose_estimator_pose_with_covariance"/>
+      <arg name="topic" value="	/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="topic_type" value="geometry_msgs/msg/PoseWithCovarianceStamped"/>
+      <arg name="diag_name" value="publish_to_sub_status"/>
+      <arg name="warn_rate" value=""/>
+      <arg name="error_rate" value=""/>
+      <arg name="timeout" value=""/>
+    </include> -->
+  </group>
+</launch>
diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml
@@ -12,7 +12,9 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <exec_depend>component_state_monitor</exec_depend>
+  <exec_depend>control_cmd_switcher</exec_depend>
   <exec_depend>emergency_handler</exec_depend>
+  <exec_depend>leader_election_converter</exec_depend>
   <exec_depend>system_error_monitor</exec_depend>
   <exec_depend>system_monitor</exec_depend>
 

diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt
@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+project(control_cmd_switcher)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/control_cmd_switcher/control_cmd_switcher.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "ControlCmdSwitcher"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+install(PROGRAMS
+  tool/rely_trajectory.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
+  launch
+  config
+)
diff --git a/system/control_cmd_switcher/README.md b/system/control_cmd_switcher/README.md
@@ -0,0 +1 @@
+# control_cmd_switcher
diff --git a/system/control_cmd_switcher/config/control_cmd_switcher.yaml b/system/control_cmd_switcher/config/control_cmd_switcher.yaml
@@ -0,0 +1,4 @@
+# Default configuration for mrm handler
+---
+/**:
+  ros__parameters:
diff --git a/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml
@@ -0,0 +1,16 @@
+<launch>
+  <arg name="input_main_control_cmd" default="/main/control/command/control_cmd"/>
+  <arg name="input_sub_control_cmd" default="/sub/control/command/control_cmd"/>
+  <arg name="input_election_status" default="/system/election/status"/>
+  <arg name="output_control_cmd" default="/control/command/control_cmd"/>
+
+  <arg name="config_file" default="$(find-pkg-share control_cmd_switcher)/config/control_cmd_switcher.yaml"/>
+
+  <!-- mrm_handler -->
+  <node pkg="control_cmd_switcher" exec="control_cmd_switcher_node" name="control_cmd_switcher" output="screen">
+    <remap from="~/input/main/control_cmd" to="$(var input_main_control_cmd)"/>
+    <remap from="~/input/sub/control_cmd" to="$(var input_sub_control_cmd)"/>
+    <remap from="~/input/election/status" to="$(var input_election_status)"/>
+    <remap from="~/output/control_cmd" to="$(var output_control_cmd)"/>
+  </node>
+</launch>
diff --git a/system/control_cmd_switcher/package.xml b/system/control_cmd_switcher/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>control_cmd_switcher</name>
+  <version>0.1.0</version>
+  <description>The control_cmd_switcher ROS 2 package</description>
+
+  <maintainer email="tetsuhiro.kawaguchi@tier4.jp">Tetsuhiro Kawaguchi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_control_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>