fix(mrm_handler): fix bug in operation mode availability timeout (#6513)

* fix operation mode availability timeout

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

Author: veqcc

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -144,6 +144,8 @@ class MrmHandler : public rclcpp::Node
   // Heartbeat
   rclcpp::Time stamp_operation_mode_availability_;
   std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt;
+  bool is_operation_mode_availability_timeout;
+  void checkOperationModeAvailabilityTimeout();
 
   // Algorithm
   bool is_emergency_holding_ = false;

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

@@ -94,6 +94,7 @@ MrmHandler::MrmHandler() : Node("mrm_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_operation_mode_availability_timeout = false;
 
   // Timer
   const auto update_period_ns = rclcpp::Rate(param_.update_rate).period();
@@ -376,24 +377,29 @@ bool MrmHandler::isDataReady()
   return true;
 }
 
-void MrmHandler::onTimer()
+void MrmHandler::checkOperationModeAvailabilityTimeout()
 {
-  if (!isDataReady()) {
-    return;
-  }
-  const bool is_operation_mode_availability_timeout =
+  if (
     (this->now() - stamp_operation_mode_availability_).seconds() >
-    param_.timeout_operation_mode_availability;
-  if (is_operation_mode_availability_timeout) {
+    param_.timeout_operation_mode_availability) {
+    is_operation_mode_availability_timeout = true;
     RCLCPP_WARN_THROTTLE(
       this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
-      "heartbeat operation_mode_availability is timeout");
-    mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING;
-    publishHazardCmd();
-    publishGearCmd();
+      "operation_mode_availability is timeout");
+  } else {
+    is_operation_mode_availability_timeout = false;
+  }
+}
+
+void MrmHandler::onTimer()
+{
+  if (!isDataReady()) {
     return;
   }
 
+  // Check whether operation_mode_availability is timeout
+  checkOperationModeAvailabilityTimeout();
+
   // Update Emergency State
   updateMrmState();
 
@@ -495,6 +501,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB
 
   // State machine
   if (mrm_state_.behavior == MrmState::NONE) {
+    if (is_operation_mode_availability_timeout) {
+      return MrmState::EMERGENCY_STOP;
+    }
     if (operation_mode_availability_->pull_over) {
       if (param_.use_pull_over) {
         return MrmState::PULL_OVER;
@@ -511,6 +520,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB
     return MrmState::EMERGENCY_STOP;
   }
   if (mrm_state_.behavior == MrmState::PULL_OVER) {
+    if (is_operation_mode_availability_timeout) {
+      return MrmState::EMERGENCY_STOP;
+    }
     if (operation_mode_availability_->pull_over) {
       if (param_.use_pull_over) {
         return MrmState::PULL_OVER;
@@ -527,6 +539,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB
     return MrmState::EMERGENCY_STOP;
   }
   if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) {
+    if (is_operation_mode_availability_timeout) {
+      return MrmState::EMERGENCY_STOP;
+    }
     if (isStopped() && operation_mode_availability_->pull_over) {
       if (param_.use_pull_over) {
         return MrmState::PULL_OVER;
@@ -543,6 +558,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB
     return MrmState::EMERGENCY_STOP;
   }
   if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) {
+    if (is_operation_mode_availability_timeout) {
+      return MrmState::EMERGENCY_STOP;
+    }
     if (isStopped() && operation_mode_availability_->pull_over) {
       if (param_.use_pull_over) {
         return MrmState::PULL_OVER;
@@ -569,7 +587,8 @@ bool MrmHandler::isStopped()
 
 bool MrmHandler::isEmergency() const
 {
-  return !operation_mode_availability_->autonomous || is_emergency_holding_;
+  return !operation_mode_availability_->autonomous || is_emergency_holding_ ||
+         is_operation_mode_availability_timeout;
 }
 
 bool MrmHandler::isArrivedAtGoal()