From 9079986bcedfd40fbc442b03d89aedf14f26b0c6 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Mon, 3 Jun 2024 20:49:32 +0900 Subject: [PATCH] add pull over after stopped option --- system/mrm_handler/config/mrm_handler.param.yaml | 1 + system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp | 1 + system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 5 +++-- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index 15549d6c157f0..a5ccf7add987e 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -11,6 +11,7 @@ is_mrm_recoverable: true use_parking_after_stopped: false use_pull_over: false + use_pull_over_after_stopped: false use_comfortable_stop: false # setting whether to turn hazard lamp on for each situation 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 18484fa0fede9..fef9cd6f92387 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -54,6 +54,7 @@ struct Param bool is_mrm_recoverable; bool use_parking_after_stopped; bool use_pull_over; + bool use_pull_over_after_stopped; bool use_comfortable_stop; HazardLampPolicy turning_hazard_on{}; }; 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 b420442b33eed..7aa3d2deb569a 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -32,6 +32,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler") 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_pull_over_after_stopped = declare_parameter("use_pull_over_after_stopped", false); param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency", true); @@ -548,7 +549,7 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (isStopped() && operation_mode_availability_->pull_over) { - if (param_.use_pull_over) { + if (param_.use_pull_over && param_.use_pull_over_after_stopped) { return MrmState::PULL_OVER; } } @@ -567,7 +568,7 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (isStopped() && operation_mode_availability_->pull_over) { - if (param_.use_pull_over) { + if (param_.use_pull_over && param_.use_pull_over_after_stopped) { return MrmState::PULL_OVER; } }