From c4608ad6c3ad189e28a16cea4dde9bb58f85ebd0 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Thu, 28 Nov 2024 11:29:59 +0900 Subject: [PATCH] feat(lane_change): improve delay lane change logic (#9480) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * implement function to check if lane change delay is required Signed-off-by: mohammad alqudah * refactor function isParkedObject Signed-off-by: mohammad alqudah * refactor delay lane change parameters Signed-off-by: mohammad alqudah * update lc param yaml Signed-off-by: mohammad alqudah * separate target lane leading objects based on behavior (RT1-8532) Signed-off-by: Zulfaqar Azmi * fixed overlapped filtering and lanes debug marker Signed-off-by: Zulfaqar Azmi * combine filteredObjects function Signed-off-by: Zulfaqar Azmi * renaming functions and type Signed-off-by: Zulfaqar Azmi * update some logic to check is stopped Signed-off-by: Zulfaqar Azmi * rename expanded to stopped_outside_boundary Signed-off-by: Zulfaqar Azmi * Include docstring Signed-off-by: Zulfaqar Azmi * rename stopped_outside_boundary → stopped_at_bound Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi * spell-check Signed-off-by: Zulfaqar Azmi * add docstring for function is_delay_lane_change Signed-off-by: mohammad alqudah * remove unused functions Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * add delay parameters to README Signed-off-by: mohammad alqudah * add documentation for delay lane change behavior Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * run pre-commit checks Signed-off-by: mohammad alqudah * only check for delay lc if feature is enabled Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Signed-off-by: Zulfaqar Azmi Co-authored-by: Zulfaqar Azmi Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../README.md | 76 +- .../config/lane_change.param.yaml | 11 +- .../images/delay_lane_change_1.drawio.svg | 657 +++++++++++++++++ .../images/delay_lane_change_2.drawio.svg | 625 ++++++++++++++++ .../images/delay_lane_change_3.drawio.svg | 683 ++++++++++++++++++ .../images/delay_lane_change_4.drawio.svg | 683 ++++++++++++++++++ .../images/delay_lane_change_5.drawio.svg | 677 +++++++++++++++++ .../utils/parameters.hpp | 9 + .../utils/utils.hpp | 29 +- .../src/manager.cpp | 15 +- .../src/scene.cpp | 8 +- .../src/utils/utils.cpp | 198 ++--- 12 files changed, 3495 insertions(+), 176 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 6ebe79c180e95..d24a89bf03000 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -351,6 +351,71 @@ stop @enduml ``` +#### Delay Lane Change Check + +In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle. +To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected. + +1. The distance from object to terminal end is sufficient to perform lane change +2. The distance to object is less than the lane changing length +3. The distance from object to next object is sufficient to perform lane change + +If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked. + +The following flow chart illustrates the delay lane change check. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #White + +start +if (Is target objects, candidate path, OR current lane path empty?) then (yes) + #LightPink:Return false; + stop +else (no) +endif + +:Start checking objects from closest to furthest; +repeat + if (Is distance from object to terminal sufficient) then (yes) + else (no) + #LightPink:Return false; + stop + endif + + if (Is distance to object less than lane changing length) then (yes) + else (no) + if (Is only check parked vehicles and vehicle is not parked) then (yes) + else (no) + if(Is last object OR distance to next object is sufficient) then (yes) + #LightGreen: Return true; + stop + else (no) + endif + endif + endif + repeat while (Is finished checking all objects) is (FALSE) + +#LightPink: Return false; +stop + +@enduml +``` + +The following figures demonstrate different situations under which will or will not be triggered: + +1. Delay lane change will be triggered as there is sufficient distance ahead. + ![delay lane change 1](./images/delay_lane_change_1.drawio.svg) +2. Delay lane change will NOT be triggered as there is no sufficient distance ahead + ![delay lane change 2](./images/delay_lane_change_2.drawio.svg) +3. Delay lane change will be triggered by fist NPC as there is sufficient distance ahead. + ![delay lane change 3](./images/delay_lane_change_3.drawio.svg) +4. Delay lane change will be triggered by second NPC as there is sufficient distance ahead + ![delay lane change 4](./images/delay_lane_change_4.drawio.svg) +5. Delay lane change will NOT be triggered as there is no sufficient distance ahead. + ![delay lane change 5](./images/delay_lane_change_5.drawio.svg) + #### Candidate Path's Safety check See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) @@ -828,8 +893,6 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | | `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | | `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | -| `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 | | `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/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | @@ -860,6 +923,15 @@ The following parameters are used to judge lane change completion. | `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 | +### Delay Lane Change + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------ | ---- | ------ | ----------------------------------------------------------------------------------------------------- | ------------- | +| `delay_lane_change.enable` | [-] | bool | Flag to enable/disable lane change delay feature | true | +| `delay_lane_change.check_only_parked_vehicle` | [-] | bool | Flag to limit delay feature for only parked vehicles | false | +| `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | +| `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | + ### Collision checks #### Target Objects diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 6ac4544f2b342..6baf05edad484 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -6,10 +6,6 @@ backward_length_buffer_for_blocking_object: 3.0 # [m] backward_length_from_intersection: 5.0 # [m] - # side walk parked vehicle - object_check_min_road_shoulder_width: 0.5 # [m] - object_shiftable_ratio_threshold: 0.6 - # turn signal min_length_for_turn_signal_activation: 10.0 # [m] @@ -25,6 +21,13 @@ lon_acc_sampling_num: 5 lat_acc_sampling_num: 3 + # delay lane change + delay_lane_change: + enable: true + check_only_parked_vehicle: false + min_road_shoulder_width: 0.5 # [m] + th_parked_vehicle_shift_ratio: 0.6 + # safety check safety_check: allow_loose_check_for_cancel: true diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg new file mode 100644 index 0000000000000..f5f3e4e88559b --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg @@ -0,0 +1,657 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg new file mode 100644 index 0000000000000..8237ac312aadb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg @@ -0,0 +1,625 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg new file mode 100644 index 0000000000000..2bcbfb5cdb93d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg new file mode 100644 index 0000000000000..b38092183db14 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg @@ -0,0 +1,683 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg new file mode 100644 index 0000000000000..d4abb53a84999 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg @@ -0,0 +1,677 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp index d688782ed8d8d..7f97eb872f795 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp @@ -127,11 +127,20 @@ struct TrajectoryParameters LateralAccelerationMap lat_acc_map{}; }; +struct DelayParameters +{ + bool enable{true}; + bool check_only_parked_vehicle{false}; + double min_road_shoulder_width{0.5}; + double th_parked_vehicle_shift_ratio{0.6}; +}; + struct Parameters { TrajectoryParameters trajectory{}; SafetyParameters safety{}; CancelParameters cancel{}; + DelayParameters delay{}; // lane change parameters double backward_lane_length{200.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index a02a61d5e72b6..2ec9d4cdf4e4e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -132,14 +132,29 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); -bool passed_parked_objects( +/** + * @brief Checks if delaying of lane change maneuver is necessary + * + * @details Scans through the provided target objects (assumed to be ordered from closest to + * furthest), and returns true if any of the objects satisfy the following conditions: + * - Not near the end of current lanes + * - There is sufficient distance from object to next one to do lane change + * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects + * which pass isParkedObject() check will be considered. + * + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, + * and transient data. + * @param lane_change_path Candidate lane change path to apply checks on. + * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include + * target lane leading static objects). + * @param object_debug Collision check debug struct to be updated if any of the target objects + * satisfy the conditions. + * @return bool True if conditions to delay lane change are met + */ +bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug); - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); + const std::vector & target_objects, + CollisionCheckDebugMap & object_debug); lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 2624d0f1a87c6..fc419f149e2b9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -99,12 +99,6 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } } - // parked vehicle detection - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); - p.th_object_shiftable_ratio = - getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); - // turn signal p.min_length_for_turn_signal_activation = getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); @@ -203,6 +197,15 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s "Lane change buffer must be more than 1 meter. Modifying the buffer."); } + // lane change delay + p.delay.enable = getOrDeclareParameter(*node, parameter("delay_lane_change.enable")); + p.delay.check_only_parked_vehicle = + getOrDeclareParameter(*node, parameter("delay_lane_change.check_only_parked_vehicle")); + p.delay.min_road_shoulder_width = + getOrDeclareParameter(*node, parameter("delay_lane_change.min_road_shoulder_width")); + p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( + *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); + // lane change cancel p.cancel.enable_on_prepare_phase = getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 2e9b9e337e51c..c6d8248be134c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1341,7 +1341,7 @@ bool NormalLaneChange::check_candidate_path_safety( } if ( - !is_stuck && !utils::lane_change::passed_parked_objects( + !is_stuck && utils::lane_change::is_delay_lane_change( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, lane_change_debug_.collision_check_objects)) { throw std::logic_error( @@ -1522,10 +1522,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {false, true}; } - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data); - - if (!has_passed_parked_objects) { + if (utils::lane_change::is_delay_lane_change( + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data)) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); return {false, false}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index ff8bb971405ad..db0c1cb37abbb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -668,9 +668,6 @@ bool isParkedObject( // +: object position // o: nearest point on centerline - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; - const double object_vel_norm = std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { @@ -689,49 +686,15 @@ bool isParkedObject( const double lat_dist = autoware::motion_utils::calcLateralOffset(path.points, object_pose.position); - lanelet::BasicLineString2d bound; - double center_to_bound_buffer = 0.0; - if (lat_dist > 0.0) { - // left side vehicle - const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet); - const auto most_left_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute most_left_sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - bound = most_left_lanelet.leftBound2d().basicLineString(); - if (most_left_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } else { - // right side vehicle - const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet); - const auto most_right_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages( - most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute most_right_sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - bound = most_right_lanelet.rightBound2d().basicLineString(); - if (most_right_sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } + const auto most_side_lanelet = + lat_dist > 0.0 ? route_handler.getMostLeftLanelet(closest_lanelet, false, true) + : route_handler.getMostRightLanelet(closest_lanelet, false, true); + const auto bound = lat_dist > 0.0 ? most_side_lanelet.leftBound2d().basicLineString() + : most_side_lanelet.rightBound2d().basicLineString(); + const lanelet::Attribute lanelet_sub_type = + most_side_lanelet.attribute(lanelet::AttributeName::Subtype); + const auto center_to_bound_buffer = + lanelet_sub_type.value() == "road_shoulder" ? 0.0 : object_check_min_road_shoulder_width; return isParkedObject( closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold); @@ -776,129 +739,60 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } -bool passed_parked_objects( +bool is_delay_lane_change( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug) + const ExtendedPredictedObjects & target_objects, CollisionCheckDebugMap & object_debug) { - const auto route_handler = *common_data_ptr->route_handler_ptr; - const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; - const auto & object_check_min_road_shoulder_width = - lane_change_parameters.object_check_min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = lane_change_parameters.th_object_shiftable_ratio; const auto & current_lane_path = common_data_ptr->current_lanes_path; + const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay; - if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { - return true; - } - - const auto leading_obj_idx = getLeadingStaticObjectIdx( - route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold); - if (!leading_obj_idx) { - return true; - } - - const auto & leading_obj = objects.at(*leading_obj_idx); - auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); - const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); - if (leading_obj_poly.outer().empty()) { - return true; + if ( + !delay_lc_param.enable || target_objects.empty() || lane_change_path.path.points.empty() || + current_lane_path.points.empty()) { + return false; } - const auto & current_path_end = current_lane_path.points.back().point.pose.position; - const auto dist_to_path_end = [&](const auto & src_point) { - if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { - const auto goal_pose = route_handler.getGoalPose(); - return motion_utils::calcSignedArcLength( - current_lane_path.points, src_point, goal_pose.position); - } - return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); + const auto dist_to_end = common_data_ptr->transient_data.dist_to_terminal_end; + const auto dist_buffer = common_data_ptr->transient_data.current_dist_buffer.min; + auto is_near_end = [&dist_to_end, &dist_buffer](const ExtendedPredictedObject & obj) { + const auto dist_obj_to_end = dist_to_end - obj.dist_from_ego; + return dist_obj_to_end <= dist_buffer; }; - const auto min_dist_to_end_of_current_lane = std::invoke([&]() { - auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); - for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); - const auto dist = dist_to_path_end(obj_p); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - } - return min_dist_to_end_of_current_lane; - }); - - // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { - return true; - } - - const auto current_pose = common_data_ptr->get_ego_pose(); - const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); - - if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { - return true; - } - - debug.second.unsafe_reason = "delay lane change"; - utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return false; -} - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) -{ - const auto & path = lane_change_path.path; - - if (path.points.empty() || objects.empty()) { - return {}; - } - - const auto & lane_change_start = lane_change_path.info.lane_changing_start; - const auto & path_end = path.points.back(); - - double dist_lc_start_to_leading_obj = 0.0; - std::optional leading_obj_idx = std::nullopt; - for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { - const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose; - - // ignore non-static object - // TODO(shimizu): parametrize threshold - const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); - if (obj_vel_norm > 1.0) { - continue; - } + const auto ego_vel = common_data_ptr->get_ego_speed(); + const auto min_lon_acc = common_data_ptr->lc_param_ptr->trajectory.min_longitudinal_acc; + const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc)); + auto is_sufficient_gap = [&gap_threshold](const auto & current_obj, const auto & next_obj) { + const auto curr_obj_half_length = current_obj.shape.dimensions.x; + const auto next_obj_half_length = next_obj.shape.dimensions.x; + const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego; + const auto gap_length = dist_current_to_next - curr_obj_half_length - next_obj_half_length; + return gap_length > gap_threshold; + }; - // ignore non-parked object - if (!isParkedObject( - path, route_handler, obj, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold)) { - continue; - } + for (auto it = target_objects.begin(); it < target_objects.end(); ++it) { + if (is_near_end(*it)) break; - const double dist_back_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, path_end.point.pose.position, obj_pose.position); - if (dist_back_to_obj > 0.0) { - // object is not on the lane change path - continue; - } + if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue; - const double dist_lc_start_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, lane_change_start.position, obj_pose.position); - if (dist_lc_start_to_obj < 0.0) { - // object is on the lane changing path or behind it. It will be detected in safety check + if ( + delay_lc_param.check_only_parked_vehicle && + !isParkedObject( + lane_change_path.path, *common_data_ptr->route_handler_ptr, *it, + delay_lc_param.min_road_shoulder_width, delay_lc_param.th_parked_vehicle_shift_ratio)) { continue; } - if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) { - dist_lc_start_to_leading_obj = dist_lc_start_to_obj; - leading_obj_idx = obj_idx; + auto next_it = std::next(it); + if (next_it == target_objects.end() || is_sufficient_gap(*it, *next_it)) { + auto debug = utils::path_safety_checker::createObjectDebug(*it); + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return true; } } - return leading_obj_idx; + return false; } lanelet::BasicPolygon2d create_polygon(