diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index bffa5201f2882..65ca910369cdd 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -4,6 +4,11 @@ The Lane Change module is activated when lane change is needed and can be safely ## Lane Change Requirement +- As the prerequisite, the type of lane boundary in the HD map has to be one of the following: + - Dashed lane marking: Lane changes are permitted in both directions. + - Dashed marking on the left and solid on the right: Lane changes are allowed from left to right. + - Dashed marking on the right and solid on the left: Lane changes are allowed from right to left. + - `allow_lane_change` tags is set as `true` - During lane change request condition - The ego-vehicle isn’t on a `preferred_lane`. - There is neither intersection nor crosswalk on the path of the lane change @@ -60,6 +65,89 @@ longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - mini Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, longitudinal acceleration becomes positive value (not decelerate). +The chart illustrates the conditions under which longitudinal acceleration values are sampled. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +if (prev_module_path is empty?) then (yes) + :Return empty list; + stop +else (no) +endif + +if (max_acc <= 0.0) then (yes) + :Return **sampled acceleration values**; + note left: Calculated sampled acceleration using\ngetAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num) + stop +endif + +if (max_lane_change_length > ego's distance to the end of the current lanes.) then (yes) + :Return **sampled acceleration values**; + stop +endif + +if (isVehicleStuck(current_lanes)) then (yes) + :Return **sampled acceleration values**; + stop +else (no) +endif + +if (is goal is in target lanes) then (yes) + if (max_lane_change_length < ego's distance to the goal along the target lanes) then (yes) + :Return {max_acc}; + stop + else (no) + endif +else (no) + if (max_lane_change_length < ego's distance to the end of the target lanes.) then (yes) + :Return {max_acc}; + stop + else (no) + endif +endif + +:Return **sampled acceleration values**; +stop + +@enduml + +``` + +while the following describes the process by which longitudinal accelerations are sampled. + +```plantuml +@startuml +start +:Initialize sampled_values with min_acc; + +if (min_acc > max_acc) then (yes) + :Return empty list; + stop +elseif (max_acc - min_acc < epsilon) then (yes) + :Return {0.0}; + stop +else (no) + :Calculate resolution; +endif + +:Start loop from min_acc to max_acc with resolution step; +repeat + if (sampled_values.back() < -epsilon AND next_value > epsilon) then (yes) + :Insert 0.0 into sampled_values; + endif + :Add sampled_acc to sampled_values; + repeat while (sampled_acc < max_acc + epsilon) is (TRUE) + +:Return sampled_values; +stop +@enduml +``` + The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. ![path_samples](./images/lane_change-candidate_path_samples.png) @@ -89,67 +177,112 @@ lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_latera #### Candidate Path's validity check -A candidate path is valid if the total lane change distance is less than +A candidate path is considered valid if it meets the following criteria: -1. distance to the end of current lane -2. distance to the next intersection -3. distance from current pose to the goal. -4. distance to the crosswalk. - -The goal must also be in the list of the preferred lane. +1. The distance from the ego vehicle's current position to the end of the current lanes is sufficient to perform a single lane change. +2. The distance from the ego vehicle's current position to the goal along the current lanes is adequate to complete multiple lane changes. +3. The distance from the ego vehicle's current position to the end of the target lanes is adequate for completing multiple lane changes. +4. Intersection requirements are met (conditions are parameterized). +5. Crosswalk requirements are satisfied (conditions are parameterized). +6. Traffic light regulations are adhered to (conditions are parameterized). +7. The lane change can be completed after passing a parked vehicle. +8. The lane change is deemed safe to execute. The following flow chart illustrates the validity check. ```plantuml @startuml -skinparam monochrome true skinparam defaultTextAlignment center -skinparam noteTextAlignment left +skinparam backgroundColor #White -title Selecting Valid Candidate Paths start -:**INPUT** std::vector input_paths; +if (Check if start point is valid by check if it is covered by neighbour lanes polygon) then (not covered) + #LightPink:Reject path; + stop +else (covered) +endif -partition selectValidPaths { -:**INITIALIZE** std::vector valid_paths; +group check for distance #LightYellow + :Calculate total length and goal related distances; + if (total lane change length considering single lane change > distance from current pose to end of current lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif -:idx = 0; + if (goal is in current lanes) then (yes) + if (total lane change length considering multiple lane changes > distance from ego to goal along current lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif + else (no) + endif -while (idx < input_paths.size()?) + if (target lanes is empty) then (yes) + #LightPink:Reject path; + stop + else (no) + endif + if (total lane change length considering multiple lane changes > distance from ego to the end of target lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif +end group -:path = input_paths.at(idx); -partition hasEnoughDistance { -if(lane_change_total_distance < distance to end of current lanes -&& -lane_change_total_distance < distance to the next intersection -&& -lane_change_total_distance < distance from current pose to the goal -&& -lane_change_total_distance < distance to crosswalk -&& -goal is in route -) then (true) -:path_validity = true; -else (\n false) -:path_validity = false; +group evaluate on Crosswalk #LightCyan +if (regulate_on_crosswalk and not enough length to crosswalk) then (yes) + if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) + #LightPink:Reject path; + stop + else (no) + :Allow lane change in crosswalk; + endif +else (no) endif -} - -if(path_validity == true)then (true) - -:valid_paths.push_back(path); +end group + +group evaluate on Intersection #LightGreen +if (regulate_on_intersection and not enough length to intersection) then (yes) + if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) + #LightPink:Reject path; + stop + else (no) + :Allow lane change in intersection; + endif +else (no) +endif +end group + +group evaluate on Traffic Light #Lavender +if (regulate_on_traffic_light and not enough length to complete lane change before stop line) then (yes) + #LightPink:Reject path; + stop +elseif (stopped at red traffic light within distance) then (yes) + #LightPink:Reject path; + stop +else (no) +endif +end group -else (\nfalse) +if (ego is not stuck but parked vehicle exists in target lane) then (yes) + #LightPink:Reject path; + stop +else (no) endif -:++idx; -endwhile (false) -:**RETURN** valid_paths; +if (is safe to perform lane change) then (yes) + #Cyan:Return candidate path list; + stop +else (no) + #LightPink:Reject path; +endif -} stop + @enduml ``` @@ -165,6 +298,27 @@ First, we divide the target objects into obstacles in the target lane, obstacles Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). +The detection area for the target lane can be expanded beyond its original boundaries to enable detection of objects that are outside the target lane's limits. + +
+ + + + + +
+
+
Without Lane Expansion
+ Without lane expansion +
+ 		+
+
With Lane Expansion
+ With lane expansion +
+
+
+ ##### Collision check in prepare phase The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. @@ -310,45 +464,38 @@ The last behavior will also occur if the ego vehicle has departed from the curre ### Essential lane change parameters -The following parameters are configurable in `lane_change.param.yaml`. - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------- | ------ | ------- | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | -| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | -| `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | -| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | -| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | -| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | -| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.15, 0.15, 0.15] | -| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.5, 0.5, 0.5] | -| `target_object.car` | [-] | boolean | Include car objects for safety check | true | -| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | -| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | -| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | -| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | -| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | -| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | -| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | +The following parameters are configurable in [lane_change.param.yaml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml) + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | +| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | +| `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | +| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | +| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | +| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | +| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | +| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | +| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | +| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | ### Lane change regulations -| Name | Unit | Type | Description | Default value | -| :------------------------ | ---- | ------- | ------------------------------------- | ------------- | -| `regulation.crosswalk` | [-] | boolean | Regulate lane change on crosswalks | false | -| `regulation.intersection` | [-] | boolean | Regulate lane change on intersections | false | +| Name | Unit | Type | Description | Default value | +| :------------------------- | ---- | ------- | ---------------------------------------------------------- | ------------- | +| `regulation.crosswalk` | [-] | boolean | Allow lane change in between crosswalks | true | +| `regulation.intersection` | [-] | boolean | Allow lane change in between intersections | true | +| `regulation.traffic_light` | [-] | boolean | Allow lane change to be performed in between traffic light | true | ### Ego vehicle stuck detection @@ -357,57 +504,67 @@ The following parameters are configurable in `lane_change.param.yaml`. | `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | | `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | -### Collision checks during lane change +### Collision checks + +#### Target Objects -The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. +| Name | Unit | Type | Description | Default value | +| :------------------------- | ---- | ------- | ------------------------------------------- | ------------- | +| `target_object.car` | [-] | boolean | Include car objects for safety check | true | +| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | +| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | +| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | +| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | +| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | +| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | +| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | #### execution -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | -| `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.execution.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | -| `safety_check.execution.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | -| `safety_check.execution.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | -| `safety_check.execution.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | -| `safety_check.execution.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `safety_check.execution.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | -| `safety_check.execution.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | -| `safety_check.execution.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | - -##### cancel - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.5 | -| `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.cancel.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.5 | -| `safety_check.cancel.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.5 | -| `safety_check.cancel.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `safety_check.cancel.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.5 | -| `safety_check.cancel.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | false | -| `safety_check.cancel.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.2 | -| `safety_check.cancel.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | false | -| `safety_check.cancel.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | false | -| `safety_check.cancel.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | false | - -##### stuck - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------------------------ | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | -| `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.stuck.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | -| `safety_check.stuck.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | -| `safety_check.stuck.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `safety_check.stuck.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | -| `safety_check.stuck.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | -| `safety_check.stuck.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `safety_check.stuck.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | -| `safety_check.stuck.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | -| `safety_check.stuck.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `enable_collision_check_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | +| `enable_collision_check_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | false | +| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | +| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | +| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | + +#### safety constraints during lane change path is computed + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.execution.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | +| `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | + +##### safety constraints to cancel lane change path + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.cancel.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.cancel.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.0 | +| `safety_check.cancel.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 1.5 | +| `safety_check.cancel.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 0.8 | +| `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | +| `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 2.5 | +| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.6 | + +##### safety constraints used during lane change path is computed when ego is stuck + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.stuck.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.stuck.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | +| `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.stuck.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | (\*1) the value must be negative. diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml index 0cb7f3f1a7a92..e848aecf258cf 100644 --- a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml @@ -89,7 +89,9 @@ stop_time: 3.0 # [s] # collision check - enable_prepare_segment_collision_check: true + enable_collision_check_for_prepare_phase: + general_lanes: false + intersection: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] check_objects_on_current_lanes: true check_objects_on_other_lanes: true diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png index 411a72c30b88a..efd1df086544d 100644 Binary files a/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png and b/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png differ diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png new file mode 100644 index 0000000000000..1793eb8f6e691 Binary files /dev/null and b/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png differ diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png new file mode 100644 index 0000000000000..6b440d862c7ac Binary files /dev/null and b/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png differ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index e4af48a71c8f3..a8746b0e7de12 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -172,6 +172,8 @@ class NormalLaneChange : public LaneChangeBase bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool check_prepare_phase() const; + double calcMaximumLaneChangeLength( const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index dd7760d31eaa7..5b8e0b6439276 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -107,7 +107,8 @@ struct LaneChangeParameters double max_longitudinal_acc{1.0}; // collision check - bool enable_prepare_segment_collision_check{true}; + bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; + bool enable_collision_check_for_prepare_phase_in_intersection{true}; double prepare_segment_ignore_object_velocity_thresh{0.1}; bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index e16afcdbf19ec..dc8dba1b8a61b 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -23,6 +23,7 @@ #include "rclcpp/logger.hpp" #include +#include #include #include @@ -31,6 +32,7 @@ #include +#include #include #include @@ -189,7 +191,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters); + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); bool isCollidedPolygonsInLanelet( const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); @@ -224,5 +226,57 @@ lanelet::ConstLanelets generateExpandedLanelets( * @return rclcpp::Logger The logger instance configured for the specified lane change type. */ rclcpp::Logger getLogger(const std::string & type); + +/** + * @brief Computes the current footprint of the ego vehicle based on its pose and size. + * + * This function calculates the 2D polygon representing the current footprint of the ego vehicle. + * The footprint is determined by the vehicle's pose and its dimensions, including the distance + * from the base to the front and rear ends of the vehicle, as well as its width. + * + * @param ego_pose The current pose of the ego vehicle. + * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal + * offset, rear overhang, and width. + * + * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. + */ +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info); + +/** + * @brief Checks if the given polygon is within an intersection area. + * + * This function evaluates whether a specified polygon is located within the bounds of an + * intersection. It identifies the intersection area by checking the attributes of the provided + * lanelet. If the lanelet has an attribute indicating it is part of an intersection, the function + * then checks if the polygon is fully contained within this area. + * + * @param route_handler a shared pointer to the route_handler + * @param lanelet A lanelet to check against the + * intersection area. + * @param polygon The polygon to check for containment within the intersection area. + * + * @return bool True if the polygon is within the intersection area, false otherwise. + */ +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double time); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index 0a56c17d89fd0..3740a223d6bae 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -68,8 +68,10 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); // collision check - p.enable_prepare_segment_collision_check = - getOrDeclareParameter(*node, parameter("enable_prepare_segment_collision_check")); + p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.general_lanes")); + p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.intersection")); p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter( *node, parameter("prepare_segment_ignore_object_velocity_thresh")); p.check_objects_on_current_lanes = diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 36c782de9c89b..28a3b75334214 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -870,23 +870,27 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( target_objects.other_lane.reserve(target_obj_index.other_lane.size()); // objects in current lane + const auto is_check_prepare_phase = check_prepare_phase(); for (const auto & obj_idx : target_obj_index.current_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.current_lane.push_back(extended_object); } // objects in target lane for (const auto & obj_idx : target_obj_index.target_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.target_lane.push_back(extended_object); } // objects in other lane for (const auto & obj_idx : target_obj_index.other_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.other_lane.push_back(extended_object); } @@ -950,8 +954,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); - const auto extended_object = - utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + const auto extended_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, check_prepare_phase()); const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); @@ -1270,9 +1274,8 @@ bool NormalLaneChange::getLaneChangePaths( (prepare_duration < 1e-3) ? 0.0 : ((prepare_velocity - current_velocity) / prepare_duration); - const double prepare_length = - current_velocity * prepare_duration + - 0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2); + const auto prepare_length = utils::lane_change::calcPhaseLength( + current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); @@ -1324,9 +1327,9 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::calcLaneChangingAcceleration( initial_lane_changing_velocity, max_path_velocity, lane_changing_time, sampled_longitudinal_acc); - const auto lane_changing_length = - initial_lane_changing_velocity * lane_changing_time + - 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; + const auto lane_changing_length = utils::lane_change::calcPhaseLength( + initial_lane_changing_velocity, getCommonParam().max_vel, + longitudinal_acc_on_lane_changing, lane_changing_time); const auto terminal_lane_changing_velocity = std::min( initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, getCommonParam().max_vel); @@ -2118,4 +2121,33 @@ void NormalLaneChange::updateStopTime() stop_watch_.tic("stop_time"); } +bool NormalLaneChange::check_prepare_phase() const +{ + const auto & route_handler = getRouteHandler(); + const auto & vehicle_info = getCommonParam().vehicle_info; + + const auto check_in_general_lanes = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_DEBUG( + logger_, "Unable to get current lane. Default to %s.", + (check_in_general_lanes ? "true" : "false")); + return check_in_general_lanes; + } + + const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info); + + const auto check_in_intersection = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) { + return false; + } + + return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint); + }); + + return check_in_intersection || check_in_general_lanes; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index a331d418d5e46..3913de61d6532 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -31,7 +31,11 @@ #include #include #include +#include #include +#include + +#include #include #include @@ -435,7 +439,8 @@ PathWithLaneId getReferencePathFromTargetLane( .get_child("getReferencePathFromTargetLane"), "start: %f, end: %f", s_start, s_end); - if (s_end - s_start < lane_changing_length) { + constexpr double epsilon = 1e-4; + if (s_end - s_start + epsilon < lane_changing_length) { return PathWithLaneId(); } @@ -1123,7 +1128,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters) + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase) { ExtendedPredictedObject extended_object; extended_object.uuid = object.object_id; @@ -1133,8 +1138,6 @@ ExtendedPredictedObject transform( extended_object.shape = object.shape; const auto & time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & check_at_prepare_phase = - lane_change_parameters.enable_prepare_segment_collision_check; const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; @@ -1192,4 +1195,40 @@ rclcpp::Logger getLogger(const std::string & type) { return rclcpp::get_logger("lane_change").get_child(type); } + +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info) +{ + const auto base_to_front = ego_info.max_longitudinal_offset_m; + const auto base_to_rear = ego_info.rear_overhang_m; + const auto width = ego_info.vehicle_width_m; + + return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); +} + +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon) +{ + const std::string id = lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + return false; + } + + const auto lanelet_polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + return boost::geometry::within( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); +} + +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} } // namespace behavior_path_planner::utils::lane_change