From 94aa70fc12797afb391f868f8aaec7b14b2c98ad Mon Sep 17 00:00:00 2001
From: Shohei Sakai <saka1s.jp@gmail.com>
Date: Fri, 14 Feb 2025 14:59:27 +0900
Subject: [PATCH 1/4] Revert "report safe fault as latent fault"

This reverts commit bfacb16b0af1f284aa6a323d98886adb6e677596.
---
 system/hazard_status_converter/src/converter.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp
index 215ddfd84c2bd..cf7c680fb4045 100644
--- a/system/hazard_status_converter/src/converter.cpp
+++ b/system/hazard_status_converter/src/converter.cpp
@@ -95,7 +95,7 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph)
   const auto get_hazards_vector = [](HazardStatus & status, HazardLevel level) {
     if (level == HazardStatus::SINGLE_POINT_FAULT) return &status.diag_single_point_fault;
     if (level == HazardStatus::LATENT_FAULT) return &status.diag_latent_fault;
-    if (level == HazardStatus::SAFE_FAULT) return &status.diag_latent_fault;
+    if (level == HazardStatus::SAFE_FAULT) return &status.diag_safe_fault;
     if (level == HazardStatus::NO_FAULT) return &status.diag_no_fault;
     return static_cast<std::vector<DiagnosticStatus> *>(nullptr);
   };

From c1b4ff475064c8c4e5fd274d83dcba3433c7946b Mon Sep 17 00:00:00 2001
From: Shohei Sakai <saka1s.jp@gmail.com>
Date: Fri, 14 Feb 2025 14:59:45 +0900
Subject: [PATCH 2/4] Revert "fix"

This reverts commit 76fd57dd0ab05545ceb67dc7ff5d81a514b4f94a.
---
 launch/tier4_system_launch/launch/system.launch.xml          | 2 ++
 .../launch/hazard_status_converter.launch.xml                | 2 ++
 system/hazard_status_converter/src/converter.cpp             | 5 +++++
 system/hazard_status_converter/src/converter.hpp             | 1 +
 4 files changed, 10 insertions(+)

diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
index 2699153edb11d..f4d5216958802 100644
--- a/launch/tier4_system_launch/launch/system.launch.xml
+++ b/launch/tier4_system_launch/launch/system.launch.xml
@@ -41,6 +41,7 @@
 
   <!-- Hazard Status Converter -->
   <arg name="report_only_diag" default="false"/>
+  <arg name="report_safe_fault" default="false"/>
 
   <group>
     <push-ros-namespace namespace="/system"/>
@@ -125,6 +126,7 @@
     <group unless="$(var use_emergency_handler)">
       <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml">
         <arg name="report_only_diag" value="$(var report_only_diag)"/>
+        <arg name="report_safe_fault" value="$(var report_safe_fault)"/>
       </include>
     </group>
 
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 3cb54f2c5ca02..f8a2b1b043079 100644
--- a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml
+++ b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml
@@ -1,8 +1,10 @@
 <launch>
   <arg name="report_only_diag" default="false"/>
+  <arg name="report_safe_fault" default="false"/>
   <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"/>
     <param name="report_only_diag" value="$(var report_only_diag)"/>
+    <param name="report_safe_fault" value="$(var report_safe_fault)"/>
   </node>
 </launch>
diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp
index cf7c680fb4045..040b2bdabdff9 100644
--- a/system/hazard_status_converter/src/converter.cpp
+++ b/system/hazard_status_converter/src/converter.cpp
@@ -29,6 +29,7 @@ Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", op
   sub_graph_.subscribe(*this, 1);
 
   report_only_diag_ = declare_parameter<bool>("report_only_diag", false);
+  report_safe_fault_ = declare_parameter<bool>("report_safe_fault", false);
 }
 
 void Converter::on_create(DiagGraph::ConstSharedPtr graph)
@@ -120,6 +121,10 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph)
   hazard.stamp = graph->updated_stamp();
   hazard.status.level = get_system_level(hazard.status);
   hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT;
+  if (report_safe_fault_)
+    hazard.status.emergency &=
+      (hazard.status.level == HazardStatus::SAFE_FAULT ||
+       hazard.status.level == HazardStatus::LATENT_FAULT);
   hazard.status.emergency_holding = false;
   pub_hazard_->publish(hazard);
 }
diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp
index 28143c7644d82..0ac275d094420 100644
--- a/system/hazard_status_converter/src/converter.hpp
+++ b/system/hazard_status_converter/src/converter.hpp
@@ -42,6 +42,7 @@ class Converter : public rclcpp::Node
   DiagUnit * auto_mode_root_;
   std::unordered_set<DiagUnit *> auto_mode_tree_;
   bool report_only_diag_;
+  bool report_safe_fault_;
 };
 
 }  // namespace hazard_status_converter

From 7c88a9c5d7c3e2a8da0df8693ca178d2ffbfadcd Mon Sep 17 00:00:00 2001
From: Shohei Sakai <saka1s.jp@gmail.com>
Date: Fri, 14 Feb 2025 14:59:54 +0900
Subject: [PATCH 3/4] Revert "also report latent fault"

This reverts commit ff2b3c3f421ca13ef5c1f603a9bda33a23187555.
---
 system/hazard_status_converter/src/converter.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp
index 040b2bdabdff9..433fb7e0f9ae0 100644
--- a/system/hazard_status_converter/src/converter.cpp
+++ b/system/hazard_status_converter/src/converter.cpp
@@ -122,9 +122,7 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph)
   hazard.status.level = get_system_level(hazard.status);
   hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT;
   if (report_safe_fault_)
-    hazard.status.emergency &=
-      (hazard.status.level == HazardStatus::SAFE_FAULT ||
-       hazard.status.level == HazardStatus::LATENT_FAULT);
+    hazard.status.emergency &= hazard.status.level == HazardStatus::SAFE_FAULT;
   hazard.status.emergency_holding = false;
   pub_hazard_->publish(hazard);
 }

From 7a10e0ccff35f04652c34a50dc65e1e700f71702 Mon Sep 17 00:00:00 2001
From: Shohei Sakai <saka1s.jp@gmail.com>
Date: Fri, 14 Feb 2025 15:00:01 +0900
Subject: [PATCH 4/4] Revert "implement safe fault handling for hazard status
 converter"

This reverts commit 08a64be5740e51f5d2a021b97e5e48e7fca03884.
---
 launch/tier4_system_launch/launch/system.launch.xml      | 9 +--------
 .../launch/hazard_status_converter.launch.xml            | 4 ----
 system/hazard_status_converter/src/converter.cpp         | 6 ------
 system/hazard_status_converter/src/converter.hpp         | 2 --
 4 files changed, 1 insertion(+), 20 deletions(-)

diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
index f4d5216958802..6253dce864ddf 100644
--- a/launch/tier4_system_launch/launch/system.launch.xml
+++ b/launch/tier4_system_launch/launch/system.launch.xml
@@ -39,10 +39,6 @@
   <arg name="launch_rqt_robot_monitor" default="false"/>
   <arg name="launch_rqt_runtime_monitor_err" default="false"/>
 
-  <!-- Hazard Status Converter -->
-  <arg name="report_only_diag" default="false"/>
-  <arg name="report_safe_fault" default="false"/>
-
   <group>
     <push-ros-namespace namespace="/system"/>
 
@@ -124,10 +120,7 @@
 
     <!-- Hazard Status Converter -->
     <group unless="$(var use_emergency_handler)">
-      <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml">
-        <arg name="report_only_diag" value="$(var report_only_diag)"/>
-        <arg name="report_safe_fault" value="$(var report_safe_fault)"/>
-      </include>
+      <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
     </group>
 
     <!-- MRM Handler -->
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 f8a2b1b043079..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,10 +1,6 @@
 <launch>
-  <arg name="report_only_diag" default="false"/>
-  <arg name="report_safe_fault" default="false"/>
   <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"/>
-    <param name="report_only_diag" value="$(var report_only_diag)"/>
-    <param name="report_safe_fault" value="$(var report_safe_fault)"/>
   </node>
 </launch>
diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp
index 433fb7e0f9ae0..e8213b441bb33 100644
--- a/system/hazard_status_converter/src/converter.cpp
+++ b/system/hazard_status_converter/src/converter.cpp
@@ -27,9 +27,6 @@ Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", op
   sub_graph_.register_create_callback(std::bind(&Converter::on_create, this, _1));
   sub_graph_.register_update_callback(std::bind(&Converter::on_update, this, _1));
   sub_graph_.subscribe(*this, 1);
-
-  report_only_diag_ = declare_parameter<bool>("report_only_diag", false);
-  report_safe_fault_ = declare_parameter<bool>("report_safe_fault", false);
 }
 
 void Converter::on_create(DiagGraph::ConstSharedPtr graph)
@@ -110,7 +107,6 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph)
   HazardStatusStamped hazard;
   for (const auto & unit : graph->units()) {
     if (unit->path().empty()) continue;
-    if (report_only_diag_ && unit->type() != "diag") continue;
     const bool is_auto_tree = auto_mode_tree_.count(unit);
     const auto root_level = is_auto_tree ? auto_mode_root_->level() : DiagnosticStatus::OK;
     const auto unit_level = unit->level();
@@ -121,8 +117,6 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph)
   hazard.stamp = graph->updated_stamp();
   hazard.status.level = get_system_level(hazard.status);
   hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT;
-  if (report_safe_fault_)
-    hazard.status.emergency &= hazard.status.level == HazardStatus::SAFE_FAULT;
   hazard.status.emergency_holding = false;
   pub_hazard_->publish(hazard);
 }
diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp
index 0ac275d094420..442eedf588429 100644
--- a/system/hazard_status_converter/src/converter.hpp
+++ b/system/hazard_status_converter/src/converter.hpp
@@ -41,8 +41,6 @@ class Converter : public rclcpp::Node
 
   DiagUnit * auto_mode_root_;
   std::unordered_set<DiagUnit *> auto_mode_tree_;
-  bool report_only_diag_;
-  bool report_safe_fault_;
 };
 
 }  // namespace hazard_status_converter