fix(behavior_path_planner): make use of the avoid_linestring.distance…

… parameter (autowarefoundation#4198) Signed-off-by: Maxime CLEMENT <[email protected]>
tier4 · Aug 29, 2023 · 5cceb0e · 5cceb0e
1 parent 13d599c
commit 5cceb0e
Showing 1 changed file with 3 additions and 2 deletions.
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
@@ -165,8 +165,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) {