fix(emergency_handler): fix hazard status timeout (autowarefoundation#6399)

* fix(emergency_handler): fix hazard status timeout

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>

Author: veqcc

system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

@@ -131,14 +131,16 @@ class EmergencyHandler : public rclcpp::Node
 
   // Heartbeat
   rclcpp::Time stamp_hazard_status_;
+  bool is_hazard_status_timeout_;
+  void checkHazardStatusTimeout();
 
   // Algorithm
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();
   autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
-  bool isEmergency(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status);
+  bool isEmergency();
 };
 
 #endif  // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

@@ -83,6 +83,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   mrm_state_.stamp = this->now();
   mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL;
   mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
+  is_hazard_status_timeout_ = false;
 
   // Timer
   const auto update_period_ns = rclcpp::Rate(param_.update_rate).period();
@@ -135,7 +136,7 @@ autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHaz
   HazardLightsCommand msg;
 
   // Check emergency
-  const bool is_emergency = isEmergency(hazard_status_stamped_->status);
+  const bool is_emergency = isEmergency();
 
   if (hazard_status_stamped_->status.emergency_holding) {
     // turn hazard on during emergency holding
@@ -309,22 +310,27 @@ bool EmergencyHandler::isDataReady()
   return true;
 }
 
-void EmergencyHandler::onTimer()
+void EmergencyHandler::checkHazardStatusTimeout()
 {
-  if (!isDataReady()) {
-    return;
-  }
-  const bool is_hazard_status_timeout =
-    (this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status;
-  if (is_hazard_status_timeout) {
+  if ((this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status) {
+    is_hazard_status_timeout_ = true;
     RCLCPP_WARN_THROTTLE(
       this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
       "heartbeat_hazard_status is timeout");
-    mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING;
-    publishControlCommands();
+  } else {
+    is_hazard_status_timeout_ = false;
+  }
+}
+
+void EmergencyHandler::onTimer()
+{
+  if (!isDataReady()) {
     return;
   }
 
+  // Check whether heartbeat hazard_status is timeout
+  checkHazardStatusTimeout();
+
   // Update Emergency State
   updateMrmState();
 
@@ -369,7 +375,7 @@ void EmergencyHandler::updateMrmState()
   using autoware_auto_vehicle_msgs::msg::ControlModeReport;
 
   // Check emergency
-  const bool is_emergency = isEmergency(hazard_status_stamped_->status);
+  const bool is_emergency = isEmergency();
 
   // Get mode
   const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
@@ -412,7 +418,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre
   using autoware_auto_system_msgs::msg::HazardStatus;
 
   // Get hazard level
-  const auto level = hazard_status_stamped_->status.level;
+  auto level = hazard_status_stamped_->status.level;
+  if (is_hazard_status_timeout_) {
+    level = HazardStatus::SINGLE_POINT_FAULT;
+  }
 
   // State machine
   if (mrm_state_.behavior == MrmState::NONE) {
@@ -435,10 +444,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre
   return mrm_state_.behavior;
 }
 
-bool EmergencyHandler::isEmergency(
-  const autoware_auto_system_msgs::msg::HazardStatus & hazard_status)
+bool EmergencyHandler::isEmergency()
 {
-  return hazard_status.emergency || hazard_status.emergency_holding;
+  return hazard_status_stamped_->status.emergency ||
+         hazard_status_stamped_->status.emergency_holding || is_hazard_status_timeout_;
 }
 
 bool EmergencyHandler::isStopped()