feat(mrm_handler): mrm recoverable option (#1316)

* implement is_mrm_recoverable option of mrm_handler * add pull over after stopped option * update json schema of mrm_handler
tier4 · Jun 5, 2024 · f3d3197 · f3d3197
1 parent f45652e
commit f3d3197
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 3 deletions.
diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml
@@ -8,8 +8,10 @@
     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_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
@@ -51,8 +51,10 @@ 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_pull_over_after_stopped;
   bool use_comfortable_stop;
   HazardLampPolicy turning_hazard_on{};
 };
@@ -149,6 +151,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/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json
@@ -36,6 +36,11 @@
           "description": "If the duration of the EMERGENCY state is longer than `timeout_emergency_recovery`, it does not recover to the NORMAL state.",
           "default": 5.0
         },
+        "is_mrm_recoverable": {
+          "type": "boolean",
+          "description": "If this parameter is true, mrm state never return to normal state",
+          "default": false
+        },
         "use_parking_after_stopped": {
           "type": "boolean",
           "description": "If this parameter is true, it will publish PARKING shift command.",
@@ -46,6 +51,11 @@
           "description": "If this parameter is true, operate pull over when latent faults occur.",
           "default": "false"
         },
+        "use_pull_over_after_stopped": {
+          "type": "boolean",
+          "description": "If this parameter is true, pull over can be operated after stopped.",
+          "default": "true"
+        },
         "use_comfortable_stop": {
           "type": "boolean",
           "description": "If this parameter is true, operate comfortable stop when latent faults occur.",
@@ -70,8 +80,10 @@
         "timeout_cancel_mrm_behavior",
         "use_emergency_holding",
         "timeout_emergency_recovery",
+        "is_mrm_recoverable",
         "use_parking_after_stopped",
         "use_pull_over",
+        "use_pull_over_after_stopped",
         "use_comfortable_stop",
         "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
@@ -29,8 +29,11 @@ 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_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);
 
@@ -459,6 +462,7 @@ 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 {
@@ -545,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;
       }
     }
@@ -564,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;
       }
     }
@@ -590,7 +594,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()