feat(behavior_path_planner): curvature based drivable area expansion (#…

…632)

* Modify parameters for curvature based dynamic drivable area expansion

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Add parameter to enable/disable printing the runtime

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Add smoothing.extra_arc_length param

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

---------

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml

@@ -5,15 +5,19 @@
     drivable_area_left_bound_offset: 0.0
     drivable_area_types_to_skip: [road_border]
 
-    # Dynamic expansion by projecting the ego footprint along the path
+    # Dynamic expansion by using the path curvature
     dynamic_expansion:
-      enabled: false
+      enabled: true
+      print_runtime: false
+      max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit)
+      smoothing:
+        curvature_average_window: 3  # window size used for smoothing the curvatures using a moving window average
+        max_bound_rate: 1.0  # [m/m] maximum rate of change of the bound lateral distance over its arc length
+        extra_arc_length: 2.0  # [m] extra arc length where an expansion distance is initially applied
       ego:
-        extra_footprint_offset:
-          front: 0.5 # [m] extra length to add to the front of the ego footprint
-          rear: 0.5 # [m] extra length to add to the rear of the ego footprint
-          left: 0.5 # [m] extra length to add to the left of the ego footprint
-          right: 0.5 # [m] extra length to add to the rear of the ego footprint
+        extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase
+        extra_front_overhang: 0.5 # [m] extra length to add to the front overhang
+        extra_width: 1.0 # [m] extra length to add to the width
       dynamic_objects:
         avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects
         extra_footprint_offset:
@@ -24,16 +28,9 @@
       path_preprocessing:
         max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit)
         resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used)
-      expansion:
-        method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'.
-                         # 'lanelet': add lanelets overlapped by the ego footprints
-                         # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area
-        max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit)
-        extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint
+        reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused.
       avoid_linestring:
         types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area
           - road_border
+          - curbstone
         distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid
-        compensate:
-          enable: true  # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction
-          extra_distance: 3.0 # [m] extra distance to add to the compensation