diff --git a/system/hazard_status_converter/package.xml b/system/hazard_status_converter/package.xml index 9ae7a384927b9..082aa85cba97a 100644 --- a/system/hazard_status_converter/package.xml +++ b/system/hazard_status_converter/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs autoware_auto_system_msgs diagnostic_msgs rclcpp diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index e1da4f463f06d..58282306491dd 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/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 & nodes, int index) +void set_root_tree(std::vector & 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 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( "~/diagnostics_graph", rclcpp::QoS(3), std::bind(&Converter::on_graph, this, std::placeholders::_1)); + sub_state_ = create_subscription( + "/autoware/state", rclcpp::QoS(1), + std::bind(&Converter::on_state, this, std::placeholders::_1)); + sub_mode_ = create_subscription( + "/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; diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp index 90c6f6e42bf85..04e84cfb3c0c4 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/hazard_status_converter/src/converter.hpp @@ -17,6 +17,8 @@ #include +#include +#include #include #include @@ -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::SharedPtr sub_state_; + rclcpp::Subscription::SharedPtr sub_mode_; rclcpp::Subscription::SharedPtr sub_graph_; rclcpp::Publisher::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