From ccdcb5c8ed8d93c48888ab31ad48e4addf300889 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 8 May 2023 16:45:35 +0900 Subject: [PATCH] refactor(avoidance): remove hard code params (#3624) * refactor(avoidance): remove hard code params in shift line triming process Signed-off-by: satoshi-ota * refactor(avoidance): update config Signed-off-by: satoshi-ota * chore(avoidance): add description for params Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 8 ++++ .../avoidance/avoidance_module.hpp | 6 +-- .../utils/avoidance/avoidance_module_data.hpp | 15 ++++++ .../src/behavior_path_planner_node.cpp | 14 ++++++ .../avoidance/avoidance_module.cpp | 48 +++++++++---------- .../src/scene_module/avoidance/manager.cpp | 14 ++++++ 6 files changed, 76 insertions(+), 29 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 0183c227bce9c..71a98a65baf89 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -143,3 +143,11 @@ matrix: [2.78, 13.9, # velocity [m/s] 0.50, 1.00] # margin [m] + + shift_line_pipeline: + trim: + quantize_filter_threshold: 0.2 + same_grad_filter_1_threshold: 0.1 + same_grad_filter_2_threshold: 0.2 + same_grad_filter_3_threshold: 0.5 + sharp_shift_filter_threshold: 0.2 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 3d0fbf72bdf9d..bd65d4a7b285b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -228,12 +228,12 @@ class AvoidanceModule : public SceneModuleInterface // shift point generation: trimmers AvoidLineArray trimShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; - void quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const; - void trimSmallShiftLine(AvoidLineArray & shift_lines, const double shift_diff_thres) const; + void quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const; void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const; void trimMomentaryReturn(AvoidLineArray & shift_lines) const; void trimTooSharpShift(AvoidLineArray & shift_lines) const; - void trimSharpReturn(AvoidLineArray & shift_lines) const; + void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const; // shift point generation: return-shift generator void addReturnShiftLineFromEgo( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index c1fc19db51658..e5c25081474f4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -238,6 +238,21 @@ struct AvoidanceParameters // avoidance path will be generated unless their lateral margin difference exceeds this value. double avoidance_execution_lateral_threshold; + // For shift line generation process. The continuous shift length is quantized by this value. + double quantize_filter_threshold; + + // For shift line generation process. Merge small shift lines. (First step) + double same_grad_filter_1_threshold; + + // For shift line generation process. Merge small shift lines. (Second step) + double same_grad_filter_2_threshold; + + // For shift line generation process. Merge small shift lines. (Third step) + double same_grad_filter_3_threshold; + + // For shift line generation process. Remove sharp(=jerky) shift line. + double sharp_shift_filter_threshold; + // target velocity matrix std::vector target_velocity_matrix; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bb491086992f0..87edf64c97674 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -657,6 +657,20 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.target_velocity_matrix = declare_parameter>(ns + "matrix"); } + // shift line pipeline + { + std::string ns = "avoidance.shift_line_pipeline."; + p.quantize_filter_threshold = declare_parameter(ns + "trim.quantize_filter_threshold"); + p.same_grad_filter_1_threshold = + declare_parameter(ns + "trim.same_grad_filter_1_threshold"); + p.same_grad_filter_2_threshold = + declare_parameter(ns + "trim.same_grad_filter_2_threshold"); + p.same_grad_filter_3_threshold = + declare_parameter(ns + "trim.same_grad_filter_3_threshold"); + p.sharp_shift_filter_threshold = + declare_parameter(ns + "trim.sharp_shift_filter_threshold"); + } + return p; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index d028fc9fc3800..1ab26c99f59a2 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1288,8 +1288,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Combine avoid points that have almost same gradient. // this is to remove the noise. { - const auto CHANGE_SHIFT_THRESHOLD_FOR_NOISE = 0.1; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD_FOR_NOISE); + const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift"); } @@ -1297,8 +1297,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Quantize the shift length to reduce the shift point noise // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. { - constexpr double QUANTIZATION_DISTANCE = 0.2; - quantizeShiftLine(sl_array_trimmed, QUANTIZATION_DISTANCE); + const auto THRESHOLD = parameters_->quantize_filter_threshold; + quantizeShiftLine(sl_array_trimmed, THRESHOLD); printShiftLines(sl_array_trimmed, "after sl_array_trimmed"); debug.quantized = sl_array_trimmed; } @@ -1313,8 +1313,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Combine avoid points that have almost same gradient (again) { - const auto CHANGE_SHIFT_THRESHOLD = 0.2; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD); + const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift_second = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); } @@ -1323,15 +1323,16 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // Check if it is not too sharp for the return-to-center shift point. // If the shift is sharp, it is combined with the next shift point until it gets non-sharp. { - trimSharpReturn(sl_array_trimmed); + const auto THRESHOLD = parameters_->sharp_shift_filter_threshold; + trimSharpReturn(sl_array_trimmed, THRESHOLD); debug.trim_too_sharp_shift = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trimSharpReturn"); } // - Combine avoid points that have almost same gradient (again) { - const auto CHANGE_SHIFT_THRESHOLD = 0.5; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD); + const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift_third = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); } @@ -1362,21 +1363,20 @@ void AvoidanceModule::alignShiftLinesOrder( } } -void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const +void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const { - if (interval < 1.0e-5) { + if (threshold < 1.0e-5) { return; // no need to process } for (auto & sl : shift_lines) { - sl.end_shift_length = std::round(sl.end_shift_length / interval) * interval; + sl.end_shift_length = std::round(sl.end_shift_length / threshold) * threshold; } alignShiftLinesOrder(shift_lines); } -void AvoidanceModule::trimSmallShiftLine( - AvoidLineArray & shift_lines, const double shift_diff_thres) const +void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const { AvoidLineArray shift_lines_orig = shift_lines; shift_lines.clear(); @@ -1391,7 +1391,7 @@ void AvoidanceModule::trimSmallShiftLine( auto sl_modified = sl_now; // remove the shift point if the length is almost same as the previous one. - if (std::abs(shift_diff) < shift_diff_thres) { + if (std::abs(shift_diff) < threshold) { sl_modified.end_shift_length = sl_prev.end_shift_length; sl_modified.start_shift_length = sl_prev.end_shift_length; DEBUG_PRINT( @@ -1409,7 +1409,7 @@ void AvoidanceModule::trimSmallShiftLine( } void AvoidanceModule::trimSimilarGradShiftLine( - AvoidLineArray & avoid_lines, const double change_shift_dist_threshold) const + AvoidLineArray & avoid_lines, const double threshold) const { AvoidLineArray avoid_lines_orig = avoid_lines; avoid_lines.clear(); @@ -1436,8 +1436,7 @@ void AvoidanceModule::trimSimilarGradShiftLine( const auto longitudinal = original.end_longitudinal - combined_al.start_longitudinal; const auto new_length = combined_al.getGradient() * longitudinal + combined_al.start_shift_length; - const bool has_large_change = - std::abs(new_length - original.end_shift_length) > change_shift_dist_threshold; + const bool has_large_change = std::abs(new_length - original.end_shift_length) > threshold; DEBUG_PRINT( "original.end_shift_length: %f, original.end_longitudinal: %f, " @@ -1446,7 +1445,7 @@ void AvoidanceModule::trimSimilarGradShiftLine( original.end_shift_length, original.end_longitudinal, combined_al.start_longitudinal, combined_al.getGradient(), new_length, has_large_change); - if (std::abs(new_length - original.end_shift_length) > change_shift_dist_threshold) { + if (std::abs(new_length - original.end_shift_length) > threshold) { return true; } } @@ -1620,7 +1619,7 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const DEBUG_PRINT("trimMomentaryReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); } -void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines) const +void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const { AvoidLineArray shift_lines_orig = shift_lines; shift_lines.clear(); @@ -1651,17 +1650,14 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines) const }; // Check if the merged shift has a conflict with the original shifts. - const auto hasViolation = [this](const auto & combined, const auto & combined_src) { - constexpr auto VIOLATION_SHIFT_THR = 0.3; + const auto hasViolation = [&threshold](const auto & combined, const auto & combined_src) { for (const auto & sl : combined_src) { const auto combined_shift = utils::avoidance::lerpShiftLengthOnArc(sl.end_longitudinal, combined); - if ( - sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + VIOLATION_SHIFT_THR) { + if (sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + threshold) { return true; } - if ( - sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - VIOLATION_SHIFT_THR) { + if (sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - threshold) { return true; } } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 993219ebfbb62..9f1b836d6e29e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -95,6 +95,20 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk); } + { + const std::string ns = "avoidance.shift_line_pipeline."; + updateParam( + parameters, ns + "trim.quantize_filter_threshold", p->quantize_filter_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_1_threshold", p->same_grad_filter_1_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_2_threshold", p->same_grad_filter_2_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_3_threshold", p->same_grad_filter_3_threshold); + updateParam( + parameters, ns + "trim.sharp_shift_filter_threshold", p->sharp_shift_filter_threshold); + } + std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { m->updateModuleParams(p); });