From 83311245dc596e51e117ebcff644b8639e7d2585 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 11 Jan 2024 01:39:06 +0900 Subject: [PATCH] fix(avoidance): return shift path was not generated expectedly (#6017) fix(avoidance): output return shift path properly Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 3 ++- .../src/shift_line_generator.cpp | 8 +++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 9d7f5cc459e9e..61941eeee6f00 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1390,7 +1390,8 @@ void AvoidanceModule::insertReturnDeadLine( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); - const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length); + const auto min_return_distance = + helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 8ed56ce9aff4d..dabb0d7257555 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -1101,12 +1101,14 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = std::max(nominal_prepare_distance, last_sl_distance); + double prepare_distance_scaled = + std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); - prepare_distance_scaled = last_sl_distance + scale * nominal_prepare_distance; + prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1219,7 +1221,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { break; }