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