diff --git a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml index f3b07bfa94834..84318a921d4ce 100644 --- a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml +++ b/system/hazard_status_converter/launch/hazard_status_converter.launch.xml @@ -2,5 +2,6 @@ + diff --git a/system/hazard_status_converter/package.xml b/system/hazard_status_converter/package.xml index ec54b4c003080..7d154abb7b26c 100644 --- a/system/hazard_status_converter/package.xml +++ b/system/hazard_status_converter/package.xml @@ -12,6 +12,7 @@ autoware_adapi_v1_msgs autoware_system_msgs + autoware_universe_utils diagnostic_graph_utils diagnostic_msgs rclcpp diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index e8213b441bb33..d92af2186f415 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -117,7 +117,10 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph) hazard.stamp = graph->updated_stamp(); hazard.status.level = get_system_level(hazard.status); hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT; - hazard.status.emergency_holding = false; + + const auto is_emergency_holding = sub_emergency_holding_.takeData(); + hazard.status.emergency_holding = + is_emergency_holding == nullptr ? false : is_emergency_holding->is_holding; pub_hazard_->publish(hazard); } diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp index 442eedf588429..8011b911f3d42 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/hazard_status_converter/src/converter.hpp @@ -15,10 +15,12 @@ #ifndef CONVERTER_HPP_ #define CONVERTER_HPP_ +#include #include #include #include +#include #include @@ -38,6 +40,9 @@ class Converter : public rclcpp::Node void on_update(DiagGraph::ConstSharedPtr graph); diagnostic_graph_utils::DiagGraphSubscription sub_graph_; rclcpp::Publisher::SharedPtr pub_hazard_; + autoware::universe_utils::InterProcessPollingSubscriber< + tier4_system_msgs::msg::EmergencyHoldingState> + sub_emergency_holding_{this, "~/input/emergency_holding"}; DiagUnit * auto_mode_root_; std::unordered_set auto_mode_tree_;