diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index d0e4a49605dd9..d6bf2067c9cb7 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -54,28 +54,6 @@ The following modules are currently supported: ## Inner-workings / Algorithms -### Drivable Area Generation - -Drivable lanes are quantized and drawn on an image as a drivable area, whose resolution is `drivable_area_resolution`. -To prevent the quantization from causing instability to the planning modules, drivable area's pose follows the rules below. - -- Drivable area is generated in the map coordinate. -- Its position is quantized with `drivable_area_resolution`. -- Its orientation is 0. - -The size of the drivable area changes dynamically to realize both decreasing the computation cost and covering enough lanes to follow. -For the second purpose, the drivable area covers a certain length forward and backward lanes with some margins defined by parameters. - -#### Parameters for drivable area generation - -| Name | Unit | Type | Description | Default value | -| :---------------------------- | :--- | :----- | :------------------------------------------------------------------------- | :------------ | -| drivable_area_resolution | [m] | double | resolution of the image of the drivable area | 0.1 | -| drivable_lane_forward_length | [m] | double | length of the forward lane from the ego covered by the drivable area | 50.0 | -| drivable_lane_backward_length | [m] | double | length of the backward lane from the ego covered by the drivable area | 5.0 | -| drivable_lane_margin | [m] | double | forward and backward lane margin from the ego covered by the drivable area | 3.0 | -| drivable_area_margin | [m] | double | margin of width and height of the drivable area | 6.0 | - ### Behavior Tree In the behavior path planner, the behavior tree mechanism is used to manage which modules are activated in which situations. In general, this "behavior manager" like function is expected to become bigger as more and more modules are added in the future. To improve maintainability, we adopted the behavior tree. The behavior tree has the following advantages: easy visualization, easy configuration management (behaviors can be changed by replacing configuration files), and high scalability compared to the state machine. diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index cefc482be4896..821190b62956b 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -422,6 +422,7 @@ The avoidance specific parameter configuration file can be located at `src/autow | 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_bound_clipping | `true` | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | (\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value. diff --git a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md new file mode 100644 index 0000000000000..88dcf30299a59 --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md @@ -0,0 +1,78 @@ +# Drivable Area + +Drivable Area represents the area where ego vehicle can pass. + +## Purpose / Role + +In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are `left_bound` line and `right_bound` line respectively. Both `left_bound` and `right_bound` are created from left and right boundaries of lanelets. Note that `left_bound` and `right bound` are generated by `generateDrivableArea` function. + +## Assumption + +Our drivable area has several assumptions. + +- Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in `follow lane` mode, drivable area should not contain adjacent lanes. + +- When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections). + +- Both left and right bounds should cover the front of the path and the end of the path. + +## Limitations + +Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative. + +## Parameters for drivable area generation + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | +| drivable_area_right_bound_offset | [m] | double | right offset length to expand drivable area | 5.0 | +| drivable_area_left_bound_offset | [m] | double | left offset length to expand drivable area | 5.0 | + +Note that default values can be varied by the module. Please refer to the `config/drivable_area_expansion.yaml` file. + +## Inner-workings / Algorithms + +This section gives details of the generation of the drivable area (`left_bound` and `right_bound`). + +### Drivable Lanes Generation + +Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Pull Over`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. + +```plantuml +struct DrivalbleLanes +{ + lanelet::ConstLanelet right_lanelet; // right most lane + lanelet::ConstLanelet left_lanelet; // left most lane + lanelet::ConstLanelets middle_lanelets; // middle lanes +}; +``` + +The image of the sorted drivable lanes is depicted in the following picture. + +![sorted_lanes](./image/drivable_area/sorted_lanes.drawio.svg) + +Note that, the order of drivable lanes become + +```plantuml +drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5} +``` + +### Drivable Area Expansion + +Each module can expand the drivable area based on parameters. It expands right bound and left bound of target lanes. This enables large vehicles to pass narrow curve. The image of this process can be described as + +![expanded_lanes](./image/drivable_area/expanded_lanes.drawio.svg) + +Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane. + +### Drivable Area Generation + +In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created `left_bound` from left boundary of the leftmost lanelet and `right_bound` from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the `Path` and `PathWithLaneId` messages as + +```plantuml +std::vector left_bound; +std::vector right_bound; +``` + +and each point of right bound and left bound has a position in the absolute coordinate system. + +![drivable_lines](./image/drivable_area/drivable_lines.drawio.svg) diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg b/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg new file mode 100644 index 0000000000000..fd1729013c1ba --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg @@ -0,0 +1,241 @@ + + + + + + + + + + +
+
+
+ Lane 3 +
+
+
+
+ Lane 3 +
+
+ + + + +
+
+
+ Lane 7 +
+
+
+
+ Lane 7 +
+
+ + + + +
+
+
+ Lane 2 +
+
+
+
+ Lane 2 +
+
+ + + + +
+
+
+ Lane 6 +
+
+
+
+ Lane 6 +
+
+ + + + + +
+
+
+ Lane 4 +
+
+
+
+ Lane 4 +
+
+ + + + +
+
+
+ Lane 8 +
+
+
+
+ Lane 8 +
+
+ + + + +
+
+
+ Lane 10 +
+
+
+
+ Lane 10 +
+
+ + + + +
+
+
+ Lane 9 +
+
+
+
+ Lane 9 +
+
+ + + + +
+
+
+ Lane 11 +
+
+
+
+ Lane 11 +
+
+ + + + +
+
+
+ + Lane 5 + +
+
+
+
+ Lane 5 +
+
+ + + + + + + + +
+
+
+ Lane 1 +
+
+
+
+ Lane 1 +
+
+ + + + + + + + + + + + + + +
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg b/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg new file mode 100644 index 0000000000000..c1bd0ccdd0e09 --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg @@ -0,0 +1,115 @@ + + + + + + + + + + +
+
+
+ + Lane1 +
+
+
+
+
+
+ Lane1 +
+
+ + + + + + + +
+
+
+ expanded lanes +
+
+
+
+ expanded lanes +
+
+ + + + + + + + +
+
+
+ + Lane2 +
+
+
+
+
+
+ Lane2 +
+
+ + + + +
+
+
+ + Lane3 +
+
+
+
+
+
+ Lane3 +
+
+ + + + + + + +
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg b/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg new file mode 100644 index 0000000000000..3c6bc512ff328 --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg @@ -0,0 +1,409 @@ + + + + + + + + + + +
+
+
+ Lane 3 +
+
+
+
+ Lane 3 +
+
+ + + + +
+
+
+ Lane 7 +
+
+
+
+ Lane 7 +
+
+ + + + +
+
+
+ Lane 2 +
+
+
+
+ Lane 2 +
+
+ + + + +
+
+
+ Lane 6 +
+
+
+
+ Lane 6 +
+
+ + + + + + + +
+
+
+ Lane 4 +
+
+
+
+ Lane 4 +
+
+ + + + +
+
+
+ Lane 8 +
+
+
+
+ Lane 8 +
+
+ + + + +
+
+
+ Lane 10 +
+
+
+
+ Lane 10 +
+
+ + + + +
+
+
+ Lane 9 +
+
+
+
+ Lane 9 +
+
+ + + + +
+
+
+ Lane 11 +
+
+
+
+ Lane 11 +
+
+ + + + +
+
+
+ + Lane 5 + +
+
+
+
+ Lane 5 +
+
+ + + + + + + + +
+
+
+ Lane 1 +
+
+
+
+ Lane 1 +
+
+ + + + +
+
+
+
+ DrivableLanes1 +
+
+ { +
+
+ right_lane = 1 +
+
+ left_lane = 1 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+
+ DrivableLanes1... +
+
+ + + + +
+
+
+
+ DrivableLanes2 +
+
+ { +
+
+ right_lane = 4 +
+
+ left_lane = 2 +
+
+ middle_lanes = {3} +
+
+ } +
+
+
+
+
+ DrivableLanes2... +
+
+ + + + + + + + +
+
+
+
+ DrivableLanes3 +
+
+ { +
+
+ right_lane = 8 +
+
+ left_lane = 5 +
+
+ middle_lanes = {6,7} +
+
+ } +
+
+
+
+
+ DrivableLanes3... +
+
+ + + + + + +
+
+
+
+ DrivableLanes4 +
+
+ { +
+
+ right_lane = 10 +
+
+ left_lane = 9 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+
+ DrivableLanes4... +
+
+ + + + +
+
+
+
+ DrivableLanes5 +
+
+ { +
+
+ right_lane = 11 +
+
+ left_lane = 11 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+
+ DrivableLanes5... +
+
+ + +
+ + + + Text is not SVG - cannot display + + +