Skip to content

Commit 97c1195

Browse files
mkqudamaxime-clem
mkquda
and
maxime-clem
authored
feat(behavior_path_planner_common): modify drivable area expansion to be able to avoid static objects (#10220)
* modify drivable area expansion to avoid static objects Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * rename parameters and update drivable area design md Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * correct parameters description Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> --------- Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
1 parent fb43521 commit 97c1195

File tree

7 files changed

+127
-79
lines changed

7 files changed

+127
-79
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml

+9-7
Original file line numberDiff line numberDiff line change
@@ -19,13 +19,15 @@
1919
extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase
2020
extra_front_overhang: 0.5 # [m] extra length to add to the front overhang
2121
extra_width: 1.0 # [m] extra length to add to the width
22-
dynamic_objects:
23-
avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects
24-
extra_footprint_offset:
25-
front: 0.5 # [m] extra length to add to the front of the dynamic object footprint
26-
rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint
27-
left: 0.5 # [m] extra length to add to the left of the dynamic object footprint
28-
right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint
22+
object_exclusion:
23+
exclude_static: true # if true, the drivable area is not expanded over static objects
24+
exclude_dynamic: false # if true, the drivable area is not expanded in the predicted path of dynamic objects
25+
th_stopped_object_velocity: 1.0 # [m/s] velocity threshold for static objects
26+
safety_margin:
27+
front: 0.75 # [m] margin to add to the front of object footprint
28+
rear: 0.75 # [m] margin to add to the rear of object footprint
29+
left: 0.75 # [m] margin to add to the left of object footprint
30+
right: 0.75 # [m] margin to add to the rear of object footprint
2931
path_preprocessing:
3032
max_arc_length: 100.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit)
3133
resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used)

‎planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

+15-9
Original file line numberDiff line numberDiff line change
@@ -806,9 +806,12 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
806806
update_param(
807807
parameters, DrivableAreaExpansionParameters::ENABLED_PARAM,
808808
planner_data_->drivable_area_expansion_parameters.enabled);
809+
update_param(
810+
parameters, DrivableAreaExpansionParameters::AVOID_STA_OBJECTS_PARAM,
811+
planner_data_->drivable_area_expansion_parameters.object_exclusion.exclude_static);
809812
update_param(
810813
parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM,
811-
planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects);
814+
planner_data_->drivable_area_expansion_parameters.object_exclusion.exclude_dynamic);
812815
update_param(
813816
parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM,
814817
planner_data_->drivable_area_expansion_parameters.avoid_linestring_types);
@@ -825,17 +828,20 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
825828
parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH,
826829
planner_data_->drivable_area_expansion_parameters.extra_width);
827830
update_param(
828-
parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT,
829-
planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset);
831+
parameters, DrivableAreaExpansionParameters::OBJECTS_SAFE_MARGIN_FRONT,
832+
planner_data_->drivable_area_expansion_parameters.object_exclusion.front_offset);
833+
update_param(
834+
parameters, DrivableAreaExpansionParameters::OBJECTS_SAFE_MARGIN_REAR,
835+
planner_data_->drivable_area_expansion_parameters.object_exclusion.rear_offset);
830836
update_param(
831-
parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_REAR,
832-
planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_rear_offset);
837+
parameters, DrivableAreaExpansionParameters::OBJECTS_SAFE_MARGIN_LEFT,
838+
planner_data_->drivable_area_expansion_parameters.object_exclusion.left_offset);
833839
update_param(
834-
parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_LEFT,
835-
planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_left_offset);
840+
parameters, DrivableAreaExpansionParameters::OBJECTS_SAFE_MARGIN_RIGHT,
841+
planner_data_->drivable_area_expansion_parameters.object_exclusion.right_offset);
836842
update_param(
837-
parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_RIGHT,
838-
planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_right_offset);
843+
parameters, DrivableAreaExpansionParameters::STOPPED_OBJ_VEL_THRESH,
844+
planner_data_->drivable_area_expansion_parameters.object_exclusion.stopped_obj_vel_th);
839845
update_param(
840846
parameters, DrivableAreaExpansionParameters::MAX_EXP_DIST_PARAM,
841847
planner_data_->drivable_area_expansion_parameters.max_expansion_distance);

‎planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md

+30-23
Original file line numberDiff line numberDiff line change
@@ -32,27 +32,29 @@ Currently, when clipping left bound or right bound, it can clip the bound more t
3232

3333
### Dynamic expansion
3434

35-
| Name | Unit | Type | Description | Default value |
36-
| :------------------------------------------- | :---- | :----------- | :------------------------------------------------------------------------------------------------------ | :--------------------------- |
37-
| enabled | [-] | boolean | if true, dynamically expand the drivable area based on the path curvature | true |
38-
| print_runtime | [-] | boolean | if true, runtime is logged by the node | true |
39-
| max_expansion_distance | [m] | double | maximum distance by which the original drivable area can be expanded (no limit if set to 0) | 0.0 |
40-
| smoothing.curvature_average_window | [-] | int | window size used for smoothing the curvatures using a moving window average | 3 |
41-
| smoothing.max_bound_rate | [m/m] | double | maximum rate of change of the bound lateral distance over its arc length | 1.0 |
42-
| smoothing.arc_length_range | [m] | double | arc length range where an expansion distance is initially applied | 2.0 |
43-
| ego.extra_wheel_base | [m] | double | extra ego wheelbase | 0.0 |
44-
| ego.extra_front_overhang | [m] | double | extra ego overhang | 0.5 |
45-
| ego.extra_width | [m] | double | extra ego width | 1.0 |
46-
| dynamic_objects.avoid | [-] | boolean | if true, the drivable area is not expanded in the predicted path of dynamic objects | true |
47-
| dynamic_objects.extra_footprint_offset.front | [m] | double | extra length to add to the front of the ego footprint | 0.5 |
48-
| dynamic_objects.extra_footprint_offset.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.5 |
49-
| dynamic_objects.extra_footprint_offset.left | [m] | double | extra length to add to the left of the ego footprint | 0.5 |
50-
| dynamic_objects.extra_footprint_offset.right | [m] | double | extra length to add to the rear of the ego footprint | 0.5 |
51-
| path_preprocessing.max_arc_length | [m] | double | maximum arc length along the path where the ego footprint is projected (0.0 means no limit) | 100.0 |
52-
| path_preprocessing.resample_interval | [m] | double | fixed interval between resampled path points (0.0 means path points are directly used) | 2.0 |
53-
| path_preprocessing.reuse_max_deviation | [m] | double | if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused | 0.5 |
54-
| avoid_linestring.types | [-] | string array | linestring types in the lanelet maps that will not be crossed when expanding the drivable area | ["road_border", "curbstone"] |
55-
| avoid_linestring.distance | [m] | double | distance to keep between the drivable area and the linestrings to avoid | 0.0 |
35+
| Name | Unit | Type | Description | Default value |
36+
| :------------------------------------------ | :---- | :----------- | :------------------------------------------------------------------------------------------------------ | :--------------------------- |
37+
| enabled | [-] | boolean | if true, dynamically expand the drivable area based on the path curvature | true |
38+
| print_runtime | [-] | boolean | if true, runtime is logged by the node | true |
39+
| max_expansion_distance | [m] | double | maximum distance by which the original drivable area can be expanded (no limit if set to 0) | 0.0 |
40+
| smoothing.curvature_average_window | [-] | int | window size used for smoothing the curvatures using a moving window average | 3 |
41+
| smoothing.max_bound_rate | [m/m] | double | maximum rate of change of the bound lateral distance over its arc length | 1.0 |
42+
| smoothing.arc_length_range | [m] | double | arc length range where an expansion distance is initially applied | 2.0 |
43+
| ego.extra_wheel_base | [m] | double | extra ego wheelbase | 0.0 |
44+
| ego.extra_front_overhang | [m] | double | extra ego overhang | 0.5 |
45+
| ego.extra_width | [m] | double | extra ego width | 1.0 |
46+
| object_exclusion.exclude_static | [-] | boolean | if true, the drivable area is not expanded over static objects | true |
47+
| object_exclusion.exclude_dynamic | [-] | boolean | if true, the drivable area is not expanded in the predicted path of dynamic objects | true |
48+
| object_exclusion.th_stopped_object_velocity | [m/s] | double | velocity threshold for static objects | 1.0 |
49+
| object_exclusion.safety_margin.front | [m] | double | extra length to add to the front of the object footprint | 0.75 |
50+
| object_exclusion.safety_margin.rear | [m] | double | extra length to add to the rear of the object footprint | 0.75 |
51+
| object_exclusion.safety_margin.left | [m] | double | extra length to add to the left of the object footprint | 0.75 |
52+
| object_exclusion.safety_margin.right | [m] | double | extra length to add to the right of the object footprint | 0.75 |
53+
| path_preprocessing.max_arc_length | [m] | double | maximum arc length along the path where the ego footprint is projected (0.0 means no limit) | 100.0 |
54+
| path_preprocessing.resample_interval | [m] | double | fixed interval between resampled path points (0.0 means path points are directly used) | 2.0 |
55+
| path_preprocessing.reuse_max_deviation | [m] | double | if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused | 0.5 |
56+
| avoid_linestring.types | [-] | string array | linestring types in the lanelet maps that will not be crossed when expanding the drivable area | ["road_border", "curbstone"] |
57+
| avoid_linestring.distance | [m] | double | distance to keep between the drivable area and the linestrings to avoid | 0.0 |
5658

5759
## Inner-workings / Algorithms
5860

@@ -133,9 +135,14 @@ This equation was derived from the work of [Lim, H., Kim, C., and Jo, A., "Model
133135

134136
![min width](../images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg)
135137

136-
##### 3 Calculate maximum expansion distances of each bound point based on dynamic objects and linestring of the vector map (optional)
138+
##### 3 Calculate maximum expansion distances of each bound point based on detected objects and linestring of the vector map (optional)
139+
140+
For each drivable area bound point, we calculate its maximum expansion distance as its distance to the closest "obstacle" which could be:
141+
142+
1. Map linestring with type `avoid_linestrings.type`.
143+
2. Static object footprint (if `object_exclusion.exclude_static` is set to `true`).
144+
3. Dynamic object path footprint (if `object_exclusion.exclude_dynamic` is set to `true`).
137145

138-
For each drivable area bound point, we calculate its maximum expansion distance as its distance to the closest "obstacle" (either a map linestring with type `avoid_linestrings.type`, or a dynamic object footprint if `dynamic_objects.avoid` is set to `true`).
139146
If `max_expansion_distance` is not `0.0`, it is use here if smaller than the distance to the closest obstacle.
140147

141148
![max distances](../images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg)

0 commit comments

Comments
 (0)