Merge pull request #1214 from tier4/feat/launch-mrm-handler

feat(tier4_system_launch): add option to launch mrm handler (autowarefoundation#6660)

launch/tier4_system_launch/launch/system.launch.xml

@@ -22,6 +22,12 @@
   <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 -->
@@ -62,7 +68,7 @@
     </group>
 
     <!-- Error Monitor -->
-    <group>
+    <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;)"/>
@@ -76,7 +82,7 @@
     </group>
 
     <!-- Emergency Handler -->
-    <group>
+    <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>
@@ -99,6 +105,29 @@
         <arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
       </include>
     </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 -->

system/hazard_status_converter/launch/hazard_status_converter.launch.xml

@@ -1,3 +1,6 @@
 <launch>
-  <node pkg="hazard_status_converter" exec="converter" name="hazard_status_converter"/>
+  <node pkg="hazard_status_converter" exec="converter" name="hazard_status_converter">
+    <remap from="~/diagnostics_graph" to="/diagnostics_graph"/>
+    <remap from="~/hazard_status" to="/system/emergency/hazard_status"/>
+  </node>
 </launch>

system/hazard_status_converter/src/converter.cpp

@@ -127,9 +127,9 @@ namespace hazard_status_converter
 
 Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", options)
 {
-  pub_hazard_ = create_publisher<HazardStatusStamped>("/hazard_status", rclcpp::QoS(1));
+  pub_hazard_ = create_publisher<HazardStatusStamped>("~/hazard_status", rclcpp::QoS(1));
   sub_graph_ = create_subscription<DiagnosticGraph>(
-    "/diagnostics_graph", rclcpp::QoS(3),
+    "~/diagnostics_graph", rclcpp::QoS(3),
     std::bind(&Converter::on_graph, this, std::placeholders::_1));
 }