diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 3fcf68bac06a2..035cc579d2a82 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -160,8 +160,9 @@ multi_polygon_t createExpansionPolygons( : footprint_dist; auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); + const auto uncrossable_dist_limit = std::max( + 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - + params.avoid_linestring_dist); if (uncrossable_dist_limit < limited_dist) { limited_dist = uncrossable_dist_limit; if (params.compensate_uncrossable_lines) {