From 82dba03c7179638640313d6053fc5bc2c876ff41 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 17 Aug 2023 13:32:52 +0900 Subject: [PATCH] fix(avoidance): fix trimmed shift line alignment (#4632) * fix(avoidance): fix trimmed shift line alignment Signed-off-by: satoshi-ota * fix(avoidance): fill shift line gap Signed-off-by: satoshi-ota * fixup! fix(avoidance): fix trimmed shift line alignment --------- Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.hpp | 2 + .../avoidance/avoidance_module.cpp | 61 ++++++++++++++++--- 2 files changed, 55 insertions(+), 8 deletions(-) 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 55ecee17b9778..436fbed76cf5e 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 @@ -329,6 +329,8 @@ class AvoidanceModule : public SceneModuleInterface AvoidLineArray applyPreProcessToRawShiftLines( AvoidLineArray & current_raw_shift_points, DebugData & debug) const; + AvoidLineArray getFillGapShiftLines(const AvoidLineArray & shift_lines) const; + /* * @brief merge negative & positive shift lines. * @param original shift lines. 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 10a6307b263b4..88bc583345966 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 @@ -700,6 +700,8 @@ void AvoidanceModule::updateRegisteredRawShiftLines() AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( AvoidLineArray & raw_shift_lines, DebugData & debug) const { + const auto fill_gap_shift_lines = getFillGapShiftLines(raw_shift_lines); + /** * Use all registered points. For the current points, if the similar one of the current * points are already registered, will not use it. @@ -729,6 +731,8 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( * Add gap filled shift lines so that merged shift lines connect smoothly. */ fillShiftLineGap(raw_shift_lines); + raw_shift_lines.insert( + raw_shift_lines.end(), fill_gap_shift_lines.begin(), fill_gap_shift_lines.end()); debug.gap_filled = raw_shift_lines; /** @@ -974,10 +978,6 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( al_avoid.end_longitudinal = o.longitudinal - offset; al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; - - if (is_valid_shift_line(al_avoid)) { - avoid_lines.push_back(al_avoid); - } } AvoidLine al_return; @@ -997,10 +997,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; + } - if (is_valid_shift_line(al_return)) { - avoid_lines.push_back(al_return); - } + if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + avoid_lines.push_back(al_avoid); + avoid_lines.push_back(al_return); } o.is_avoidable = true; @@ -1216,6 +1217,38 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ return merged_avoid_lines; } +AvoidLineArray AvoidanceModule::getFillGapShiftLines(const AvoidLineArray & shift_lines) const +{ + AvoidLineArray ret{}; + + if (shift_lines.empty()) { + return ret; + } + + const auto calc_gap_shift_line = [&](const auto & line1, const auto & line2) { + AvoidLine gap_filled_line{}; + gap_filled_line.start_shift_length = line1.end_shift_length; + gap_filled_line.start_longitudinal = line1.end_longitudinal; + gap_filled_line.end_shift_length = line2.start_shift_length; + gap_filled_line.end_longitudinal = line2.start_longitudinal; + gap_filled_line.id = getOriginalShiftLineUniqueId(); + + return gap_filled_line; + }; + + // fill gap among shift lines. + for (size_t i = 0; i < shift_lines.size() - 1; i += 2) { + if (shift_lines.at(i).end_longitudinal > shift_lines.at(i + 1).start_longitudinal) { + continue; + } + ret.push_back(calc_gap_shift_line(shift_lines.at(i), shift_lines.at(i + 1))); + } + + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoidance_data_, ret); + + return ret; +} + void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const { using utils::avoidance::setEndData; @@ -2343,12 +2376,16 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat std::abs(candidates.at(i).getRelativeLength()) > parameters_->lateral_small_shift_threshold) { if (has_large_shift) { - break; + return; } has_large_shift = true; } + if (!isComfortable(AvoidLineArray{candidates.at(i)})) { + return; + } + subsequent.push_back(candidates.at(i)); } }; @@ -2357,10 +2394,18 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat const auto get_subsequent_shift = [&, this](size_t i) { AvoidLineArray subsequent{candidates.at(i)}; + if (!isComfortable(subsequent)) { + return subsequent; + } + if (candidates.size() == i + 1) { return subsequent; } + if (!isComfortable(AvoidLineArray{candidates.at(i + 1)})) { + return subsequent; + } + if ( std::abs(candidates.at(i).getRelativeLength()) < parameters_->lateral_small_shift_threshold) { const auto has_large_shift =