1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.LinkedHashMap;
4 import java.util.LinkedHashSet;
5 import java.util.Map;
6 import java.util.Objects;
7 import java.util.Set;
8 import java.util.SortedSet;
9 import java.util.TreeSet;
10 import java.util.WeakHashMap;
11
12 import org.djunits.value.vdouble.scalar.Length;
13 import org.djutils.exceptions.Throw;
14 import org.djutils.exceptions.Try;
15 import org.opentrafficsim.base.TimeStampedObject;
16 import org.opentrafficsim.base.parameters.ParameterException;
17 import org.opentrafficsim.core.gtu.GTUException;
18 import org.opentrafficsim.core.gtu.RelativePosition;
19 import org.opentrafficsim.core.network.LateralDirectionality;
20 import org.opentrafficsim.core.network.NetworkException;
21 import org.opentrafficsim.core.network.route.Route;
22 import org.opentrafficsim.road.gtu.lane.Break;
23 import org.opentrafficsim.road.gtu.lane.perception.InfrastructureLaneChangeInfo;
24 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
25 import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
26 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
27 import org.opentrafficsim.road.network.lane.Lane;
28 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
29 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
30 import org.opentrafficsim.road.network.speed.SpeedLimitProspect;
31 import org.opentrafficsim.road.network.speed.SpeedLimitTypes;
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48 public class DirectInfrastructurePerception extends LaneBasedAbstractPerceptionCategory implements InfrastructurePerception
49 {
50
51
52 private static final long serialVersionUID = 20160811L;
53
54
55 private final Map<RelativeLane, TimeStampedObject<SortedSet<InfrastructureLaneChangeInfo>>> infrastructureLaneChangeInfo =
56 new LinkedHashMap<>();
57
58
59 private Map<RelativeLane, TimeStampedObject<SpeedLimitProspect>> speedLimitProspect = new LinkedHashMap<>();
60
61
62 private final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<
63 LaneChangePossibility>>> legalLaneChangePossibility = new LinkedHashMap<>();
64
65
66 private final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<
67 LaneChangePossibility>>> physicalLaneChangePossibility = new LinkedHashMap<>();
68
69
70 private TimeStampedObject<SortedSet<RelativeLane>> crossSection;
71
72
73 private final Map<LaneStructureRecord, Boolean> anyNextOkCache = new WeakHashMap<>();
74
75
76 private final Set<LaneStructureRecord> cutOff = new LinkedHashSet<>();
77
78
79 private LaneStructureRecord root;
80
81
82 private Set<Lane> lanes;
83
84
85 private Route route;
86
87
88
89
90 public DirectInfrastructurePerception(final LanePerception perception)
91 {
92 super(perception);
93 }
94
95
96 @Override
97 public void updateAll() throws GTUException, ParameterException
98 {
99 updateCrossSection();
100
101 Set<RelativeLane> cs = getCrossSection();
102 this.infrastructureLaneChangeInfo.keySet().retainAll(cs);
103 this.legalLaneChangePossibility.keySet().retainAll(cs);
104 this.physicalLaneChangePossibility.keySet().retainAll(cs);
105 this.speedLimitProspect.keySet().retainAll(cs);
106
107 LaneStructureRecord newRoot = getPerception().getLaneStructure().getRootRecord();
108 if (this.root == null || !newRoot.equals(this.root) || !this.lanes.equals(getPerception().getGtu().positions(
109 RelativePosition.REFERENCE_POSITION).keySet()) || !Objects.equals(this.route, getPerception().getGtu()
110 .getStrategicalPlanner().getRoute()) || this.cutOff.stream().filter((record) -> !record.isCutOffEnd())
111 .count() > 0)
112 {
113 this.cutOff.clear();
114 this.root = newRoot;
115 this.lanes = getPerception().getGtu().positions(RelativePosition.REFERENCE_POSITION).keySet();
116 this.route = getPerception().getGtu().getStrategicalPlanner().getRoute();
117
118 this.speedLimitProspect.clear();
119 for (RelativeLane lane : getCrossSection())
120 {
121 updateInfrastructureLaneChangeInfo(lane);
122 updateLegalLaneChangePossibility(lane, LateralDirectionality.LEFT);
123 updateLegalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
124 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.LEFT);
125 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
126 }
127 }
128
129
130 for (RelativeLane lane : getCrossSection())
131 {
132 updateSpeedLimitProspect(lane);
133 }
134 for (RelativeLane lane : getCrossSection())
135 {
136 if (!this.infrastructureLaneChangeInfo.containsKey(lane))
137 {
138 updateInfrastructureLaneChangeInfo(lane);
139 updateLegalLaneChangePossibility(lane, LateralDirectionality.LEFT);
140 updateLegalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
141 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.LEFT);
142 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
143 }
144 }
145 }
146
147
148 @Override
149 public final void updateInfrastructureLaneChangeInfo(final RelativeLane lane) throws GTUException, ParameterException
150 {
151 if (this.infrastructureLaneChangeInfo.containsKey(lane) && this.infrastructureLaneChangeInfo.get(lane).getTimestamp()
152 .equals(getTimestamp()))
153 {
154
155 return;
156 }
157 updateCrossSection();
158
159
160 SortedSet<InfrastructureLaneChangeInfo> resultSet = new TreeSet<>();
161 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
162 try
163 {
164 record = getPerception().getLaneStructure().getFirstRecord(lane);
165 if (!record.allowsRoute(getGtu().getStrategicalPlanner().getRoute(), getGtu().getGTUType()))
166 {
167 resultSet.add(InfrastructureLaneChangeInfo.fromInaccessibleLane(record.isDeadEnd()));
168 this.infrastructureLaneChangeInfo.put(lane, new TimeStampedObject<>(resultSet, getTimestamp()));
169 return;
170 }
171 }
172 catch (NetworkException exception)
173 {
174 throw new GTUException("Route has no destination.", exception);
175 }
176 Map<LaneStructureRecord, InfrastructureLaneChangeInfo> currentSet = new LinkedHashMap<>();
177 Map<LaneStructureRecord, InfrastructureLaneChangeInfo> nextSet = new LinkedHashMap<>();
178 RelativePosition front = getPerception().getGtu().getFront();
179 currentSet.put(record, new InfrastructureLaneChangeInfo(0, record, front, record.isDeadEnd(),
180 LateralDirectionality.NONE));
181 while (!currentSet.isEmpty())
182 {
183
184 nextSet.putAll(currentSet);
185 for (LaneStructureRecord laneRecord : currentSet.keySet())
186 {
187 while (laneRecord.legalLeft() && !nextSet.containsKey(laneRecord.getLeft()))
188 {
189 InfrastructureLaneChangeInfo info = nextSet.get(laneRecord).left(laneRecord.getLeft(), front, laneRecord
190 .getLeft().isDeadEnd());
191 nextSet.put(laneRecord.getLeft(), info);
192 laneRecord = laneRecord.getLeft();
193 }
194 }
195 for (LaneStructureRecord laneRecord : currentSet.keySet())
196 {
197 while (laneRecord.legalRight() && !nextSet.containsKey(laneRecord.getRight()))
198 {
199 InfrastructureLaneChangeInfo info = nextSet.get(laneRecord).right(laneRecord.getRight(), front, laneRecord
200 .getRight().isDeadEnd());
201 nextSet.put(laneRecord.getRight(), info);
202 laneRecord = laneRecord.getRight();
203 }
204 }
205
206 currentSet = nextSet;
207 nextSet = new LinkedHashMap<>();
208 InfrastructureLaneChangeInfo bestOk = null;
209 InfrastructureLaneChangeInfo bestNotOk = null;
210 boolean deadEnd = false;
211 for (LaneStructureRecord laneRecord : currentSet.keySet())
212 {
213 boolean anyOk = Try.assign(() -> anyNextOk(laneRecord), "Route has no destination.");
214 if (anyOk)
215 {
216
217 for (LaneStructureRecord next : laneRecord.getNext())
218 {
219 try
220 {
221 if (next.allowsRoute(getGtu().getStrategicalPlanner().getRoute(), getGtu().getGTUType()))
222 {
223 InfrastructureLaneChangeInfo prev = currentSet.get(laneRecord);
224 InfrastructureLaneChangeInfotion/InfrastructureLaneChangeInfo.html#InfrastructureLaneChangeInfo">InfrastructureLaneChangeInfo info = new InfrastructureLaneChangeInfo(prev
225 .getRequiredNumberOfLaneChanges(), next, front, next.isDeadEnd(), prev
226 .getLateralDirectionality());
227 nextSet.put(next, info);
228 }
229 }
230 catch (NetworkException exception)
231 {
232 throw new RuntimeException("Network exception while considering route on next lane.", exception);
233 }
234 }
235
236 if (bestOk == null || currentSet.get(laneRecord).getRequiredNumberOfLaneChanges() < bestOk
237 .getRequiredNumberOfLaneChanges())
238 {
239 bestOk = currentSet.get(laneRecord);
240 }
241 }
242 else
243 {
244
245 deadEnd = deadEnd || currentSet.get(laneRecord).isDeadEnd();
246 if (bestNotOk == null || currentSet.get(laneRecord).getRequiredNumberOfLaneChanges() < bestNotOk
247 .getRequiredNumberOfLaneChanges())
248 {
249 bestNotOk = currentSet.get(laneRecord);
250 }
251 }
252
253 }
254 if (bestOk == null)
255 {
256 break;
257 }
258
259 if (bestNotOk != null && bestOk.getRequiredNumberOfLaneChanges() > bestNotOk.getRequiredNumberOfLaneChanges())
260 {
261 bestOk.setDeadEnd(deadEnd);
262 resultSet.add(bestOk);
263 }
264 currentSet = nextSet;
265 nextSet = new LinkedHashMap<>();
266 }
267
268
269 this.infrastructureLaneChangeInfo.put(lane, new TimeStampedObject<>(resultSet, getTimestamp()));
270 }
271
272
273
274
275
276
277
278
279
280
281 private boolean anyNextOk(final LaneStructureRecord record) throws NetworkException, GTUException
282 {
283 if (record.isCutOffEnd())
284 {
285 this.cutOff.add(record);
286 return true;
287 }
288
289 Boolean ok = this.anyNextOkCache.get(record);
290 if (ok != null)
291 {
292 return ok;
293 }
294
295 for (SingleSensor s : record.getLane().getSensors())
296 {
297
298 if (s instanceof SinkSensor)
299 {
300 this.anyNextOkCache.put(record, true);
301 return true;
302 }
303 }
304
305 Route currentRoute = getGtu().getStrategicalPlanner().getRoute();
306 try
307 {
308 if (currentRoute != null && currentRoute.destinationNode().equals(record.getToNode()))
309 {
310 this.anyNextOkCache.put(record, true);
311 return true;
312 }
313 }
314 catch (NetworkException exception)
315 {
316 throw new RuntimeException("Could not determine destination node.", exception);
317 }
318
319 if (record.getNext().isEmpty())
320 {
321 this.anyNextOkCache.put(record, false);
322 return false;
323 }
324
325 if (currentRoute == null)
326 {
327 this.anyNextOkCache.put(record, true);
328 return true;
329 }
330
331 ok = record.allowsRouteAtEnd(currentRoute, getGtu().getGTUType());
332 this.anyNextOkCache.put(record, ok);
333 return ok;
334 }
335
336
337 @Override
338 public final void updateSpeedLimitProspect(final RelativeLane lane) throws GTUException, ParameterException
339 {
340 updateCrossSection();
341 checkLaneIsInCrossSection(lane);
342 TimeStampedObject<SpeedLimitProspect> tsSlp = this.speedLimitProspect.get(lane);
343 SpeedLimitProspect slp;
344 if (tsSlp != null)
345 {
346 slp = tsSlp.getObject();
347 slp.update(getGtu().getOdometer());
348 }
349 else
350 {
351 slp = new SpeedLimitProspect(getGtu().getOdometer());
352 slp.addSpeedInfo(Length.ZERO, SpeedLimitTypes.MAX_VEHICLE_SPEED, getGtu().getMaximumSpeed(), getGtu());
353 }
354 try
355 {
356 Lane laneObj = getGtu().getReferencePosition().getLane();
357 if (!slp.containsAddSource(laneObj))
358 {
359 slp.addSpeedInfo(Length.ZERO, SpeedLimitTypes.FIXED_SIGN, laneObj.getSpeedLimit(getGtu().getGTUType()),
360 laneObj);
361 }
362 }
363 catch (NetworkException exception)
364 {
365 throw new RuntimeException("Could not obtain speed limit from lane for perception.", exception);
366 }
367 this.speedLimitProspect.put(lane, new TimeStampedObject<>(slp, getTimestamp()));
368 }
369
370
371 @Override
372 public final void updateLegalLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat)
373 throws GTUException, ParameterException
374 {
375 updateLaneChangePossibility(lane, lat, true, this.legalLaneChangePossibility);
376 }
377
378
379 @Override
380 public final void updatePhysicalLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat)
381 throws GTUException, ParameterException
382 {
383 updateLaneChangePossibility(lane, lat, false, this.physicalLaneChangePossibility);
384 }
385
386
387
388
389
390
391
392
393
394
395
396
397
398 private void updateLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat, final boolean legal,
399 final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<LaneChangePossibility>>> possibilityMap)
400 throws GTUException, ParameterException
401 {
402 updateCrossSection();
403 checkLaneIsInCrossSection(lane);
404
405 if (possibilityMap.get(lane) == null)
406 {
407 possibilityMap.put(lane, new LinkedHashMap<>());
408 }
409 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
410
411 Length tail = getPerception().getGtu().getRear().getDx();
412 while (record != null && record.getStartDistance().gt(tail) && !record.getPrev().isEmpty() && ((lat.isLeft() && record
413 .possibleLeft(legal)) || (lat.isRight() && record.possibleRight(legal))))
414 {
415 if (record.getPrev().size() > 1)
416 {
417
418 possibilityMap.get(lane).put(lat, new TimeStampedObject<>(new LaneChangePossibility(record.getPrev().get(0),
419 tail, true), getTimestamp()));
420 return;
421 }
422 else if (record.getPrev().isEmpty())
423 {
424
425 break;
426 }
427 record = record.getPrev().get(0);
428 if ((lat.isLeft() && !record.possibleLeft(legal)) || (lat.isRight() && !record.possibleRight(legal)))
429 {
430
431 possibilityMap.get(lane).put(lat, new TimeStampedObject<>(new LaneChangePossibility(record, tail, true),
432 getTimestamp()));
433 return;
434 }
435 }
436
437 LaneStructureRecord prevRecord = null;
438 record = getPerception().getLaneStructure().getFirstRecord(lane);
439
440 Length dx;
441 if ((lat.isLeft() && record.possibleLeft(legal)) || (lat.isRight() && record.possibleRight(legal)))
442 {
443 dx = getPerception().getGtu().getFront().getDx();
444 while (record != null && ((lat.isLeft() && record.possibleLeft(legal)) || (lat.isRight() && record.possibleRight(
445 legal))))
446 {
447
448 prevRecord = record;
449 record = record.getNext().isEmpty() ? null : record.getNext().get(0);
450 }
451 }
452 else
453 {
454 dx = getPerception().getGtu().getRear().getDx();
455 while (record != null && ((lat.isLeft() && !record.possibleLeft(legal)) || (lat.isRight() && !record.possibleRight(
456 legal))))
457 {
458
459 prevRecord = record;
460 record = record.getNext().isEmpty() ? null : record.getNext().get(0);
461 }
462 }
463 possibilityMap.get(lane).put(lat, new TimeStampedObject<>(new LaneChangePossibility(prevRecord, dx, true),
464 getTimestamp()));
465 }
466
467
468
469
470
471 private void checkLaneIsInCrossSection(final RelativeLane lane) throws GTUException
472 {
473 Throw.when(!getCrossSection().contains(lane), GTUException.class,
474 "The requeasted lane %s is not in the most recent cross section.", lane);
475 }
476
477
478 @Override
479 public final void updateCrossSection() throws GTUException, ParameterException
480 {
481 if (this.crossSection != null && this.crossSection.getTimestamp().equals(getTimestamp()))
482 {
483
484 return;
485 }
486 this.crossSection = new TimeStampedObject<>(getPerception().getLaneStructure().getExtendedCrossSection(),
487 getTimestamp());
488 }
489
490
491 @Override
492 public final SortedSet<InfrastructureLaneChangeInfo> getInfrastructureLaneChangeInfo(final RelativeLane lane)
493 {
494 return this.infrastructureLaneChangeInfo.get(lane).getObject();
495 }
496
497
498 @Override
499 public final SpeedLimitProspect getSpeedLimitProspect(final RelativeLane lane)
500 {
501 return this.speedLimitProspect.get(lane).getObject();
502 }
503
504
505 @Override
506 public final Length getLegalLaneChangePossibility(final RelativeLane fromLane, final LateralDirectionality lat)
507 {
508 return this.legalLaneChangePossibility.get(fromLane).get(lat).getObject().getDistance(lat);
509 }
510
511
512 @Override
513 public final Length getPhysicalLaneChangePossibility(final RelativeLane fromLane, final LateralDirectionality lat)
514 {
515 return this.physicalLaneChangePossibility.get(fromLane).get(lat).getObject().getDistance(lat);
516 }
517
518
519 @Override
520 public final SortedSet<RelativeLane> getCrossSection()
521 {
522 return this.crossSection.getObject();
523 }
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548 public final TimeStampedObject<SortedSet<InfrastructureLaneChangeInfo>> getTimeStampedInfrastructureLaneChangeInfo(
549 final RelativeLane lane)
550 {
551 return this.infrastructureLaneChangeInfo.get(lane);
552 }
553
554
555
556
557
558
559 public final TimeStampedObject<SpeedLimitProspect> getTimeStampedSpeedLimitProspect(final RelativeLane lane)
560 {
561 return this.speedLimitProspect.get(lane);
562 }
563
564
565
566
567
568
569
570
571 public final TimeStampedObject<Length> getTimeStampedLegalLaneChangePossibility(final RelativeLane fromLane,
572 final LateralDirectionality lat)
573 {
574 TimeStampedObject<LaneChangePossibility> tsLcp = this.legalLaneChangePossibility.get(fromLane).get(lat);
575 LaneChangePossibility lcp = tsLcp.getObject();
576 return new TimeStampedObject<>(lcp.getDistance(lat), tsLcp.getTimestamp());
577 }
578
579
580
581
582
583
584
585
586 public final TimeStampedObject<Length> getTimeStampedPhysicalLaneChangePossibility(final RelativeLane fromLane,
587 final LateralDirectionality lat)
588 {
589 TimeStampedObject<LaneChangePossibility> tsLcp = this.physicalLaneChangePossibility.get(fromLane).get(lat);
590 LaneChangePossibility lcp = tsLcp.getObject();
591 return new TimeStampedObject<>(lcp.getDistance(lat), tsLcp.getTimestamp());
592 }
593
594
595
596
597
598 public final TimeStampedObject<SortedSet<RelativeLane>> getTimeStampedCrossSection()
599 {
600 return this.crossSection;
601 }
602
603
604 @Override
605 public final String toString()
606 {
607 return "DirectInfrastructurePerception";
608 }
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623 private class LaneChangePossibility
624 {
625
626
627 private final LaneStructureRecord record;
628
629
630 private final double dx;
631
632
633 private final boolean legal;
634
635
636
637
638
639
640 LaneChangePossibility(final LaneStructureRecord record, final Length dx, final boolean legal)
641 {
642 this.record = record;
643 this.dx = dx.si;
644 this.legal = legal;
645 }
646
647
648
649
650
651
652 final Length getDistance(final LateralDirectionality lat)
653 {
654 double d = this.record.getStartDistance().si + this.record.getLane().getLength().si - this.dx;
655 if ((lat.isLeft() && this.record.possibleLeft(this.legal)) || (lat.isRight() && this.record.possibleRight(
656 this.legal)))
657 {
658 return Length.instantiateSI(d);
659 }
660 return Length.instantiateSI(-d);
661 }
662
663 }
664
665 }