Tmp/revert pr 1725 (#1817)

* Revert "report safe fault as latent fault"

This reverts commit bfacb16.

* Revert "fix"

This reverts commit 76fd57d.

* Revert "also report latent fault"

This reverts commit ff2b3c3.

* Revert "implement safe fault handling for hazard status converter"

This reverts commit 08a64be.

Author: saka1-s

launch/tier4_system_launch/launch/system.launch.xml

@@ -39,9 +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"/>
-
   <group>
     <push-ros-namespace namespace="/system"/>
 
@@ -123,9 +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)"/>
-      </include>
+      <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
     </group>
 
     <!-- MRM Handler -->

system/hazard_status_converter/launch/hazard_status_converter.launch.xml

@@ -1,8 +1,6 @@
 <launch>
-  <arg name="report_only_diag" 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)"/>
   </node>
 </launch>

system/hazard_status_converter/src/converter.cpp

@@ -27,8 +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);
 }
 
 void Converter::on_create(DiagGraph::ConstSharedPtr graph)
@@ -95,7 +93,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);
   };
@@ -109,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();

system/hazard_status_converter/src/converter.hpp

@@ -41,7 +41,6 @@ class Converter : public rclcpp::Node
 
   DiagUnit * auto_mode_root_;
   std::unordered_set<DiagUnit *> auto_mode_tree_;
-  bool report_only_diag_;
 };
 
 }  // namespace hazard_status_converter