diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index c706e1d773907..98f12ba6b9467 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/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 stamp_autonomous_become_unavailable_ = std::nullopt; + bool is_operation_mode_availability_timeout; + void checkOperationModeAvailabilityTimeout(); // Algorithm bool is_emergency_holding_ = false; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 8ed9017a3ad3b..03cc08de2d160 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -91,6 +91,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(); @@ -358,24 +359,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(); @@ -477,6 +483,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; @@ -493,6 +502,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; @@ -509,6 +521,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; @@ -525,6 +540,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; @@ -551,7 +569,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()