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 Signed-off-by: Tomohito Ando <[email protected]>
tier4 · Nov 22, 2024 · f5e0af7 · f5e0af7
1 parent 63d3c28
commit f5e0af7
Show file tree

Hide file tree

Showing 4 changed files with 26 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
@@ -53,8 +53,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{};
 };
@@ -142,6 +144,7 @@ class MrmHandler : public rclcpp::Node
   // Algorithm
   bool is_emergency_holding_ = false;
   uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE};
+  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(const rclcpp::NodeOptions & options) : 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);
 
@@ -405,6 +408,9 @@ void MrmHandler::updateMrmState()
     case MrmState::NORMAL:
       if (is_control_mode_autonomous && is_operation_mode_autonomous) {
         transitionTo(MrmState::MRM_OPERATING);
+        if (!param_.is_mrm_recoverable) {
+          is_mrm_holding_ = true;
+        }
       }
       return;
 
@@ -485,7 +491,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;
       }
     }
@@ -504,7 +510,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;
       }
     }
@@ -528,7 +534,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::isControlModeAutonomous()