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

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -140,6 +140,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

@@ -1,4 +1,4 @@
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -91,6 +91,7 @@
   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();
@@ -358,24 +359,29 @@
   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();
 
@@ -477,22 +483,28 @@
 
   // 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;
       }
     }
     if (operation_mode_availability_->comfortable_stop) {
       if (param_.use_comfortable_stop) {
         return MrmState::COMFORTABLE_STOP;
       }
     }
     if (!operation_mode_availability_->emergency_stop) {
       RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
     }
     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;
@@ -509,6 +521,9 @@
     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;
@@ -525,6 +540,9 @@
     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;
@@ -551,7 +569,8 @@
 
 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()