diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 28565d9ed387f..be36233684674 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/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,7 +165,8 @@ 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])); @@ -172,7 +174,7 @@ void apply_extra_arc_length( 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]); } } }