feat(hazard_status_converter): hazard status converter subscribe emer…

…gency holding (#9287) * feat: add subscriber for emergency_holding Signed-off-by: TetsuKawa <[email protected]> * modify: fix value name Signed-off-by: TetsuKawa <[email protected]> * style(pre-commit): autofix Signed-off-by: TetsuKawa <[email protected]> * feat: add const to is_emergency_holding Co-authored-by: Takagi, Isamu <[email protected]> Signed-off-by: TetsuKawa <[email protected]> --------- Signed-off-by: TetsuKawa <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <[email protected]>
autowarefoundation · Nov 14, 2024 · 827a5d9 · 827a5d9
1 parent 4e12cb0
commit 827a5d9
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 1 deletion.
diff --git 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 @@
   <node pkg="hazard_status_converter" exec="converter" name="hazard_status_converter">
     <remap from="~/diagnostics_graph" to="/diagnostics_graph"/>
     <remap from="~/hazard_status" to="/system/emergency/hazard_status"/>
+    <remap from="~/input/emergency_holding" to="/system/emergency_holding"/>
   </node>
 </launch>
diff --git a/system/hazard_status_converter/package.xml b/system/hazard_status_converter/package.xml
@@ -12,6 +12,7 @@
 
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_system_msgs</depend>
+  <depend>autoware_universe_utils</depend>
   <depend>diagnostic_graph_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>rclcpp</depend>

diff --git 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
@@ -15,10 +15,12 @@
 #ifndef CONVERTER_HPP_
 #define CONVERTER_HPP_
 
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <diagnostic_graph_utils/subscription.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_system_msgs/msg/hazard_status_stamped.hpp>
+#include <tier4_system_msgs/msg/emergency_holding_state.hpp>
 
 #include <unordered_set>
 
@@ -38,6 +40,9 @@ class Converter : public rclcpp::Node
   void on_update(DiagGraph::ConstSharedPtr graph);
   diagnostic_graph_utils::DiagGraphSubscription sub_graph_;
   rclcpp::Publisher<HazardStatusStamped>::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<DiagUnit *> auto_mode_tree_;