From 0ff9c5645b8c7360a855284febb80ee8a4ccefd7 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 16 Feb 2024 17:45:29 +0900 Subject: [PATCH 1/3] feat(avoidance): wait next shift approval until the ego reaches shift length threshold Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 4 +++ .../data_structs.hpp | 5 ++++ .../behavior_path_avoidance_module/helper.hpp | 27 +++++++++++++++++++ .../parameter_helper.hpp | 2 ++ .../src/scene.cpp | 3 ++- 5 files changed, 40 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 ab4eda81f93c6..67f4e29c0b8aa 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -208,6 +208,10 @@ max_right_shift_length: 5.0 max_left_shift_length: 5.0 max_deviation_from_lane: 0.5 # [m] + # approve next shift line after reaching this percentage of current shift line length. + # this param is added so that it can wait return shift approval until the behind occlusion of avoid target is clear. + # it's able to disable this feature by setting this param 0.0. + 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 223db73e72a4c..e4df3a8069b16 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}; @@ -539,6 +542,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..8fee46b45696a 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; + } + 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 fcaad191b1a33..a0d1db9b9198a 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 @@ -258,6 +258,8 @@ 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"); } // avoidance maneuver (longitudinal) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 27cd15cc525df..ed3dc8d34a698 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( From a96fff6ac7c3006768c2b77baeb0037e35ce71fe Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 28 Feb 2024 08:49:58 +0900 Subject: [PATCH 2/3] fix: parameter description Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../config/avoidance.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 67f4e29c0b8aa..8b5f98a5a22ae 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -208,9 +208,9 @@ max_right_shift_length: 5.0 max_left_shift_length: 5.0 max_deviation_from_lane: 0.5 # [m] - # approve next shift line after reaching this percentage of current shift line length. - # this param is added so that it can wait return shift approval until the behind occlusion of avoid target is clear. - # it's able to disable this feature by setting this param 0.0. + # approve the next shift line after reaching this percentage of the current shift line length. + # 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 0.0. ratio_for_return_shift_approval: 0.5 # [-] # avoidance distance parameters longitudinal: From 38a377aad6f5ed8a8dfa274dfe810c9edee09989 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 28 Feb 2024 09:40:42 +0900 Subject: [PATCH 3/3] fix(avoidance): add param validation Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 3 ++- .../include/behavior_path_avoidance_module/helper.hpp | 2 +- .../behavior_path_avoidance_module/parameter_helper.hpp | 4 ++++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 8b5f98a5a22ae..cd2a51851fe5f 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -209,8 +209,9 @@ 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 0.0. + # 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: 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 8fee46b45696a..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 @@ -294,7 +294,7 @@ class AvoidanceHelper // 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; + return ego_shift_ratio < parameters_->ratio_for_return_shift_approval + 1e-3; } bool isShifted() const 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 a0d1db9b9198a..ce75fa03e96ad 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 @@ -260,6 +260,10 @@ AvoidanceParameters getParameter(rclcpp::Node * node) 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)