fix(hazard_status_converter): check current operation mode (autowarefoundation#6733)

* fix: hazard status converter

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: topic name and modes

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix check target mode

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix message type

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Revert "fix check target mode"

This reverts commit 8b190b7.

---------

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Author: isamu-takagi

system/hazard_status_converter/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>rclcpp</depend>

system/hazard_status_converter/src/converter.cpp

@@ -32,10 +32,10 @@ enum class HazardLevel { NF, SF, LF, SPF };
 struct TempNode
 {
   const DiagnosticNode & node;
-  bool is_auto_tree;
+  bool is_root_tree;
 };
 
-HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel auto_mode_level)
+HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel root_mode_level)
 {
   // Convert the level according to the table below.
   // The Level other than auto mode is considered OK.
@@ -49,7 +49,7 @@ HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel auto_mode_le
   // | STALE | SF    | LF    | SPF   | SPF   |
   // |-------|-------------------------------|
 
-  const auto root_level = node.is_auto_tree ? auto_mode_level : DiagnosticStatus::OK;
+  const auto root_level = node.is_root_tree ? root_mode_level : DiagnosticStatus::OK;
   const auto node_level = node.node.status.level;
 
   if (node_level == DiagnosticStatus::OK) {
@@ -67,20 +67,21 @@ HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel auto_mode_le
   return HazardLevel::SPF;
 }
 
-void set_auto_tree(std::vector<TempNode> & nodes, int index)
+void set_root_tree(std::vector<TempNode> & nodes, int index)
 {
   TempNode & node = nodes[index];
-  if (node.is_auto_tree) {
+  if (node.is_root_tree) {
     return;
   }
 
-  node.is_auto_tree = true;
+  node.is_root_tree = true;
   for (const auto & link : node.node.links) {
-    set_auto_tree(nodes, link.index);
+    set_root_tree(nodes, link.index);
   }
 }
 
-HazardStatusStamped convert_hazard_diagnostics(const DiagnosticGraph & graph)
+HazardStatusStamped convert_hazard_diagnostics(
+  const DiagnosticGraph & graph, const std::string & root, bool ignore)
 {
   // Create temporary tree for conversion.
   std::vector<TempNode> nodes;
@@ -90,19 +91,21 @@ HazardStatusStamped convert_hazard_diagnostics(const DiagnosticGraph & graph)
   }
 
   // Mark nodes included in the auto mode tree.
-  DiagnosticLevel auto_mode_level = DiagnosticStatus::STALE;
-  for (size_t index = 0; index < nodes.size(); ++index) {
-    const auto & status = nodes[index].node.status;
-    if (status.name == "/autoware/modes/autonomous") {
-      set_auto_tree(nodes, index);
-      auto_mode_level = status.level;
+  DiagnosticLevel root_mode_level = DiagnosticStatus::STALE;
+  if (!root.empty() && !ignore) {
+    for (size_t index = 0; index < nodes.size(); ++index) {
+      const auto & status = nodes[index].node.status;
+      if (status.name == root) {
+        set_root_tree(nodes, index);
+        root_mode_level = status.level;
+      }
     }
   }
 
   // Calculate hazard level from node level and root level.
   HazardStatusStamped hazard;
   for (const auto & node : nodes) {
-    switch (get_hazard_level(node, auto_mode_level)) {
+    switch (get_hazard_level(node, root_mode_level)) {
       case HazardLevel::NF:
         hazard.status.diag_no_fault.push_back(node.node.status);
         break;
@@ -131,6 +134,22 @@ Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", op
   sub_graph_ = create_subscription<DiagnosticGraph>(
     "~/diagnostics_graph", rclcpp::QoS(3),
     std::bind(&Converter::on_graph, this, std::placeholders::_1));
+  sub_state_ = create_subscription<AutowareState>(
+    "/autoware/state", rclcpp::QoS(1),
+    std::bind(&Converter::on_state, this, std::placeholders::_1));
+  sub_mode_ = create_subscription<OperationMode>(
+    "/system/operation_mode/state", rclcpp::QoS(1).transient_local(),
+    std::bind(&Converter::on_mode, this, std::placeholders::_1));
+}
+
+void Converter::on_state(const AutowareState::ConstSharedPtr msg)
+{
+  state_ = msg;
+}
+
+void Converter::on_mode(const OperationMode::ConstSharedPtr msg)
+{
+  mode_ = msg;
 }
 
 void Converter::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
@@ -148,7 +167,29 @@ void Converter::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
     return HazardStatus::NO_FAULT;
   };
 
-  HazardStatusStamped hazard = convert_hazard_diagnostics(*msg);
+  const auto is_ignore = [this]() {
+    if (mode_ && state_) {
+      if (mode_->mode == OperationMode::AUTONOMOUS || mode_->mode == OperationMode::STOP) {
+        if (state_->state == AutowareState::WAITING_FOR_ROUTE) return true;
+        if (state_->state == AutowareState::PLANNING) return true;
+      }
+      if (state_->state == AutowareState::INITIALIZING) return true;
+      if (state_->state == AutowareState::FINALIZING) return true;
+    }
+    return false;
+  };
+
+  const auto get_root = [this]() {
+    if (mode_) {
+      if (mode_->mode == OperationMode::STOP) return "/autoware/modes/stop";
+      if (mode_->mode == OperationMode::AUTONOMOUS) return "/autoware/modes/autonomous";
+      if (mode_->mode == OperationMode::LOCAL) return "/autoware/modes/local";
+      if (mode_->mode == OperationMode::REMOTE) return "/autoware/modes/remote";
+    }
+    return "";
+  };
+
+  HazardStatusStamped hazard = convert_hazard_diagnostics(*msg, get_root(), is_ignore());
   hazard.stamp = msg->stamp;
   hazard.status.level = get_system_level(hazard.status);
   hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT;

system/hazard_status_converter/src/converter.hpp

@@ -17,6 +17,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_auto_system_msgs/msg/autoware_state.hpp>
 #include <autoware_auto_system_msgs/msg/hazard_status_stamped.hpp>
 #include <tier4_system_msgs/msg/diagnostic_graph.hpp>
 
@@ -33,11 +35,20 @@ class Converter : public rclcpp::Node
   explicit Converter(const rclcpp::NodeOptions & options);
 
 private:
+  using AutowareState = autoware_auto_system_msgs::msg::AutowareState;
+  using OperationMode = autoware_adapi_v1_msgs::msg::OperationModeState;
   using DiagnosticGraph = tier4_system_msgs::msg::DiagnosticGraph;
   using HazardStatusStamped = autoware_auto_system_msgs::msg::HazardStatusStamped;
+  rclcpp::Subscription<AutowareState>::SharedPtr sub_state_;
+  rclcpp::Subscription<OperationMode>::SharedPtr sub_mode_;
   rclcpp::Subscription<DiagnosticGraph>::SharedPtr sub_graph_;
   rclcpp::Publisher<HazardStatusStamped>::SharedPtr pub_hazard_;
+  void on_state(const AutowareState::ConstSharedPtr msg);
+  void on_mode(const OperationMode::ConstSharedPtr msg);
   void on_graph(const DiagnosticGraph::ConstSharedPtr msg);
+
+  AutowareState::ConstSharedPtr state_;
+  OperationMode::ConstSharedPtr mode_;
 };
 
 }  // namespace hazard_status_converter