add pull over after stopped option

tier4 · Jun 3, 2024 · 9079986 · 9079986
1 parent 9dc94c3
commit 9079986
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 2 deletions.
diff --git 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
@@ -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
@@ -32,6 +32,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
   param_.is_mrm_recoverable = declare_parameter<bool>("is_mrm_recoverable", true);
   param_.use_parking_after_stopped = declare_parameter<bool>("use_parking_after_stopped", false);
   param_.use_pull_over = declare_parameter<bool>("use_pull_over", false);
+  param_.use_pull_over_after_stopped = declare_parameter<bool>("use_pull_over_after_stopped", false);
   param_.use_comfortable_stop = declare_parameter<bool>("use_comfortable_stop", false);
   param_.turning_hazard_on.emergency = declare_parameter<bool>("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;
       }
     }