Skip to content

Commit e2bd68f

Browse files
kyoichi-sugaharakyoichi-sugahara
authored
docs(start_planner): add start/end condition (#6329)
add start/end condition Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent b396413 commit e2bd68f

6 files changed

+468
-230
lines changed

‎planning/behavior_path_start_planner_module/README.md

+201-79
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,207 @@ Here are some notable limitations:
7373
- The performance of safety check relies on the accuracy of the predicted paths generated by the map_based_prediction node. It's important to note that, currently, predicted paths that consider acceleration are not generated, and paths for low-speed objects may not be accurately produced, which requires caution.
7474
- Given the current specifications of the rule-based algorithms, there is a trade-off between the safety of a path and its naturalness to humans. While it's possible to adjust parameters to manage this trade-off, improvements are necessary to better reconcile these aspects.
7575

76+
## Start/End Conditions
77+
78+
### **Start Conditions**
79+
80+
The `StartPlannerModule` is designed to initiate its execution based on specific criteria evaluated by the `isExecutionRequested` function. The module will **not** start under the following conditions:
81+
82+
1. **Start pose on the middle of the road**: The module will not initiate if the start pose of the vehicle is determined to be in the middle of the road. This ensures the planner starts from a roadside position.
83+
84+
2. **Vehicle is far from start position**: If the vehicle is far from the start position, the module will not execute. This prevents redundant execution when the new goal is given.
85+
86+
3. **Vehicle reached goal**: The module will not start if the vehicle has already reached its goal position, avoiding unnecessary execution when the destination is attained.
87+
88+
4. **Vehicle in motion**: If the vehicle is still moving, the module will defer starting. This ensures that planning occurs from a stable, stationary state for safety.
89+
90+
5. **Goal behind in same route segment**: The module will not initiate if the goal position is behind the ego vehicle within the same route segment. This condition is checked to avoid complications with planning routes that require the vehicle to move backward on its current path, which is currently not supported.
91+
92+
### **End Conditions**
93+
94+
The `StartPlannerModule` terminates if the pull out / freespace maneuver has been completed. The `canTransitSuccessState` function assesses these conditions to decide if the module should terminate its execution.
95+
96+
```plantuml
97+
@startuml
98+
start
99+
:Start hasFinishedPullOut();
100+
101+
if (status_.driving_forward && status_.found_pull_out_path) then (yes)
102+
103+
if (status_.planner_type == FREESPACE) then (yes)
104+
:Calculate distance\nto pull_out_path.end_pose;
105+
if (distance < th_arrived_distance) then (yes)
106+
:return true;\n(Terminate module);
107+
else (no)
108+
:return false;\n(Continue running);
109+
endif
110+
else (no)
111+
:Calculate arclength for\ncurrent_pose and pull_out_path.end_pose;
112+
if (arclength_current - arclength_pull_out_end > offset) then (yes)
113+
:return true;\n(Terminate module);
114+
else (no)
115+
:return false;\n(Continue running);
116+
endif
117+
endif
118+
119+
else (no)
120+
:return false;\n(Continue running);
121+
endif
122+
123+
stop
124+
@enduml
125+
```
126+
127+
## Concept of safety assurance
128+
129+
The approach to collision safety is divided into two main components: generating paths that consider static information, and detecting collisions with dynamic obstacles to ensure the safety of the generated paths.
130+
131+
### 1. Generating path with static information
132+
133+
- **Path deviation checks**: This ensures that the path remains within the designated lanelets. By default, this feature is active, but it can be deactivated if necessary.
134+
135+
- **Static obstacle clearance from the path**: This involves verifying that a sufficient margin around static obstacles is maintained. The process includes creating a vehicle-sized footprint from the current position to the pull-out endpoint, which can be adjusted via parameters. The distance to static obstacle polygons is then calculated. If this distance is below a specified threshold, the path is deemed unsafe. Threshold levels (e.g., [2.0, 1.0, 0.5, 0.1]) can be configured, and the system searches for paths that meet the highest possible threshold based on a set search priority explained in following section, ensuring the selection of the safe path based on the policy. If no path meets the minimum threshold, it's determined that no safe path is available.
136+
137+
- **Clearance from stationary objects**: Maintaining an adequate distance from stationary objects positioned in front of and behind the vehicle is imperative for safety. Despite the path and stationary objects having a confirmed margin, the path is deemed unsafe if the distance from the shift start position to a front stationary object falls below `collision_check_margin_from_front_object` meters, or if the distance to a rear stationary object is shorter than `back_objects_collision_check_margin` meters.
138+
139+
- Why is a margin from the front object necessary?
140+
Consider a scenario in a "geometric pull out path" where the clearance from the path to a static obstacle is minimal, and there is a stopped vehicle ahead. In this case, although the path may meet safety standards and thus be generated, a concurrently operating avoidance module might deem it impossible to avoid the obstacle, potentially leading to vehicle deadlock. To ensure there is enough distance for avoidance maneuvers, the distance to the front obstacle is assessed. Increasing this parameter can prevent immobilization within the avoidance module but may also lead to the frequent generation of backward paths or geometric pull out path, resulting in paths that may seem unnatural to humans.
141+
142+
- Why is a margin from the rear object necessary?
143+
For objects ahead, another behavior module can intervene, allowing the path to overwrite itself through an avoidance plan, even if the clearance from the path to a static obstacle is minimal, thus maintaining a safe distance from static obstacles. However, for objects behind the vehicle, it is impossible for other behavior modules other than the start_planner to alter the path to secure a margin, potentially leading to a deadlock by an action module like "obstacle_cruise_planner" and subsequent immobilization. Therefore, a margin is set for stationary objects at the rear.
144+
145+
Here's the expression of the steps start pose searching steps, considering the `collision_check_margins` is set at [2.0, 1.0, 0.5, 0.1] as example. The process is as follows:
146+
147+
1. **Generating start pose candidates**
148+
149+
- Set the current position of the vehicle as the base point.
150+
- Determine the area of consideration behind the vehicle up to the `max_back_distance`.
151+
- Generate candidate points for the start pose in the backward direction at intervals defined by `backward_search_resolution`.
152+
- Include the current position as one of the start pose candidates.
153+
154+
![start pose candidate](images/start_pose_candidate.drawio.svg){width=1100}
155+
156+
2. **Starting search at maximum margin**
157+
158+
- Begin the search with the largest threshold (e.g., 2.0 meters).
159+
- Evaluate each start pose candidate to see if it maintains a margin of more than 2.0 meters.
160+
- Simultaneously, verify that the path generated from that start pose meets other necessary criteria (e.g., path deviation check).
161+
- Following the search priority described later, evaluate each in turn and adopt the start pose if it meets the conditions.
162+
163+
3. **Repeating search according to threshold levels**
164+
165+
- If no start pose meeting the conditions is found, lower the threshold to the next level (e.g., 1.0 meter) and repeat the search.
166+
167+
4. **Continuing the search**
168+
169+
- Continue the search until a start pose that meets the conditions is found, or the threshold level reaches the minimum value (e.g., 0.1 meter).
170+
- The aim of this process is to find a start pose that not only secures as large a margin as possible but also satisfies the conditions required for the path.
171+
172+
5. **Generating a stop path**
173+
- If no start pose satisfies the conditions at any threshold level, generate a stop path to ensure safety.
174+
175+
#### **search priority**
176+
177+
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.
178+
179+
During this backward search, different policies can be applied based on `search_priority` parameters:
180+
181+
Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move.
182+
Opting for `short_back_distance` aims to find a location with the least possible backward movement.
183+
184+
![priority_order](./images/priority_order.drawio.svg)
185+
186+
`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.
187+
188+
##### `efficient_path`
189+
190+
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.
191+
192+
| Index | Planner Type |
193+
| ----- | ------------------ |
194+
| 0 | shift_pull_out |
195+
| 1 | shift_pull_out |
196+
| ... | ... |
197+
| N | shift_pull_out |
198+
| 0 | geometric_pull_out |
199+
| 1 | geometric_pull_out |
200+
| ... | ... |
201+
| N | geometric_pull_out |
202+
203+
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.
204+
205+
##### `short_back_distance`
206+
207+
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.
208+
209+
| Index | Planner Type |
210+
| ----- | ------------------ |
211+
| 0 | shift_pull_out |
212+
| 0 | geometric_pull_out |
213+
| 1 | shift_pull_out |
214+
| 1 | geometric_pull_out |
215+
| ... | ... |
216+
| N | shift_pull_out |
217+
| N | geometric_pull_out |
218+
219+
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.
220+
221+
### 2. Collision detection with dynamic obstacles
222+
223+
- **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md)
224+
225+
- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring there is no overlap with the road lane's centerline. This is to avoid hindering the progress of following vehicles.
226+
227+
- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and without crossing the travel lane's centerline.
228+
229+
```plantuml
230+
@startuml
231+
start
232+
:Path Generation;
233+
234+
if (Collision with dynamic objects detected?) then (yes)
235+
if (Before departure?) then (yes)
236+
:Deactivate module decision is registered;
237+
else (no)
238+
if (Can stop within constraints \n && \n no crossing centerline?) then (yes)
239+
:Stop;
240+
else (no)
241+
:Continue with caution;
242+
endif
243+
endif
244+
else (no)
245+
endif
246+
247+
stop
248+
@enduml
249+
```
250+
251+
#### **example of safety check performed range for shift pull out**
252+
253+
Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint overlaps with the center line of the road lane, the safety check against dynamic objects is disabled.
254+
255+
<figure markdown>
256+
![safety check performed range for shift pull out](images/collision_check_range_shift_pull_out.drawio.svg){width=1100}
257+
</figure>
258+
259+
**As a note, no safety check is performed during backward movements. Safety verification commences at the point where the backward motion ceases.**
260+
261+
## RTC interface
262+
263+
The system operates distinctly under manual and auto mode, especially concerning the before the departure and interaction with dynamic obstacles. Below are the specific behaviors for each mode:
264+
265+
### When approval is required?
266+
267+
#### Forward driving
268+
269+
- **Start approval required**: Even if a path is generated, approval is required to initiate movement. If a dynamic object poses a risk, such as an approaching vehicle from behind, candidate paths may be displayed, but approval is necessary for departure.
270+
271+
#### Backward driving + forward driving
272+
273+
- **Multiple approvals required**: When the planned path includes a backward driving, multiple approvals are needed before starting the reverse and again before restarting forward movement. Before initiating forward movement, the system conducts safety checks against dynamic obstacles. If a detection is detected, approval for departure is necessary.
274+
275+
This differentiation ensures that the vehicle operates safely by requiring human intervention in manual mode(`enable_rtc` is true) at critical junctures and incorporating automatic safety checks in both modes to prevent unsafe operations in the presence of dynamic obstacles.
276+
76277
## Design
77278

78279
```plantuml
@@ -132,39 +333,6 @@ PullOutPath --o PullOutPlannerBase
132333
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
133334
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
134335

135-
## Safety check with static obstacles
136-
137-
1. Calculate ego-vehicle's footprint on pull out path between from current position to pull out end point. (Illustrated by blue frame)
138-
2. Calculate object's polygon
139-
3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path
140-
141-
![pull_out_collision_check](./images/pull_out_collision_check.drawio.svg)
142-
143-
## Safety check with dynamic obstacles
144-
145-
### **Basic concept of safety check against dynamic obstacles**
146-
147-
This is based on the concept of RSS. For the logic used, refer to the link below.
148-
See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md)
149-
150-
### **Collision check performed range**
151-
152-
A collision check with dynamic objects is primarily performed between the shift start point and end point. The range for safety check varies depending on the type of path generated, so it will be explained for each pattern.
153-
154-
#### **Shift pull out**
155-
156-
For the "shift pull out", safety verification starts at the beginning of the shift and ends at the shift's conclusion.
157-
158-
#### **Geometric pull out**
159-
160-
Since there's a stop at the midpoint during the shift, this becomes the endpoint for safety verification. After stopping, safety verification resumes.
161-
162-
#### **Backward pull out start point search**
163-
164-
During backward movement, no safety check is performed. Safety check begins at the point where the backward movement ends.
165-
166-
![collision_check_range](./images/collision_check_range.drawio.svg)
167-
168336
### **Ego vehicle's velocity planning**
169337

170338
WIP
@@ -303,52 +471,6 @@ If a safe path cannot be generated from the current position, search backwards f
303471

304472
[pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4)
305473

306-
### **search priority**
307-
308-
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.
309-
310-
During this backward search, different policies can be applied based on `search_priority` parameters:
311-
312-
Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move.
313-
Opting for `short_back_distance` aims to find a location with the least possible backward movement.
314-
315-
![priority_order](./images/priority_order.drawio.svg)
316-
317-
`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.
318-
319-
#### `efficient_path`
320-
321-
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.
322-
323-
| Index | Planner Type |
324-
| ----- | ------------------ |
325-
| 0 | shift_pull_out |
326-
| 1 | shift_pull_out |
327-
| ... | ... |
328-
| N | shift_pull_out |
329-
| 0 | geometric_pull_out |
330-
| 1 | geometric_pull_out |
331-
| ... | ... |
332-
| N | geometric_pull_out |
333-
334-
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.
335-
336-
#### `short_back_distance`
337-
338-
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.
339-
340-
| Index | Planner Type |
341-
| ----- | ------------------ |
342-
| 0 | shift_pull_out |
343-
| 0 | geometric_pull_out |
344-
| 1 | shift_pull_out |
345-
| 1 | geometric_pull_out |
346-
| ... | ... |
347-
| N | shift_pull_out |
348-
| N | geometric_pull_out |
349-
350-
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.
351-
352474
### **parameters for backward pull out start point search**
353475

354476
| Name | Unit | Type | Description | Default value |

0 commit comments

Comments
 (0)