diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md index b6c90a3fe8db5..5f153bd6e8f7b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md @@ -32,29 +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 | -| 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 | extra length to add to the front of the ego footprint | 0.5 | -| object_exclusion.safety_margin.front | [m] | double | extra length to add to the front of the ego footprint | 0.5 | -| object_exclusion.safety_margin.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | -| object_exclusion.safety_margin.left | [m] | double | extra length to add to the left of the ego footprint | 0.5 | -| object_exclusion.safety_margin.right | [m] | double | extra length to add to the right 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 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 557d729a54a9d..db8cb0fb0d223 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -50,8 +50,10 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.path_preprocessing.max_arc_length"; static constexpr auto MAX_REUSE_DEVIATION_PARAM = "dynamic_expansion.path_preprocessing.reuse_max_deviation"; - static constexpr auto AVOID_STA_OBJECTS_PARAM = "dynamic_expansion.object_exclusion.exclude_static"; - static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.object_exclusion.exclude_dynamic"; + static constexpr auto AVOID_STA_OBJECTS_PARAM = + "dynamic_expansion.object_exclusion.exclude_static"; + static constexpr auto AVOID_DYN_OBJECTS_PARAM = + "dynamic_expansion.object_exclusion.exclude_dynamic"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; static constexpr auto AVOID_LINESTRING_DIST_PARAM = "dynamic_expansion.avoid_linestring.distance"; static constexpr auto SMOOTHING_CURVATURE_WINDOW_PARAM = @@ -83,7 +85,8 @@ struct DrivableAreaExpansionParameters double min_bound_interval{}; bool print_runtime{}; - struct ObjectExclusion{ + struct ObjectExclusion + { bool exclude_static{}; bool exclude_dynamic{}; double front_offset{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 184d0c9590321..e93187fd128ff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -15,7 +15,6 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" - #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include @@ -74,11 +73,14 @@ MultiPolygon2d create_object_footprints( continue; } - if (params.object_exclusion.exclude_static && velocity_filter( - object.kinematics.initial_twist_with_covariance.twist, -std::numeric_limits::epsilon(), - params.object_exclusion.stopped_obj_vel_th)) { - footprints.push_back(create_footprint(object.kinematics.initial_pose_with_covariance.pose, base_footprint)); - } + if ( + params.object_exclusion.exclude_static && + velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, + -std::numeric_limits::epsilon(), params.object_exclusion.stopped_obj_vel_th)) { + footprints.push_back( + create_footprint(object.kinematics.initial_pose_with_covariance.pose, base_footprint)); + } } return footprints; }