@@ -121,7 +121,7 @@ start
121
121
:Initialize sampled_values with min_acc;
122
122
123
123
if (min_acc > max_acc) then (yes)
124
- :Return {} ;
124
+ :Return empty list ;
125
125
stop
126
126
elseif (max_acc - min_acc < epsilon) then (yes)
127
127
:Return {0.0};
@@ -185,54 +185,97 @@ The following flow chart illustrates the validity check.
185
185
186
186
``` plantuml
187
187
@startuml
188
- skinparam monochrome true
189
188
skinparam defaultTextAlignment center
190
- skinparam noteTextAlignment left
189
+ skinparam backgroundColor #White
191
190
192
- title Selecting Valid Candidate Paths
193
191
start
194
- :**INPUT** std::vector<LaneChangePath> input_paths;
192
+ if (Check if start point is valid by check if it is covered by neighbour lanes polygon) then (not covered)
193
+ #LightPink:Reject path;
194
+ stop
195
+ else (covered)
196
+ endif
195
197
196
- partition selectValidPaths {
197
- :**INITIALIZE** std::vector<LaneChangePath> valid_paths;
198
+ group check for distance #LightYellow
199
+ :Calculate total length and goal related distances;
200
+ if (total lane change length considering single lane change > distance from current pose to end of current lanes) then (yes)
201
+ #LightPink:Reject path;
202
+ stop
203
+ else (no)
204
+ endif
198
205
199
- :idx = 0;
206
+ if (goal is in current lanes) then (yes)
207
+ if (total lane change length considering multiple lane changes > distance from ego to goal along current lanes) then (yes)
208
+ #LightPink:Reject path;
209
+ stop
210
+ else (no)
211
+ endif
212
+ else (no)
213
+ endif
200
214
201
- while (idx < input_paths.size()?)
215
+ if (target lanes is empty) then (yes)
216
+ #LightPink:Reject path;
217
+ stop
218
+ else (no)
219
+ endif
220
+ if (total lane change length considering multiple lane changes > distance from ego to the end of target lanes) then (yes)
221
+ #LightPink:Reject path;
222
+ stop
223
+ else (no)
224
+ endif
225
+ end group
202
226
203
- :path = input_paths.at(idx);
204
227
205
- partition hasEnoughDistance {
206
228
207
- if(lane_change_total_distance < distance to end of current lanes
208
- &&
209
- lane_change_total_distance < distance to the next intersection
210
- &&
211
- lane_change_total_distance < distance from current pose to the goal
212
- &&
213
- lane_change_total_distance < distance to crosswalk
214
- &&
215
- goal is in route
216
- ) then (true)
217
- :path_validity = true;
218
- else (\n false)
219
- :path_validity = false;
229
+ group evaluate on Crosswalk #LightCyan
230
+ if (regulate_on_crosswalk and not enough length to crosswalk) then (yes)
231
+ if (stop time < stop time threshold\n(Related to stuck detection)) then (yes)
232
+ #LightPink:Reject path;
233
+ stop
234
+ else (no)
235
+ :Allow lane change in crosswalk;
236
+ endif
237
+ else (no)
220
238
endif
221
- }
239
+ end group
222
240
223
- if(path_validity == true)then (true)
241
+ group evaluate on Intersection #LightGreen
242
+ if (regulate_on_intersection and not enough length to intersection) then (yes)
243
+ if (stop time < stop time threshold\n(Related to stuck detection)) then (yes)
244
+ #LightPink:Reject path;
245
+ stop
246
+ else (no)
247
+ :Allow lane change in intersection;
248
+ endif
249
+ else (no)
250
+ endif
251
+ end group
224
252
225
- :valid_paths.push_back(path);
253
+ group evaluate on Traffic Light #Lavender
254
+ if (regulate_on_traffic_light and not enough length to complete lane change before stop line) then (yes)
255
+ #LightPink:Reject path;
256
+ stop
257
+ elseif (stopped at red traffic light within distance) then (yes)
258
+ #LightPink:Reject path;
259
+ stop
260
+ else (no)
261
+ endif
262
+ end group
226
263
227
- else (\nfalse)
264
+ if (ego is not stuck but parked vehicle exists in target lane) then (yes)
265
+ #LightPink:Reject path;
266
+ stop
267
+ else (no)
228
268
endif
229
- :++idx;
230
- endwhile (false)
231
269
232
- :**RETURN** valid_paths;
270
+ if (is safe to perform lane change) then (yes)
271
+ #Cyan:Return candidate path list;
272
+ stop
273
+ else (no)
274
+ #LightPink:Reject path;
275
+ endif
233
276
234
- }
235
277
stop
278
+
236
279
@enduml
237
280
```
238
281
0 commit comments