From 9dc94c361982725897fab9253d8b8e0883af9979 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Mon, 3 Jun 2024 15:32:58 +0900 Subject: [PATCH] implement is_mrm_recoverable option of mrm_handler --- system/mrm_handler/config/mrm_handler.param.yaml | 1 + system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp | 2 ++ system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 5 ++++- 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index e82ee36a7825a..15549d6c157f0 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -8,6 +8,7 @@ timeout_cancel_mrm_behavior: 0.01 use_emergency_holding: false timeout_emergency_recovery: 5.0 + is_mrm_recoverable: true use_parking_after_stopped: false use_pull_over: false use_comfortable_stop: false 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 1dd0f6b081337..18484fa0fede9 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -51,6 +51,7 @@ struct Param double timeout_cancel_mrm_behavior; bool use_emergency_holding; double timeout_emergency_recovery; + bool is_mrm_recoverable; bool use_parking_after_stopped; bool use_pull_over; bool use_comfortable_stop; @@ -149,6 +150,7 @@ class MrmHandler : public rclcpp::Node // Algorithm bool is_emergency_holding_ = false; + bool is_mrm_holding_ = false; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); 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 e594465150ad0..b420442b33eed 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -29,6 +29,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler") declare_parameter("timeout_cancel_mrm_behavior", 0.01); param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); + param_.is_mrm_recoverable = declare_parameter("is_mrm_recoverable", true); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); param_.use_pull_over = declare_parameter("use_pull_over", false); param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); @@ -459,6 +460,8 @@ void MrmHandler::updateMrmState() // NORMAL if (is_vehicle_auto_mode && is_operation_mode_auto_mode && is_emergency) { transitionTo(MrmState::MRM_OPERATING); + if (!param_.is_mrm_recoverable) + is_mrm_holding_ = true; return; } } else { @@ -590,7 +593,7 @@ bool MrmHandler::isStopped() bool MrmHandler::isEmergency() const { return !operation_mode_availability_->autonomous || is_emergency_holding_ || - is_operation_mode_availability_timeout; + is_operation_mode_availability_timeout || is_mrm_holding_; } bool MrmHandler::isArrivedAtGoal()