From 2b3aa06e2090384042d49b9e8ea34ba1b828fc7f Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Wed, 6 Mar 2024 13:25:18 +0900 Subject: [PATCH] feat(mrm_handler, emergency_handler): remove takeover (#6522) update(mrm_handler, emergency_handler): remove takeover Signed-off-by: veqcc Signed-off-by: kaigohirao --- .../config/emergency_handler.param.yaml | 2 -- .../emergency_handler_core.hpp | 4 --- .../schema/emergency_handler.schema.json | 12 -------- .../emergency_handler_core.cpp | 28 +++---------------- .../mrm_handler/config/mrm_handler.param.yaml | 2 -- .../include/mrm_handler/mrm_handler_core.hpp | 4 --- .../schema/mrm_handler.schema.json | 12 -------- .../src/mrm_handler/mrm_handler_core.cpp | 28 +++---------------- 8 files changed, 8 insertions(+), 84 deletions(-) diff --git a/system/emergency_handler/config/emergency_handler.param.yaml b/system/emergency_handler/config/emergency_handler.param.yaml index 652a984ce539a..2309e7f23deb0 100644 --- a/system/emergency_handler/config/emergency_handler.param.yaml +++ b/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 diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 6d83c6781fad7..e10e31b5d3351 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/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(); diff --git a/system/emergency_handler/schema/emergency_handler.schema.json b/system/emergency_handler/schema/emergency_handler.schema.json index 8295a5c6efb6b..3276ae3fa1553 100644 --- a/system/emergency_handler/schema/emergency_handler.schema.json +++ b/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" diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index 17fc92f3d36c7..d591abad8c8ed 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/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("update_rate"); param_.timeout_hazard_status = declare_parameter("timeout_hazard_status"); - param_.timeout_takeover_request = declare_parameter("timeout_takeover_request"); - param_.use_takeover_request = declare_parameter("use_takeover_request"); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped"); param_.use_comfortable_stop = declare_parameter("use_comfortable_stop"); param_.turning_hazard_on.emergency = declare_parameter("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); diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index 292d459f69d88..a0295fb07627c 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/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 diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 8a6ec81cbafff..74892f2b785d7 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/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 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(); diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json index 86b18449cb734..aedb8076ef05c 100644 --- a/system/mrm_handler/schema/mrm_handler.schema.json +++ b/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", diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 976f5b3164abd..1e016af5c1fe1 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -26,8 +26,6 @@ MrmHandler::MrmHandler() : Node("mrm_handler") declare_parameter("timeout_operation_mode_availability", 0.5); param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); - param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); - param_.use_takeover_request = declare_parameter("use_takeover_request", false); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); param_.use_pull_over = declare_parameter("use_pull_over", false); param_.use_comfortable_stop = declare_parameter("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()) {