From f5e0af7e5fa886badba3f6d88c3f2844477c6d59 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 5 Jun 2024 09:57:07 +0900 Subject: [PATCH] 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 --- system/mrm_handler/config/mrm_handler.param.yaml | 2 ++ .../include/mrm_handler/mrm_handler_core.hpp | 3 +++ system/mrm_handler/schema/mrm_handler.schema.json | 12 ++++++++++++ .../mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 12 +++++++++--- 4 files changed, 26 insertions(+), 3 deletions(-) diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index e82ee36a7825a..a5ccf7add987e 100644 --- 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 index f73d0df4153ce..b2d835778e93e 100644 --- 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 index 9a50c6a326f01..75ab13cd913d9 100644 --- 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 index bac932a7d7e1e..e8c057152ad94 100644 --- 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("timeout_cancel_mrm_behavior", 0.01); param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); + param_.is_mrm_recoverable = declare_parameter("is_mrm_recoverable", true); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); param_.use_pull_over = declare_parameter("use_pull_over", false); + param_.use_pull_over_after_stopped = + declare_parameter("use_pull_over_after_stopped", false); param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); param_.turning_hazard_on.emergency = declare_parameter("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()