From 2cd19f684c8fc3ae7ee88785ffd85efe2098594d Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com>
Date: Fri, 22 Mar 2024 21:00:34 +0900
Subject: [PATCH 1/2] feat(tier4_system_launch): add option to launch mrm
 handler (#6660)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../launch/system.launch.xml                  | 29 +++++++++++++++++--
 .../launch/hazard_status_converter.launch.xml |  5 +++-
 .../hazard_status_converter/src/converter.cpp |  4 +--
 3 files changed, 33 insertions(+), 5 deletions(-)

diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
index e4e6e4eee31cd..06fe448c1d215 100644
--- a/launch/tier4_system_launch/launch/system.launch.xml
+++ b/launch/tier4_system_launch/launch/system.launch.xml
@@ -22,6 +22,11 @@
   <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)"/>
+
   <let name="sensor_launch_pkg" value="$(find-pkg-share $(var sensor_model)_launch)"/>
 
   <!-- Dummy Diag Publisher -->
@@ -62,7 +67,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 +81,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 +104,26 @@
         <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)">
+      <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 diagnostic_graph_aggregator_graph_path)"/>
+      </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 -->
diff --git a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml
index ff50dec3a342f..f3b07bfa94834 100644
--- a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml
+++ b/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>
diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp
index b09f3de891b45..e1da4f463f06d 100644
--- a/system/hazard_status_converter/src/converter.cpp
+++ b/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));
 }
 

From 4037478eb52b40f0fd7f448dcb727e1b88f3a282 Mon Sep 17 00:00:00 2001
From: Tomohito ANDO <tomohito.ando@tier4.jp>
Date: Fri, 29 Mar 2024 09:34:24 +0900
Subject: [PATCH 2/2] chore(tier4_system_launch): add option to select graph
 path depending on running mode (#6700)

chore(tier4_system_launch): add option of using graph path for simulation

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 launch/tier4_system_launch/launch/system.launch.xml | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
index 06fe448c1d215..b70b4fb24d892 100644
--- a/launch/tier4_system_launch/launch/system.launch.xml
+++ b/launch/tier4_system_launch/launch/system.launch.xml
@@ -26,6 +26,7 @@
   <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)"/>
 
@@ -114,9 +115,12 @@
 
     <!-- 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 diagnostic_graph_aggregator_graph_path)"/>
+        <arg name="graph_file" value="$(var graph_file)"/>
       </include>
     </group>