From f2502482ddd7a4a490d6ee5daab1f4d563313e3e Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 10 Jan 2024 10:01:55 +0900 Subject: [PATCH 1/5] docs(start_planner): update explanation about start pose candidate's priority (#6003) * add explanation about start pose candidate priority Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kosuke Takeuchi --- .../README.md | 46 +++ .../images/priority_order.drawio.svg | 319 ++++++++++++++++++ 2 files changed, 365 insertions(+) create mode 100644 planning/behavior_path_start_planner_module/images/priority_order.drawio.svg diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 1a730212f828b..f4aa3936bee56 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -243,6 +243,52 @@ If a safe path cannot be generated from the current position, search backwards f [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) +### **search priority** + +If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation. + +During this backward search, different policies can be applied based on `search_priority` parameters: + +Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move. +Opting for `short_back_distance` aims to find a location with the least possible backward movement. + +![priority_order](./images/priority_order.drawio.svg) + +`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation. + +#### `efficient_path` + +When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 1 | shift_pull_out | +| ... | ... | +| N | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | geometric_pull_out | + +This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate. + +#### `short_back_distance` + +For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | shift_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | shift_pull_out | +| N | geometric_pull_out | + +This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position. + ### **parameters for backward pull out start point search** | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg new file mode 100644 index 0000000000000..ab48c30b6fdae --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg @@ -0,0 +1,319 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + (0, shift_pull_out) +
+ + (1, shift_pull_out) +
+ (2, shift_pull_out) +
+ (3, shift_pull_out) +
+ (4, shift_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (0, + geometric_pull_out + ) +
+ (1, + geometric_pull_out + ) +
+ (2, + geometric_pull_out + ) +
+ (3, + geometric_pull_out + ) +
+ (4, + geometric_pull_out + ) +
+ ︙ +
+ (N, + geometric_pull_out + ) + +
+ +
+
+ + +
+
backward_search_resolution
+
+ + +
+
geometric pull out path
+
+ + +
+
start pose candidate
+
+ + +
+
idx: 0
+
+ + +
+
idx: 1
+
+ + +
+
idx: 2
+
+ + +
+
idx: 3
+
+ + +
+
idx: 4
+
+ + +
+
shift pull out path
+
+ + +
+
+ (idx, planner_type):= +
+ (3, geometric_pull_out) +
+
+ + +
+
(3, shift_pull_out)
+
+ + +
+
idx: N
+
+ + +
+
+ + efficient_path +
+ +
+
+ + +
+
+ (0, shift_pull_out) +
+ (0, geometric_pull_out) +
+ (1, shift_pull_out) +
+ (1, geometric_pull_out) +
+ (2, shift_pull_out) +
+ (2, geometric_pull_out) +
+ (3, shift_pull_out) +
+ (3, geometric_pull_out) +
+ (4, shift_pull_out) +
+ (4, geometric_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (N, geometric_pull_out) + +
+ +
+
+ + +
+
+ + short_back_distance +
+ +
+
+ + +
+
high priority
+
+ + +
+
low priority
+
+ + From 0c7f9266f4a791b2bca7d0f90f80ac96e627eef2 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 10 Jan 2024 14:10:43 +0900 Subject: [PATCH 2/5] fix(start_planner): don't update start pose when backward driving is finished (#5998) Fix conditional check in updatePullOutStatus function Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 9870a324caf64..66e04d7e9bb7a 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -839,8 +839,10 @@ void StartPlannerModule::updatePullOutStatus() return {*refined_start_pose}; }); - planWithPriority( - start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + if (!status_.backward_driving_complete) { + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + } start_planner_data_.refined_start_pose = *refined_start_pose; start_planner_data_.start_pose_candidates = start_pose_candidates; From 912bfd7609865ae276a96310300b0151f7c406f7 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 10 Jan 2024 15:16:13 +0900 Subject: [PATCH 3/5] docs(start_planner): update Purpose / Role of the document (#6002) Update Start Planner module to stop in response to dynamic obstacles Signed-off-by: kyoichi-sugahara --- .../README.md | 20 +++++++---- .../images/start_from_road_lane.drawio.svg | 34 ------------------- 2 files changed, 13 insertions(+), 41 deletions(-) delete mode 100644 planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index f4aa3936bee56..2f90cc2ee3cf2 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -2,17 +2,23 @@ ## Purpose / Role -The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and implementing safety checks against dynamic obstacles. (Note: The feature of safety checks against dynamic obstacles is currently a work in progress.) -This module is activated when a new route is received. +The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and stopping in response to dynamic obstacles when a collision is detected. -Use cases are as follows +Use cases include: -- start smoothly from the current ego position to centerline. - ![case1](./images/start_from_road_lane.drawio.svg) - pull out from the side of the road lane to centerline. - ![case2](./images/start_from_road_side.drawio.svg) + +
+ ![case1](images/start_from_road_side.drawio.svg){width=1000} +
pull out from side of the road lane
+
+ - pull out from the shoulder lane to the road lane centerline. - ![case3](./images/start_from_road_shoulder.drawio.svg) + +
+ ![case2](images/start_from_start_from_road_shoulder.drawio.svg){width=1000} +
pull out from the shoulder lane
+
## Design diff --git a/planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg deleted file mode 100644 index 7a89cc2183acf..0000000000000 --- a/planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - From f9dcafba05b94aaca7675e379f34dbd50a52efd2 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 11 Jan 2024 11:39:43 +0900 Subject: [PATCH 4/5] feat(start_planner): keep distance against front objects (#5983) * refactor extractCollisionCheckPath Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kosuke Takeuchi --- .../behavior_path_start_planner_module/README.md | 3 ++- .../config/start_planner.param.yaml | 1 + .../start_planner_module.hpp | 2 +- .../start_planner_parameters.hpp | 1 + .../src/manager.cpp | 2 ++ .../src/start_planner_module.cpp | 15 +++++++++++---- 6 files changed, 18 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 2f90cc2ee3cf2..80ad8d6675e9a 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -75,7 +75,8 @@ PullOutPath --o PullOutPlannerBase | intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | | length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | | collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | | center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 514d61e225ecd..d04728861a52e 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -7,6 +7,7 @@ th_stopped_time: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 + collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index a451a92c16ff1..eb1827ec0045d 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -174,7 +174,7 @@ class StartPlannerModule : public SceneModuleInterface const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose); - PathWithLaneId extractCollisionCheckPath(const PullOutPath & path); + PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 783aace0983ca..2df75107d872c 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -44,6 +44,7 @@ struct StartPlannerParameters double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; double collision_check_margin{0.0}; double collision_check_distance_from_end{0.0}; + double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 58d0fbee7921b..f719718a3389e 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -46,6 +46,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); + p.collision_check_margin_from_front_object = + node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 66e04d7e9bb7a..fd23adf654502 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -645,7 +645,7 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckPath(*pull_out_path), pull_out_lane_stop_objects, + vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, parameters_->collision_check_margin)) { return false; } @@ -660,7 +660,7 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckPath(const PullOutPath & path) +PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) { PathWithLaneId combined_path; for (const auto & partial_path : path.partial_paths) { @@ -912,6 +912,10 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, backward_path_length, std::numeric_limits::max()); + const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, + std::numeric_limits::max()); + // Set the maximum backward distance less than the distance from the vehicle's base_link to the // lane's rearmost point to prevent lane departure. const double current_arc_length = @@ -924,9 +928,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( back_path_from_start_pose.points, start_pose.position, -back_distance); - if (!backed_pose) { + if (!backed_pose) continue; + + if (utils::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, front_stop_objects_in_pull_out_lanes, + parameters_->collision_check_margin_from_front_object)) continue; - } const double backed_pose_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length; From a068b861cbee0afbf129a6c4bdcfd9fc4fba57a9 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 11 Jan 2024 14:53:37 +0900 Subject: [PATCH 5/5] feat(start_planner): define collision check margin as list (#5994) * define collision check margins list in start planner module Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../parking_departure/common_module_data.hpp | 2 + .../README.md | 26 +++++------ .../config/start_planner.param.yaml | 2 +- .../start_planner_module.hpp | 2 +- .../start_planner_parameters.hpp | 2 +- .../src/manager.cpp | 3 +- .../src/start_planner_module.cpp | 45 ++++++++++--------- 7 files changed, 45 insertions(+), 37 deletions(-) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 91683f4547659..3dc230f0e92ed 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -41,6 +41,8 @@ struct StartGoalPlannerData Pose refined_start_pose; std::vector start_pose_candidates; + size_t selected_start_pose_candidate_index; + double margin_for_start_pose_candidate; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 80ad8d6675e9a..aeefd18357a5c 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -65,19 +65,19 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | -| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | [double] | Obstacle collision check margins list | [2.0, 1.5, 1.0] | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index d04728861a52e..d174b54b9ccbe 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,7 +5,7 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margin: 1.0 + collision_check_margins: [2.0, 1.5, 1.0] collision_check_distance_from_end: 1.0 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index eb1827ec0045d..41a80e59d56bf 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -172,7 +172,7 @@ class StartPlannerModule : public SceneModuleInterface const std::string & search_priority, const size_t start_pose_candidates_num); bool findPullOutPath( const Pose & start_pose_candidate, const std::shared_ptr & planner, - const Pose & refined_start_pose, const Pose & goal_pose); + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); void updateStatusWithCurrentPath( diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 2df75107d872c..66dbddebf99bb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -42,7 +42,7 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; - double collision_check_margin{0.0}; + std::vector collision_check_margins{}; double collision_check_distance_from_end{0.0}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index f719718a3389e..8cc0b34082e44 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -43,7 +43,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); - p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); + p.collision_check_margins = + node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); p.collision_check_margin_from_front_object = diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index fd23adf654502..1183281a9392b 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -585,9 +585,16 @@ void StartPlannerModule::planWithPriority( const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); - for (const auto & [index, planner] : order_priority) { - if (findPullOutPath(start_pose_candidates[index], planner, refined_start_pose, goal_pose)) - return; + for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath( + start_pose_candidates[index], planner, refined_start_pose, goal_pose, + collision_check_margin)) { + start_planner_data_.selected_start_pose_candidate_index = index; + start_planner_data_.margin_for_start_pose_candidate = collision_check_margin; + return; + } + } } updateStatusIfNoSafePathFound(); @@ -618,7 +625,7 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( bool StartPlannerModule::findPullOutPath( const Pose & start_pose_candidate, const std::shared_ptr & planner, - const Pose & refined_start_pose, const Pose & goal_pose) + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin) { const auto & dynamic_objects = planner_data_->dynamic_object; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -646,7 +653,7 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, - parameters_->collision_check_margin)) { + collision_check_margin)) { return false; } @@ -667,23 +674,21 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat combined_path.points.insert( combined_path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - // calculate collision check end idx - size_t collision_check_end_idx = 0; - const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); - - if (collision_check_end_pose) { - collision_check_end_idx = - motion_utils::findNearestIndex(combined_path.points, collision_check_end_pose->position); - } + const size_t collision_check_end_idx = std::invoke([&]() { + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + if (collision_check_end_pose) { + return motion_utils::findNearestIndex( + combined_path.points, collision_check_end_pose->position); + } else { + return combined_path.points.size() - 1; + } + }); // remove the point behind of collision check end pose - if (collision_check_end_idx + 1 < combined_path.points.size()) { - combined_path.points.erase( - combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); - } - + combined_path.points.erase( + combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); return combined_path; } @@ -950,7 +955,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( if (utils::checkCollisionBetweenFootprintAndObjects( local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, - parameters_->collision_check_margin)) { + parameters_->collision_check_margins.back())) { break; // poses behind this has a collision, so break. }