fix(hazard_status_converter): backport autowarefoundation#10224 to v0.39.0

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

Author: isamu-takagi

system/hazard_status_converter/CMakeLists.txt

@@ -6,6 +6,7 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/converter.cpp
+  src/mode_interface.cpp
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}

system/hazard_status_converter/src/converter.cpp

@@ -14,13 +14,15 @@
 
 #include "converter.hpp"
 
+#include <string>
 #include <utility>
 #include <vector>
 
 namespace hazard_status_converter
 {
 
-Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", options)
+Converter::Converter(const rclcpp::NodeOptions & options)
+: Node("converter", options), mode_interface_(*this)
 {
   using std::placeholders::_1;
   pub_hazard_ = create_publisher<HazardStatusStamped>("~/hazard_status", rclcpp::QoS(1));
@@ -31,9 +33,9 @@ Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", op
 
 void Converter::on_create(DiagGraph::ConstSharedPtr graph)
 {
-  const auto find_auto_mode_root = [](const DiagGraph & graph) {
+  const auto find_auto_mode_root = [](const DiagGraph & graph, const std::string & root_name) {
     for (const auto & unit : graph.units()) {
-      if (unit->path() == "/autoware/modes/autonomous") return unit;
+      if (unit->path() == root_name) return unit;
     }
     return static_cast<DiagUnit *>(nullptr);
   };
@@ -53,8 +55,17 @@ void Converter::on_create(DiagGraph::ConstSharedPtr graph)
     return result;
   };
 
-  auto_mode_root_ = find_auto_mode_root(*graph);
-  auto_mode_tree_ = make_auto_mode_tree(auto_mode_root_);
+  std::vector<std::pair<RootModeStatus::Mode, std::string>> root_modes = {
+    {RootModeStatus::Mode::AUTO, "/autoware/modes/autonomous"},
+    {RootModeStatus::Mode::REMOTE, "/autoware/modes/remote"},
+    {RootModeStatus::Mode::LOCAL, "/autoware/modes/local"},
+  };
+  for (const auto & [mode, root_name] : root_modes) {
+    ModeSubgraph subgraph;
+    subgraph.root = find_auto_mode_root(*graph, root_name);
+    subgraph.nodes = make_auto_mode_tree(subgraph.root);
+    mode_subgraphs_[mode] = subgraph;
+  }
 }
 
 void Converter::on_update(DiagGraph::ConstSharedPtr graph)
@@ -98,17 +109,24 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph)
     return static_cast<std::vector<DiagnosticStatus> *>(nullptr);
   };
 
-  if (!auto_mode_root_) {
-    RCLCPP_ERROR_STREAM_THROTTLE(get_logger(), *get_clock(), 10000, "No auto mode unit.");
+  // Get subgraph related to the current operation mode.
+  const auto root = mode_interface_.get_root();
+  if (root.mode == RootModeStatus::Mode::UNKNOWN) {
+    RCLCPP_ERROR_STREAM_THROTTLE(get_logger(), *get_clock(), 10000, "Unknown root mode.");
+    return;
+  }
+  const auto & subgraph = mode_subgraphs_.at(root.mode);
+  if (!subgraph.root) {
+    RCLCPP_ERROR_STREAM_THROTTLE(get_logger(), *get_clock(), 10000, "No current mode root.");
     return;
   }
 
   // Calculate hazard level from unit level and root level.
   HazardStatusStamped hazard;
   for (const auto & unit : graph->units()) {
     if (unit->path().empty()) 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 bool is_ignored = root.ignore || !subgraph.nodes.count(unit);
+    const auto root_level = is_ignored ? DiagnosticStatus::OK : subgraph.root->level();
     const auto unit_level = unit->level();
     if (auto diags = get_hazards_vector(hazard.status, get_hazard_level(unit_level, root_level))) {
       diags->push_back(unit->create_diagnostic_status());

system/hazard_status_converter/src/converter.hpp

@@ -15,11 +15,14 @@
 #ifndef CONVERTER_HPP_
 #define CONVERTER_HPP_
 
+#include "mode_interface.hpp"
+
 #include <diagnostic_graph_utils/subscription.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_system_msgs/msg/hazard_status_stamped.hpp>
 
+#include <unordered_map>
 #include <unordered_set>
 
 namespace hazard_status_converter
@@ -39,8 +42,14 @@ class Converter : public rclcpp::Node
   diagnostic_graph_utils::DiagGraphSubscription sub_graph_;
   rclcpp::Publisher<HazardStatusStamped>::SharedPtr pub_hazard_;
 
-  DiagUnit * auto_mode_root_;
-  std::unordered_set<DiagUnit *> auto_mode_tree_;
+  struct ModeSubgraph
+  {
+    DiagUnit * root;
+    std::unordered_set<DiagUnit *> nodes;
+  };
+
+  ModeInterface mode_interface_;
+  std::unordered_map<RootModeStatus::Mode, ModeSubgraph> mode_subgraphs_;
 };
 
 }  // namespace hazard_status_converter

system/hazard_status_converter/src/mode_interface.cpp

@@ -0,0 +1,65 @@
+// Copyright 2025 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "mode_interface.hpp"
+
+namespace hazard_status_converter
+{
+
+ModeInterface::ModeInterface(rclcpp::Node & node) : node_(node)
+{
+  sub_operation_mode_state_ = node.create_subscription<OperationModeState>(
+    "/api/operation_mode/state", rclcpp::QoS(1).transient_local(),
+    [this](const OperationModeState & msg) { operation_mode_state_ = msg; });
+  sub_autoware_state_ = node.create_subscription<AutowareState>(
+    "/autoware/state", rclcpp::QoS(1),
+    [this](const AutowareState & msg) { autoware_state_ = msg; });
+}
+
+RootModeStatus ModeInterface::get_root() const
+{
+  const auto is_not_autoware_running = [this]() {
+    if (autoware_state_->state == AutowareState::INITIALIZING) return true;
+    if (autoware_state_->state == AutowareState::FINALIZING) return true;
+    return false;
+  };
+
+  const auto is_not_autonomous_ready = [this]() {
+    if (autoware_state_->state == AutowareState::WAITING_FOR_ROUTE) return true;
+    if (autoware_state_->state == AutowareState::PLANNING) return true;
+    return false;
+  };
+
+  // Returns unknown before receiving state messages.
+  if (!operation_mode_state_ || !autoware_state_) {
+    return {RootModeStatus::Mode::UNKNOWN, false};
+  }
+
+  // During stop mode refer to autonomous mode diagnostics.
+  if (operation_mode_state_->mode == OperationModeState::STOP) {
+    return {RootModeStatus::Mode::AUTO, is_not_autoware_running() || is_not_autonomous_ready()};
+  }
+  if (operation_mode_state_->mode == OperationModeState::AUTONOMOUS) {
+    return {RootModeStatus::Mode::AUTO, is_not_autoware_running() || is_not_autonomous_ready()};
+  }
+  if (operation_mode_state_->mode == OperationModeState::REMOTE) {
+    return {RootModeStatus::Mode::REMOTE, is_not_autoware_running()};
+  }
+  if (operation_mode_state_->mode == OperationModeState::LOCAL) {
+    return {RootModeStatus::Mode::LOCAL, is_not_autoware_running()};
+  }
+  return {RootModeStatus::Mode::UNKNOWN, false};
+}
+
+}  // namespace hazard_status_converter

system/hazard_status_converter/src/mode_interface.hpp

@@ -0,0 +1,59 @@
+// Copyright 2025 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MODE_INTERFACE_HPP_
+#define MODE_INTERFACE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_system_msgs/msg/autoware_state.hpp>
+
+#include <optional>
+
+namespace hazard_status_converter
+{
+
+struct RootModeStatus
+{
+  enum class Mode {
+    UNKNOWN,
+    AUTO,
+    REMOTE,
+    LOCAL,
+  };
+  Mode mode;
+  bool ignore;
+};
+
+class ModeInterface
+{
+public:
+  explicit ModeInterface(rclcpp::Node & node);
+  RootModeStatus get_root() const;
+
+private:
+  using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState;
+  using AutowareState = autoware_system_msgs::msg::AutowareState;
+  rclcpp::Node & node_;
+  rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_state_;
+  rclcpp::Subscription<AutowareState>::SharedPtr sub_autoware_state_;
+
+  std::optional<OperationModeState> operation_mode_state_;
+  std::optional<AutowareState> autoware_state_;
+};
+
+}  // namespace hazard_status_converter
+
+#endif  // MODE_INTERFACE_HPP_