From 7f320aa56e3057dfe9260a026b9dd9750a3061ba Mon Sep 17 00:00:00 2001 From: veqcc Date: Thu, 29 Feb 2024 13:47:47 +0900 Subject: [PATCH 1/3] fix operation mode availability timeout Signed-off-by: veqcc --- .../include/mrm_handler/mrm_handler_core.hpp | 2 ++ .../src/mrm_handler/mrm_handler_core.cpp | 31 ++++++++++++------- 2 files changed, 21 insertions(+), 12 deletions(-) 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 8a6ec81cbafff..4b9df373401c5 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -143,6 +143,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 rclcpp::Time takeover_requested_time_; 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 976f5b3164abd..9ebdfaa0e43b0 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -93,6 +93,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(); @@ -369,22 +370,26 @@ bool MrmHandler::isDataReady() return true; } +void MrmHandler::checkOperationModeAvailabilityTimeout() { + if ((this->now() - stamp_operation_mode_availability_).seconds() > + 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(), + "operation_mode_availability is timeout"); + } else { + is_operation_mode_availability_timeout = false; + } +} + void MrmHandler::onTimer() { if (!isDataReady()) { return; } - const bool is_operation_mode_availability_timeout = - (this->now() - stamp_operation_mode_availability_).seconds() > - param_.timeout_operation_mode_availability; - if (is_operation_mode_availability_timeout) { - 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; - publishControlCommands(); - return; - } + + // Check whether operation_mode_availability is timeout + checkOperationModeAvailabilityTimeout(); // Update Emergency State updateMrmState(); @@ -576,7 +581,9 @@ 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() From 4d4dd18dfc9d8f7fb58ee41a1e284d1aad7dfada Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 29 Feb 2024 04:54:50 +0000 Subject: [PATCH 2/3] style(pre-commit): autofix --- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 9ebdfaa0e43b0..163f725f46b35 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -370,8 +370,10 @@ bool MrmHandler::isDataReady() return true; } -void MrmHandler::checkOperationModeAvailabilityTimeout() { - if ((this->now() - stamp_operation_mode_availability_).seconds() > +void MrmHandler::checkOperationModeAvailabilityTimeout() +{ + if ( + (this->now() - stamp_operation_mode_availability_).seconds() > param_.timeout_operation_mode_availability) { is_operation_mode_availability_timeout = true; RCLCPP_WARN_THROTTLE( @@ -581,8 +583,7 @@ 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; } From 28c7155c20d3aa3b3c4e43cb30ee1142bfcfa63c Mon Sep 17 00:00:00 2001 From: veqcc Date: Tue, 5 Mar 2024 14:06:09 +0900 Subject: [PATCH 3/3] fix behavior during timeout Signed-off-by: veqcc --- .../mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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 163f725f46b35..91e68b86ecc52 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -509,6 +509,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; @@ -525,6 +528,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; @@ -541,6 +547,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; @@ -557,6 +566,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;