From 3eb031244bf12d7da3888645eb019505b7a8da0a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 29 Feb 2024 00:19:56 +0900 Subject: [PATCH] feat(avoidance): wait next shift approval until the ego reaches shift length threshold (#6501) Signed-off-by: satoshi-ota Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../config/avoidance.param.yaml | 5 ++++ .../data_structs.hpp | 5 ++++ .../behavior_path_avoidance_module/helper.hpp | 27 +++++++++++++++++++ .../parameter_helper.hpp | 6 +++++ .../src/scene.cpp | 3 ++- 5 files changed, 45 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index d9ae9ed623cb3..4c93f7a460e42 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -207,6 +207,11 @@ max_right_shift_length: 5.0 max_left_shift_length: 5.0 max_deviation_from_lane: 0.5 # [m] + # approve the next shift line after reaching this percentage of the current shift line length. + # this parameter should be within range of 0.0 to 1.0. + # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. + # this feature can be disabled by setting this parameter to 1.0. (in that case, avoidance module creates return shift as soon as possible.) + ratio_for_return_shift_approval: 0.5 # [-] # avoidance distance parameters longitudinal: min_prepare_time: 1.0 # [s] diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index a536fa1b568cb..6aace5a939f9b 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -276,6 +276,9 @@ struct AvoidanceParameters // use for judge if the ego is shifting or not. double lateral_avoid_check_threshold{0.0}; + // use for return shift approval. + double ratio_for_return_shift_approval{0.0}; + // For shift line generation process. The continuous shift length is quantized by this value. double quantize_filter_threshold{0.0}; @@ -542,6 +545,8 @@ struct AvoidancePlanningData bool valid{false}; + bool ready{false}; + bool success{false}; bool comfortable{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index cc00bfbb978f5..4f15261f42246 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -270,6 +270,33 @@ class AvoidanceHelper }); } + bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const + { + if (std::abs(current_shift_length) < 1e-3) { + return true; + } + + if (new_shift_lines.empty()) { + return true; + } + + const auto front_shift_relative_length = new_shift_lines.front().getRelativeLength(); + + // same direction shift. + if (current_shift_length > 0.0 && front_shift_relative_length > 0.0) { + return true; + } + + // same direction shift. + if (current_shift_length < 0.0 && front_shift_relative_length < 0.0) { + return true; + } + + // keep waiting the other side shift approval until the ego reaches shift length threshold. + const auto ego_shift_ratio = (current_shift_length - getEgoShift()) / current_shift_length; + return ego_shift_ratio < parameters_->ratio_for_return_shift_approval + 1e-3; + } + bool isShifted() const { return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 03902e4328eb2..f1b273ae43c24 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -256,6 +256,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); p.max_deviation_from_lane = getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); + p.ratio_for_return_shift_approval = + getOrDeclareParameter(*node, ns + "ratio_for_return_shift_approval"); + if (p.ratio_for_return_shift_approval < 0.0 || p.ratio_for_return_shift_approval > 1.0) { + throw std::domain_error( + "ratio_for_return_shift_approval should be within range of 0.0 to 1.0"); + } } // avoidance maneuver (longitudinal) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 75529c35c2af5..cc15fe3b24bf7 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -133,7 +133,7 @@ bool AvoidanceModule::isExecutionRequested() const bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid; + return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const @@ -513,6 +513,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de */ data.comfortable = helper_->isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); + data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()); } void AvoidanceModule::fillEgoStatus(