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