fix(emergency_handler): stop using heartbeat_checher to make subscrib…

…er unique
autowarefoundation · Oct 24, 2022 · d639bb5 · d639bb5
1 parent 3a4845c
commit d639bb5
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 68 deletions.
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp
diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
@@ -30,7 +30,6 @@
 // ROS2 core
 #include <rclcpp/create_timer.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/system/heartbeat_checker.hpp>
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -102,8 +101,7 @@ class EmergencyHandler : public rclcpp::Node
   void onTimer();
 
   // Heartbeat
-  std::shared_ptr<HeaderlessHeartbeatChecker<autoware_auto_system_msgs::msg::HazardStatusStamped>>
-    heartbeat_hazard_status_;
+  rclcpp::Time stamp_hazard_status_;
 
   // Algorithm
   autoware_auto_system_msgs::msg::EmergencyState::_state_type emergency_state_{

diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml
@@ -18,7 +18,6 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
-  <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
@@ -45,11 +45,6 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   sub_control_mode_ = create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
     "~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1));
 
-  // Heartbeat
-  heartbeat_hazard_status_ = std::make_shared<
-    HeaderlessHeartbeatChecker<autoware_auto_system_msgs::msg::HazardStatusStamped>>(
-    *this, "~/input/hazard_status", param_.timeout_hazard_status);
-
   // Publisher
   pub_control_command_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_command", rclcpp::QoS{1});
@@ -76,6 +71,7 @@ void EmergencyHandler::onHazardStatusStamped(
   const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg)
 {
   hazard_status_stamped_ = msg;
+  stamp_hazard_status_ = this->now();
 }
 
 void EmergencyHandler::onPrevControlCommand(
@@ -178,7 +174,9 @@ void EmergencyHandler::onTimer()
   if (!isDataReady()) {
     return;
   }
-  if (heartbeat_hazard_status_->isTimeout()) {
+  const bool is_hazard_status_timeout =
+    (this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status;
+  if (is_hazard_status_timeout) {
     RCLCPP_WARN_THROTTLE(
       this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
       "heartbeat_hazard_status is timeout");