From 682dc5905b0abb9610658d1c0ac1da9074798a2e Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 10 Jan 2024 10:01:55 +0900 Subject: [PATCH] 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
+
+ +