feat(mrm_handler, emergency_handler): remove takeover (#6522)

update(mrm_handler, emergency_handler): remove takeover

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>

Author: veqcc

system/emergency_handler/config/emergency_handler.param.yaml

@@ -4,8 +4,6 @@
   ros__parameters:
     update_rate: 10
     timeout_hazard_status: 0.5
-    timeout_takeover_request: 10.0
-    use_takeover_request: false
     use_parking_after_stopped: false
     use_comfortable_stop: false
 

system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

@@ -45,8 +45,6 @@ struct Param
 {
   int update_rate;
   double timeout_hazard_status;
-  double timeout_takeover_request;
-  bool use_takeover_request;
   bool use_parking_after_stopped;
   bool use_comfortable_stop;
   HazardLampPolicy turning_hazard_on{};
@@ -135,8 +133,6 @@ class EmergencyHandler : public rclcpp::Node
   rclcpp::Time stamp_hazard_status_;
 
   // Algorithm
-  rclcpp::Time takeover_requested_time_;
-  bool is_takeover_request_ = false;
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();

system/emergency_handler/schema/emergency_handler.schema.json

@@ -16,16 +16,6 @@
           "description": "If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop.",
           "default": 0.5
         },
-        "timeout_takeover_request": {
-          "type": "number",
-          "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.",
-          "default": 10.0
-        },
-        "use_takeover_request": {
-          "type": "boolean",
-          "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.",
-          "default": "false"
-        },
         "use_parking_after_stopped": {
           "type": "boolean",
           "description": "If this parameter is true, it will publish PARKING shift command.",
@@ -51,8 +41,6 @@
       "required": [
         "update_rate",
         "timeout_hazard_status",
-        "timeout_takeover_request",
-        "use_takeover_request",
         "use_parking_after_stopped",
         "use_comfortable_stop",
         "turning_hazard_on"

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

@@ -23,8 +23,6 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   // Parameter
   param_.update_rate = declare_parameter<int>("update_rate");
   param_.timeout_hazard_status = declare_parameter<double>("timeout_hazard_status");
-  param_.timeout_takeover_request = declare_parameter<double>("timeout_takeover_request");
-  param_.use_takeover_request = declare_parameter<bool>("use_takeover_request");
   param_.use_parking_after_stopped = declare_parameter<bool>("use_parking_after_stopped");
   param_.use_comfortable_stop = declare_parameter<bool>("use_comfortable_stop");
   param_.turning_hazard_on.emergency = declare_parameter<bool>("turning_hazard_on.emergency");
@@ -375,41 +373,23 @@ void EmergencyHandler::updateMrmState()
 
   // Get mode
   const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
-  const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL;
 
   // State Machine
   if (mrm_state_.state == MrmState::NORMAL) {
     // NORMAL
     if (is_auto_mode && is_emergency) {
-      if (param_.use_takeover_request) {
-        takeover_requested_time_ = this->get_clock()->now();
-        is_takeover_request_ = true;
-        return;
-      } else {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
+      transitionTo(MrmState::MRM_OPERATING);
+      return;
     }
   } else {
     // Emergency
-    // Send recovery events if "not emergency" or "takeover done"
+    // Send recovery events if "not emergency"
     if (!is_emergency) {
       transitionTo(MrmState::NORMAL);
       return;
     }
-    // TODO(Kenji Miyake): Check if human can safely override, for example using DSM
-    if (is_takeover_done) {
-      transitionTo(MrmState::NORMAL);
-      return;
-    }
 
-    if (is_takeover_request_) {
-      const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_;
-      if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
-    } else if (mrm_state_.state == MrmState::MRM_OPERATING) {
+    if (mrm_state_.state == MrmState::MRM_OPERATING) {
       // TODO(Kenji Miyake): Check MRC is accomplished
       if (isStopped()) {
         transitionTo(MrmState::MRM_SUCCEEDED);

system/mrm_handler/config/mrm_handler.param.yaml

@@ -6,8 +6,6 @@
     timeout_operation_mode_availability: 0.5
     use_emergency_holding: false
     timeout_emergency_recovery: 5.0
-    timeout_takeover_request: 10.0
-    use_takeover_request: false
     use_parking_after_stopped: false
     use_pull_over: false
     use_comfortable_stop: false

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -49,8 +49,6 @@ struct Param
   double timeout_operation_mode_availability;
   bool use_emergency_holding;
   double timeout_emergency_recovery;
-  double timeout_takeover_request;
-  bool use_takeover_request;
   bool use_parking_after_stopped;
   bool use_pull_over;
   bool use_comfortable_stop;
@@ -145,8 +143,6 @@ class MrmHandler : public rclcpp::Node
   std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt;
 
   // Algorithm
-  rclcpp::Time takeover_requested_time_;
-  bool is_takeover_request_ = false;
   bool is_emergency_holding_ = false;
   void transitionTo(const int new_state);
   void updateMrmState();

system/mrm_handler/schema/mrm_handler.schema.json

@@ -26,16 +26,6 @@
           "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
         },
-        "timeout_takeover_request": {
-          "type": "number",
-          "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.",
-          "default": 10.0
-        },
-        "use_takeover_request": {
-          "type": "boolean",
-          "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.",
-          "default": "false"
-        },
         "use_parking_after_stopped": {
           "type": "boolean",
           "description": "If this parameter is true, it will publish PARKING shift command.",
@@ -68,8 +58,6 @@
         "timeout_operation_mode_availability",
         "use_emergency_holding",
         "timeout_emergency_recovery",
-        "timeout_takeover_request",
-        "use_takeover_request",
         "use_parking_after_stopped",
         "use_pull_over",
         "use_comfortable_stop",

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

@@ -26,8 +26,6 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
     declare_parameter<double>("timeout_operation_mode_availability", 0.5);
   param_.use_emergency_holding = declare_parameter<bool>("use_emergency_holding", false);
   param_.timeout_emergency_recovery = declare_parameter<double>("timeout_emergency_recovery", 5.0);
-  param_.timeout_takeover_request = declare_parameter<double>("timeout_takeover_request", 10.0);
-  param_.use_takeover_request = declare_parameter<bool>("use_takeover_request", false);
   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);
@@ -434,41 +432,23 @@ void MrmHandler::updateMrmState()
 
   // Get mode
   const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
-  const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL;
 
   // State Machine
   if (mrm_state_.state == MrmState::NORMAL) {
     // NORMAL
     if (is_auto_mode && is_emergency) {
-      if (param_.use_takeover_request) {
-        takeover_requested_time_ = this->get_clock()->now();
-        is_takeover_request_ = true;
-        return;
-      } else {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
+      transitionTo(MrmState::MRM_OPERATING);
+      return;
     }
   } else {
     // Emergency
-    // Send recovery events if "not emergency" or "takeover done"
+    // Send recovery events if "not emergency"
     if (!is_emergency) {
       transitionTo(MrmState::NORMAL);
       return;
     }
-    // TODO(TetsuKawa): Check if human can safely override, for example using DSM
-    if (is_takeover_done) {
-      transitionTo(MrmState::NORMAL);
-      return;
-    }
 
-    if (is_takeover_request_) {
-      const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_;
-      if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
-    } else if (mrm_state_.state == MrmState::MRM_OPERATING) {
+    if (mrm_state_.state == MrmState::MRM_OPERATING) {
       // TODO(TetsuKawa): Check MRC is accomplished
       if (mrm_state_.behavior == MrmState::PULL_OVER) {
         if (isStopped() && isArrivedAtGoal()) {