Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(behavior_path_planner_common): modify drivable area expansion to be able to avoid static objects #10220

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

feat(behavior_path_planner_common): modify drivable area expansion to be able to avoid static objects #10220

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
99e50a0
modify drivable area expansion to avoid static objects
mkquda Mar 3, 2025
b3844f5
rename parameters and update drivable area design md
mkquda Mar 4, 2025
40170c4
Update planning/behavior_path_planner/autoware_behavior_path_planner_…
mkquda Mar 4, 2025
59371c8
correct parameters description
mkquda Mar 4, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 9 additions & 7 deletions ...ior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,13 +19,15 @@
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: false # if true, the drivable area is not expanded in the predicted path of dynamic objects
extra_footprint_offset:
front: 0.5 # [m] extra length to add to the front of the dynamic object footprint
rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint
left: 0.5 # [m] extra length to add to the left of the dynamic object footprint
right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint
object_exclusion:
exclude_static: true # if true, the drivable area is not expanded over static objects
exclude_dynamic: false # if true, the drivable area is not expanded in the predicted path of dynamic objects
th_stopped_object_velocity: 1.0 # [m/s] velocity threshold for static objects
safety_margin:
front: 0.75 # [m] margin to add to the front of object footprint
rear: 0.75 # [m] margin to add to the rear of object footprint
left: 0.75 # [m] margin to add to the left of object footprint
right: 0.75 # [m] margin to add to the rear of object footprint
path_preprocessing:
max_arc_length: 100.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)
Expand Down
24 changes: 15 additions & 9 deletions ...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -806,36 +806,42 @@
update_param(
parameters, DrivableAreaExpansionParameters::ENABLED_PARAM,
planner_data_->drivable_area_expansion_parameters.enabled);
update_param(
parameters, DrivableAreaExpansionParameters::AVOID_STA_OBJECTS_PARAM,
planner_data_->drivable_area_expansion_parameters.object_exclusion.exclude_static);
update_param(
parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM,
planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects);
planner_data_->drivable_area_expansion_parameters.object_exclusion.exclude_dynamic);
update_param(
parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM,
planner_data_->drivable_area_expansion_parameters.avoid_linestring_types);
update_param(
parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM,
planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist);
update_param(
parameters, DrivableAreaExpansionParameters::EGO_EXTRA_FRONT_OVERHANG,
planner_data_->drivable_area_expansion_parameters.extra_front_overhang);
update_param(
parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WHEELBASE,
planner_data_->drivable_area_expansion_parameters.extra_wheelbase);
update_param(
parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH,
planner_data_->drivable_area_expansion_parameters.extra_width);
update_param(
parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT,
planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset);
parameters, DrivableAreaExpansionParameters::OBJECTS_SAFE_MARGIN_FRONT,
planner_data_->drivable_area_expansion_parameters.object_exclusion.front_offset);
update_param(
parameters, DrivableAreaExpansionParameters::OBJECTS_SAFE_MARGIN_REAR,
planner_data_->drivable_area_expansion_parameters.object_exclusion.rear_offset);
update_param(
parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_REAR,
planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_rear_offset);
parameters, DrivableAreaExpansionParameters::OBJECTS_SAFE_MARGIN_LEFT,
planner_data_->drivable_area_expansion_parameters.object_exclusion.left_offset);
update_param(
parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_LEFT,
planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_left_offset);
parameters, DrivableAreaExpansionParameters::OBJECTS_SAFE_MARGIN_RIGHT,
planner_data_->drivable_area_expansion_parameters.object_exclusion.right_offset);
update_param(
parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_RIGHT,
planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_right_offset);
parameters, DrivableAreaExpansionParameters::STOPPED_OBJ_VEL_THRESH,
planner_data_->drivable_area_expansion_parameters.object_exclusion.stopped_obj_vel_th);

Check warning on line 844 in planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

BehaviorPathPlannerNode::onSetParam increases from 89 to 95 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
update_param(
parameters, DrivableAreaExpansionParameters::MAX_EXP_DIST_PARAM,
planner_data_->drivable_area_expansion_parameters.max_expansion_distance);
Expand Down
53 changes: 30 additions & 23 deletions ...behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,27 +32,29 @@ Currently, when clipping left bound or right bound, it can clip the bound more t

### Dynamic expansion

| Name | Unit | Type | Description | Default value |
| :------------------------------------------- | :---- | :----------- | :------------------------------------------------------------------------------------------------------ | :--------------------------- |
| enabled | [-] | boolean | if true, dynamically expand the drivable area based on the path curvature | true |
| print_runtime | [-] | boolean | if true, runtime is logged by the node | true |
| max_expansion_distance | [m] | double | maximum distance by which the original drivable area can be expanded (no limit if set to 0) | 0.0 |
| smoothing.curvature_average_window | [-] | int | window size used for smoothing the curvatures using a moving window average | 3 |
| smoothing.max_bound_rate | [m/m] | double | maximum rate of change of the bound lateral distance over its arc length | 1.0 |
| smoothing.arc_length_range | [m] | double | arc length range where an expansion distance is initially applied | 2.0 |
| ego.extra_wheel_base | [m] | double | extra ego wheelbase | 0.0 |
| ego.extra_front_overhang | [m] | double | extra ego overhang | 0.5 |
| ego.extra_width | [m] | double | extra ego width | 1.0 |
| dynamic_objects.avoid | [-] | boolean | if true, the drivable area is not expanded in the predicted path of dynamic objects | true |
| dynamic_objects.extra_footprint_offset.front | [m] | double | extra length to add to the front of the ego footprint | 0.5 |
| dynamic_objects.extra_footprint_offset.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.5 |
| dynamic_objects.extra_footprint_offset.left | [m] | double | extra length to add to the left of the ego footprint | 0.5 |
| dynamic_objects.extra_footprint_offset.right | [m] | double | extra length to add to the rear of the ego footprint | 0.5 |
| 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 |
| path_preprocessing.resample_interval | [m] | double | fixed interval between resampled path points (0.0 means path points are directly used) | 2.0 |
| 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 |
| avoid_linestring.types | [-] | string array | linestring types in the lanelet maps that will not be crossed when expanding the drivable area | ["road_border", "curbstone"] |
| avoid_linestring.distance | [m] | double | distance to keep between the drivable area and the linestrings to avoid | 0.0 |
| Name | Unit | Type | Description | Default value |
| :------------------------------------------ | :---- | :----------- | :------------------------------------------------------------------------------------------------------ | :--------------------------- |
| enabled | [-] | boolean | if true, dynamically expand the drivable area based on the path curvature | true |
| print_runtime | [-] | boolean | if true, runtime is logged by the node | true |
| max_expansion_distance | [m] | double | maximum distance by which the original drivable area can be expanded (no limit if set to 0) | 0.0 |
| smoothing.curvature_average_window | [-] | int | window size used for smoothing the curvatures using a moving window average | 3 |
| smoothing.max_bound_rate | [m/m] | double | maximum rate of change of the bound lateral distance over its arc length | 1.0 |
| smoothing.arc_length_range | [m] | double | arc length range where an expansion distance is initially applied | 2.0 |
| ego.extra_wheel_base | [m] | double | extra ego wheelbase | 0.0 |
| ego.extra_front_overhang | [m] | double | extra ego overhang | 0.5 |
| ego.extra_width | [m] | double | extra ego width | 1.0 |
| object_exclusion.exclude_static | [-] | boolean | if true, the drivable area is not expanded over static objects | true |
| object_exclusion.exclude_dynamic | [-] | boolean | if true, the drivable area is not expanded in the predicted path of dynamic objects | true |
| object_exclusion.th_stopped_object_velocity | [m/s] | double | velocity threshold for static objects | 1.0 |
| object_exclusion.safety_margin.front | [m] | double | extra length to add to the front of the object footprint | 0.75 |
| object_exclusion.safety_margin.rear | [m] | double | extra length to add to the rear of the object footprint | 0.75 |
| object_exclusion.safety_margin.left | [m] | double | extra length to add to the left of the object footprint | 0.75 |
| object_exclusion.safety_margin.right | [m] | double | extra length to add to the right of the object footprint | 0.75 |
| 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 |
| path_preprocessing.resample_interval | [m] | double | fixed interval between resampled path points (0.0 means path points are directly used) | 2.0 |
| 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 |
| avoid_linestring.types | [-] | string array | linestring types in the lanelet maps that will not be crossed when expanding the drivable area | ["road_border", "curbstone"] |
| avoid_linestring.distance | [m] | double | distance to keep between the drivable area and the linestrings to avoid | 0.0 |

## Inner-workings / Algorithms

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

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

##### 3 Calculate maximum expansion distances of each bound point based on dynamic objects and linestring of the vector map (optional)
##### 3 Calculate maximum expansion distances of each bound point based on detected objects and linestring of the vector map (optional)

For each drivable area bound point, we calculate its maximum expansion distance as its distance to the closest "obstacle" which could be:

1. Map linestring with type `avoid_linestrings.type`.
2. Static object footprint (if `object_exclusion.exclude_static` is set to `true`).
3. Dynamic object path footprint (if `object_exclusion.exclude_dynamic` is set to `true`).

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`).
If `max_expansion_distance` is not `0.0`, it is use here if smaller than the distance to the closest obstacle.

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