From bca5aaeb992bdc2d03bc8b048c4502656582a640 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 17 Oct 2023 12:19:56 +0900 Subject: [PATCH] fix(drivable_area_expansion): correct bound limit check (#5325) Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 35 +++++++------------ 1 file changed, 12 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index decd145f7a57b..6c91f79c65c02 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -124,30 +124,19 @@ std::vector calculate_minimum_expansions( minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); // apply the expansion to all bound points within the extra arc length - if (bound_idx + 2 < bound.size()) { - auto up_arc_length = - tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]) + - tier4_autoware_utils::calcDistance2d(bound[bound_idx + 1], bound[bound_idx + 2]); - for (auto up_bound_idx = bound_idx + 2; - bound_idx < bound.size() && up_arc_length <= params.extra_arc_length; - ++up_bound_idx) { - minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); - if (up_bound_idx + 1 < bound.size()) - up_arc_length += - tier4_autoware_utils::calcDistance2d(bound[up_bound_idx], bound[up_bound_idx + 1]); - } + auto arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]); + for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { + tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]); + if (arc_length > params.extra_arc_length) break; + minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); } - if (bound_idx > 0) { - auto down_arc_length = - tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]) + - tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]); - for (auto down_bound_idx = bound_idx - 1; - down_bound_idx > 0 && down_arc_length <= params.extra_arc_length; --down_bound_idx) { - minimum_expansions[down_bound_idx] = std::max(minimum_expansions[down_bound_idx], dist); - if (down_bound_idx > 1) - down_arc_length += tier4_autoware_utils::calcDistance2d( - bound[down_bound_idx], bound[down_bound_idx - 1]); - } + arc_length = tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]); + for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { + const auto idx = bound_idx - down_offset_idx; + arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + if (arc_length > params.extra_arc_length) break; + minimum_expansions[idx] = std::max(minimum_expansions[idx], dist); } break; }