diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index aaa0f579c4782..a39ee96d01d2b 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -11,16 +11,18 @@ # avoidance module common setting enable_bound_clipping: false - enable_avoidance_over_same_direction: true - enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false enable_force_avoidance_for_stopped_vehicle: false enable_safety_check: true enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false disable_path_update: false - use_hatched_road_markings: false + + # drivable area setting + use_adjacent_lane: true + use_opposite_lane: true use_intersection_areas: false + use_hatched_road_markings: false # for debug publish_debug_marker: false diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 8c61e78a382af..7436c26533000 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -265,7 +265,7 @@ The shift length is set as a constant value before the feature is implemented. S - The obstacles' current lane and position. - The road shoulder with reference to the direction to avoid. -These elements are used to compute the distance from the object to the road's shoulder (`to_road_shoulder_distance`). The parameters `enable_avoidance_over_same_direction` and `enable_avoidance_over_opposite_direction` allows further configuration of the to `to_road_shoulder_distance`. The following image illustrates the configuration. +These elements are used to compute the distance from the object to the road's shoulder (`to_road_shoulder_distance`). The parameters `use_adjacent_lane` and `use_opposite_lane` allows further configuration of the to `to_road_shoulder_distance`. The following image illustrates the configuration. ![obstacle_to_road_shoulder_distance](../image/avoidance/obstacle_to_road_shoulder_distance.drawio.svg) @@ -451,6 +451,38 @@ The shift points are modified by a filtering process in order to get the expecte ## Other features +### Drivable area expansion + +This module has following parameters that sets which areas the path may extend into when generating an avoidance path. + +```yaml +# drivable area setting +use_adjacent_lane: true +use_opposite_lane: true +use_intersection_areas: false +use_hatched_road_markings: false +``` + +#### adjacent lane + +![fig1](../image/avoidance/use_adjacent_lane.svg) + +#### opposite lane + +![fig1](../image/avoidance/use_opposite_lane.svg) + +#### intersection areas + +The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) + +![fig1](../image/avoidance/use_intersection_areas.svg) + +#### hatched road markings + +The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) + +![fig1](../image/avoidance/use_hatched_road_markings.svg) + ### Safety check The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable_safety_check` as `true`. @@ -582,24 +614,32 @@ The avoidance specific parameter configuration file can be located at `src/autow namespace: `avoidance.` -| Name | Unit | Type | Description | Default value | -| :--------------------------------------- | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | -| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | -| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | -| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| enable_bound_clipping | [-] | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | -| enable_avoidance_over_same_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane. | true | -| enable_avoidance_over_opposite_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has opposite direction. `enable_avoidance_over_same_direction` must be `true` to take effects | true | -| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_safety_check | [-] | bool | Flag to enable safety check. | false | -| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | -| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | -| publish_debug_marker | [-] | bool | Flag to publish debug marker (set `false` as default since it takes considerable cost). | false | -| print_debug_info | [-] | bool | Flag to print debug info (set `false` as default since it takes considerable cost). | false | +| Name | Unit | Type | Description | Default value | +| :------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | +| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | +| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | +| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | +| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | +| enable_safety_check | [-] | bool | Flag to enable safety check. | false | +| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | +| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | **NOTE:** It has to set both `enable_safety_check` and `enable_yield_maneuver` to enable yield maneuver. +| Name | Unit | Type | Description | Default value | +| :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | +| enable_bound_clipping | [-] | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | +| use_adjacent_lane | [-] | bool | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane. | true | +| use_opposite_lane | [-] | bool | Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects | true | +| use_intersection_areas | [-] | bool | Extend drivable to intersection area. | false | +| use_hatched_road_markings | [-] | bool | Extend drivable to hatched road marking area. | false | + +| Name | Unit | Type | Description | Default value | +| :------------------ | ---- | ---- | --------------------------------------------------------------------------------------- | ------------- | +| output_debug_marker | [-] | bool | Flag to publish debug marker (set `false` as default since it takes considerable cost). | false | +| output_debug_info | [-] | bool | Flag to print debug info (set `false` as default since it takes considerable cost). | false | + ### Avoidance target filtering parameters namespace: `avoidance.target_object.` @@ -772,7 +812,7 @@ Developers can see what is going on in each process by visualizing all the avoid ![fig1](../image/avoidance/avoidance-debug-marker.png) -To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. +To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.output_debug_marker true` (no restart is needed) or simply set the `output_debug_marker` to `true` in the `avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. ### Echoing debug message to find out why the objects were ignored diff --git a/planning/behavior_path_planner/image/avoidance/use_adjacent_lane.svg b/planning/behavior_path_planner/image/avoidance/use_adjacent_lane.svg new file mode 100644 index 0000000000000..fb83c668aac35 --- /dev/null +++ b/planning/behavior_path_planner/image/avoidance/use_adjacent_lane.svg @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ Adjacent lane +
+
+
+ + Adjacent lane + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/avoidance/use_hatched_road_markings.svg b/planning/behavior_path_planner/image/avoidance/use_hatched_road_markings.svg new file mode 100644 index 0000000000000..0bbf48e9b56c4 --- /dev/null +++ b/planning/behavior_path_planner/image/avoidance/use_hatched_road_markings.svg @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ Hatched road marking +
+
+
+ + Hatched road marking + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/avoidance/use_intersection_areas.svg b/planning/behavior_path_planner/image/avoidance/use_intersection_areas.svg new file mode 100644 index 0000000000000..43391c5f85c5d --- /dev/null +++ b/planning/behavior_path_planner/image/avoidance/use_intersection_areas.svg @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Intersection area +
+
+
+ + Intersection area + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/avoidance/use_opposite_lane.svg b/planning/behavior_path_planner/image/avoidance/use_opposite_lane.svg new file mode 100644 index 0000000000000..a7ef53443e103 --- /dev/null +++ b/planning/behavior_path_planner/image/avoidance/use_opposite_lane.svg @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ Opposite lane +
+
+
+ + Opposite lane + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 4ee08ca82e276..744dca120e191 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -81,11 +81,11 @@ struct AvoidanceParameters double detection_area_left_expand_dist = 1.0; // enable avoidance to be perform only in lane with same direction - bool enable_avoidance_over_same_direction{true}; + bool use_adjacent_lane{true}; // enable avoidance to be perform in opposite lane direction // to use this, enable_avoidance_over_same_direction need to be set to true. - bool enable_avoidance_over_opposite_direction{true}; + bool use_opposite_lane{true}; // enable update path when if detected objects on planner data is gone. bool enable_update_path_when_object_is_gone{false}; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index b2a3f8dcaafd4..a7ab4a4cac5cd 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2261,7 +2261,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output const auto & route_handler = planner_data_->route_handler; const auto & current_lanes = avoidance_data_.current_lanelets; - const auto & enable_opposite = parameters_->enable_avoidance_over_opposite_direction; + const auto & enable_opposite = parameters_->use_opposite_lane; std::vector drivable_lanes; for (const auto & current_lane : current_lanes) { @@ -2269,7 +2269,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output current_drivable_lanes.left_lane = current_lane; current_drivable_lanes.right_lane = current_lane; - if (!parameters_->enable_avoidance_over_same_direction) { + if (!parameters_->use_adjacent_lane) { drivable_lanes.push_back(current_drivable_lanes); continue; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 8156a39059f68..75e03106795e5 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -55,10 +55,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.detection_area_left_expand_dist = get_parameter(node, ns + "detection_area_left_expand_dist"); p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); - p.enable_avoidance_over_same_direction = - get_parameter(node, ns + "enable_avoidance_over_same_direction"); - p.enable_avoidance_over_opposite_direction = - get_parameter(node, ns + "enable_avoidance_over_opposite_direction"); p.enable_update_path_when_object_is_gone = get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = @@ -68,12 +64,19 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.enable_yield_maneuver_during_shifting = get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); p.disable_path_update = get_parameter(node, ns + "disable_path_update"); - p.use_hatched_road_markings = get_parameter(node, ns + "use_hatched_road_markings"); - p.use_intersection_areas = get_parameter(node, ns + "use_intersection_areas"); p.publish_debug_marker = get_parameter(node, ns + "publish_debug_marker"); p.print_debug_info = get_parameter(node, ns + "print_debug_info"); } + // drivable area + { + std::string ns = "avoidance."; + p.use_adjacent_lane = get_parameter(node, ns + "use_adjacent_lane"); + p.use_opposite_lane = get_parameter(node, ns + "use_opposite_lane"); + p.use_intersection_areas = get_parameter(node, ns + "use_intersection_areas"); + p.use_hatched_road_markings = get_parameter(node, ns + "use_hatched_road_markings"); + } + // target object { const auto get_object_param = [&](std::string && ns) { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index f336d1325b688..93fe8a3ef3b20 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -200,10 +200,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( get_parameter(node, ns + "detection_area_right_expand_dist"); p.detection_area_left_expand_dist = get_parameter(node, ns + "detection_area_left_expand_dist"); - p.enable_avoidance_over_same_direction = - get_parameter(node, ns + "enable_avoidance_over_same_direction"); - p.enable_avoidance_over_opposite_direction = - get_parameter(node, ns + "enable_avoidance_over_opposite_direction"); p.enable_update_path_when_object_is_gone = get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index b2fa6a1f99216..ce51062a959f9 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -877,9 +877,9 @@ void filterTargetObjects( lanelet::BasicPoint3d overhang_basic_pose( o.overhang_pose.position.x, o.overhang_pose.position.y, o.overhang_pose.position.z); - const bool get_left = isOnRight(o) && parameters->enable_avoidance_over_same_direction; - const bool get_right = !isOnRight(o) && parameters->enable_avoidance_over_same_direction; - const bool get_opposite = parameters->enable_avoidance_over_opposite_direction; + const bool get_left = isOnRight(o) && parameters->use_adjacent_lane; + const bool get_right = !isOnRight(o) && parameters->use_adjacent_lane; + const bool get_opposite = parameters->use_opposite_lane; lanelet::ConstLineString3d target_line{}; o.to_road_shoulder_distance = std::numeric_limits::max();