implement is_mrm_recoverable option of mrm_handler

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

Hide file tree

Showing 3 changed files with 7 additions and 1 deletion.
diff --git 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
@@ -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
@@ -29,6 +29,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
     declare_parameter<double>("timeout_cancel_mrm_behavior", 0.01);
   param_.use_emergency_holding = declare_parameter<bool>("use_emergency_holding", false);
   param_.timeout_emergency_recovery = declare_parameter<double>("timeout_emergency_recovery", 5.0);
+  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_comfortable_stop = declare_parameter<bool>("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()