Correctly use the max expansion distance when smoothing over arc lengths

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -156,6 +156,7 @@ void apply_extra_arc_length(
   const auto & bound_projections =
     side == LEFT ? expansion.left_projections : expansion.right_projections;
   auto & bound_expansions = side == LEFT ? expansion.left_distances : expansion.right_distances;
+  const auto original_expansions = bound_expansions;
   for (auto path_idx = 0UL; path_idx < bound_indexes.size(); ++path_idx) {
     const auto bound_idx = bound_indexes[path_idx];
     auto arc_length = boost::geometry::distance(
@@ -164,15 +165,16 @@ void apply_extra_arc_length(
       arc_length +=
         tier4_autoware_utils::calcDistance2d(bound[up_bound_idx - 1], bound[up_bound_idx]);
       if (arc_length > extra_arc_length) break;
-      bound_expansions[up_bound_idx] = bound_expansions[bound_idx];
+      bound_expansions[up_bound_idx] =
+        std::max(bound_expansions[up_bound_idx], original_expansions[bound_idx]);
     }
     arc_length =
       boost::geometry::distance(bound_projections[path_idx].point, convert_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 > extra_arc_length) break;
-      bound_expansions[idx] = bound_expansions[bound_idx];
+      bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]);
     }
   }
 }