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.djutils.exceptions.Throw;
15 import org.djutils.exceptions.Try;
16 import org.opentrafficsim.base.TimeStampedObject;
17 import org.opentrafficsim.base.parameters.ParameterException;
18 import org.opentrafficsim.core.gtu.GTUException;
19 import org.opentrafficsim.core.gtu.RelativePosition;
20 import org.opentrafficsim.core.network.LateralDirectionality;
21 import org.opentrafficsim.core.network.NetworkException;
22 import org.opentrafficsim.core.network.route.Route;
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 HashMap<>();
57
58
59 private Map<RelativeLane, TimeStampedObject<SpeedLimitProspect>> speedLimitProspect = new HashMap<>();
60
61
62 private final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<LaneChangePossibility>>> legalLaneChangePossibility =
63 new HashMap<>();
64
65
66 private final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<LaneChangePossibility>>> physicalLaneChangePossibility =
67 new HashMap<>();
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)
109 || !this.lanes.equals(getPerception().getGtu().positions(RelativePosition.REFERENCE_POSITION).keySet())
110 || !Objects.equals(this.route, getPerception().getGtu().getStrategicalPlanner().getRoute())
111 || this.cutOff.stream().filter((record) -> !record.isCutOffEnd()).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 for (RelativeLane lane : getCrossSection())
130 {
131 updateSpeedLimitProspect(lane);
132 }
133 for (RelativeLane lane : getCrossSection())
134 {
135 if (!this.infrastructureLaneChangeInfo.containsKey(lane))
136 {
137 updateInfrastructureLaneChangeInfo(lane);
138 updateLegalLaneChangePossibility(lane, LateralDirectionality.LEFT);
139 updateLegalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
140 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.LEFT);
141 updatePhysicalLaneChangePossibility(lane, LateralDirectionality.RIGHT);
142 }
143 }
144 }
145
146
147 @Override
148 public final void updateInfrastructureLaneChangeInfo(final RelativeLane lane) throws GTUException, ParameterException
149 {
150
151 if (this.infrastructureLaneChangeInfo.containsKey(lane)
152 && this.infrastructureLaneChangeInfo.get(lane).getTimestamp().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,
180 new InfrastructureLaneChangeInfo(0, record, front, record.isDeadEnd(), 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 =
190 nextSet.get(laneRecord).left(laneRecord.getLeft(), front, laneRecord.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 =
200 nextSet.get(laneRecord).right(laneRecord.getRight(), front, laneRecord.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 InfrastructureLaneChangeInfo info =
225 new InfrastructureLaneChangeInfo(prev.getRequiredNumberOfLaneChanges(), next, front,
226 next.isDeadEnd(), prev.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 if (s instanceof SinkSensor)
298 {
299 this.anyNextOkCache.put(record, true);
300 return true;
301 }
302 }
303
304 Route currentRoute = getGtu().getStrategicalPlanner().getRoute();
305 try
306 {
307 if (currentRoute != null && currentRoute.destinationNode().equals(record.getToNode()))
308 {
309 this.anyNextOkCache.put(record, true);
310 return true;
311 }
312 }
313 catch (NetworkException exception)
314 {
315 throw new RuntimeException("Could not determine destination node.", exception);
316 }
317
318 if (record.getNext().isEmpty())
319 {
320 this.anyNextOkCache.put(record, false);
321 return false;
322 }
323
324 if (currentRoute == null)
325 {
326 this.anyNextOkCache.put(record, true);
327 return true;
328 }
329
330 ok = record.allowsRouteAtEnd(currentRoute, getGtu().getGTUType());
331 this.anyNextOkCache.put(record, ok);
332 return ok;
333 }
334
335
336 @Override
337 public final void updateSpeedLimitProspect(final RelativeLane lane) throws GTUException, ParameterException
338 {
339 updateCrossSection();
340 checkLaneIsInCrossSection(lane);
341 TimeStampedObject<SpeedLimitProspect> tsSlp = this.speedLimitProspect.get(lane);
342 SpeedLimitProspect slp;
343 if (tsSlp != null)
344 {
345 slp = tsSlp.getObject();
346 slp.update(getGtu().getOdometer());
347 }
348 else
349 {
350 slp = new SpeedLimitProspect(getGtu().getOdometer());
351 slp.addSpeedInfo(Length.ZERO, SpeedLimitTypes.MAX_VEHICLE_SPEED, getGtu().getMaximumSpeed(), getGtu());
352 }
353 try
354 {
355 Lane laneObj = getGtu().getReferencePosition().getLane();
356 if (!slp.containsAddSource(laneObj))
357 {
358 slp.addSpeedInfo(Length.ZERO, SpeedLimitTypes.FIXED_SIGN, laneObj.getSpeedLimit(getGtu().getGTUType()),
359 laneObj);
360 }
361 }
362 catch (NetworkException exception)
363 {
364 throw new RuntimeException("Could not obtain speed limit from lane for perception.", exception);
365 }
366 this.speedLimitProspect.put(lane, new TimeStampedObject<>(slp, getTimestamp()));
367 }
368
369
370 @Override
371 public final void updateLegalLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat)
372 throws GTUException, ParameterException
373 {
374 updateLaneChangePossibility(lane, lat, true, this.legalLaneChangePossibility);
375 }
376
377
378 @Override
379 public final void updatePhysicalLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat)
380 throws GTUException, ParameterException
381 {
382 updateLaneChangePossibility(lane, lat, false, this.physicalLaneChangePossibility);
383 }
384
385
386
387
388
389
390
391
392
393
394
395
396
397 private void updateLaneChangePossibility(final RelativeLane lane, final LateralDirectionality lat, final boolean legal,
398 final Map<RelativeLane, Map<LateralDirectionality, TimeStampedObject<LaneChangePossibility>>> possibilityMap)
399 throws GTUException, ParameterException
400 {
401 updateCrossSection();
402 checkLaneIsInCrossSection(lane);
403
404 if (possibilityMap.get(lane) == null)
405 {
406 possibilityMap.put(lane, new HashMap<>());
407 }
408 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
409
410 Length tail = getPerception().getGtu().getRear().getDx();
411 while (record != null && record.getStartDistance().gt(tail) && !record.getPrev().isEmpty()
412 && ((lat.isLeft() && record.possibleLeft(legal)) || (lat.isRight() && record.possibleRight(legal))))
413 {
414 if (record.getPrev().size() > 1)
415 {
416
417 possibilityMap.get(lane).put(lat, new TimeStampedObject<>(
418 new LaneChangePossibility(record.getPrev().get(0), tail, true), getTimestamp()));
419 return;
420 }
421 else if (record.getPrev().isEmpty())
422 {
423
424 break;
425 }
426 record = record.getPrev().get(0);
427 if ((lat.isLeft() && !record.possibleLeft(legal)) || (lat.isRight() && !record.possibleRight(legal)))
428 {
429
430 possibilityMap.get(lane).put(lat,
431 new TimeStampedObject<>(new LaneChangePossibility(record, tail, true), getTimestamp()));
432 return;
433 }
434 }
435
436 LaneStructureRecord prevRecord = null;
437 record = getPerception().getLaneStructure().getFirstRecord(lane);
438
439 Length dx;
440 if ((lat.isLeft() && record.possibleLeft(legal)) || (lat.isRight() && record.possibleRight(legal)))
441 {
442 dx = getPerception().getGtu().getFront().getDx();
443 while (record != null
444 && ((lat.isLeft() && record.possibleLeft(legal)) || (lat.isRight() && record.possibleRight(legal))))
445 {
446
447 prevRecord = record;
448 record = record.getNext().isEmpty() ? null : record.getNext().get(0);
449 }
450 }
451 else
452 {
453 dx = getPerception().getGtu().getRear().getDx();
454 while (record != null
455 && ((lat.isLeft() && !record.possibleLeft(legal)) || (lat.isRight() && !record.possibleRight(legal))))
456 {
457
458 prevRecord = record;
459 record = record.getNext().isEmpty() ? null : record.getNext().get(0);
460 }
461 }
462 possibilityMap.get(lane).put(lat,
463 new TimeStampedObject<>(new LaneChangePossibility(prevRecord, dx, true), getTimestamp()));
464 }
465
466
467
468
469
470 private void checkLaneIsInCrossSection(final RelativeLane lane) throws GTUException
471 {
472 Throw.when(!getCrossSection().contains(lane), GTUException.class,
473 "The requeasted lane %s is not in the most recent cross section.", lane);
474 }
475
476
477 @Override
478 public final void updateCrossSection() throws GTUException, ParameterException
479 {
480 if (this.crossSection != null && this.crossSection.getTimestamp().equals(getTimestamp()))
481 {
482
483 return;
484 }
485 this.crossSection =
486 new TimeStampedObject<>(getPerception().getLaneStructure().getExtendedCrossSection(), getTimestamp());
487 }
488
489
490 @Override
491 public final SortedSet<InfrastructureLaneChangeInfo> getInfrastructureLaneChangeInfo(final RelativeLane lane)
492 {
493 return this.infrastructureLaneChangeInfo.get(lane).getObject();
494 }
495
496
497 @Override
498 public final SpeedLimitProspect getSpeedLimitProspect(final RelativeLane lane)
499 {
500 return this.speedLimitProspect.get(lane).getObject();
501 }
502
503
504 @Override
505 public final Length getLegalLaneChangePossibility(final RelativeLane fromLane, final LateralDirectionality lat)
506 {
507 return this.legalLaneChangePossibility.get(fromLane).get(lat).getObject().getDistance(lat);
508 }
509
510
511 @Override
512 public final Length getPhysicalLaneChangePossibility(final RelativeLane fromLane, final LateralDirectionality lat)
513 {
514 return this.physicalLaneChangePossibility.get(fromLane).get(lat).getObject().getDistance(lat);
515 }
516
517
518 @Override
519 public final SortedSet<RelativeLane> getCrossSection()
520 {
521 return this.crossSection.getObject();
522 }
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 public final TimeStampedObject<SortedSet<InfrastructureLaneChangeInfo>> getTimeStampedInfrastructureLaneChangeInfo(
548 final RelativeLane lane)
549 {
550 return this.infrastructureLaneChangeInfo.get(lane);
551 }
552
553
554
555
556
557
558 public final TimeStampedObject<SpeedLimitProspect> getTimeStampedSpeedLimitProspect(final RelativeLane lane)
559 {
560 return this.speedLimitProspect.get(lane);
561 }
562
563
564
565
566
567
568
569
570 public final TimeStampedObject<Length> getTimeStampedLegalLaneChangePossibility(final RelativeLane fromLane,
571 final LateralDirectionality lat)
572 {
573 TimeStampedObject<LaneChangePossibility> tsLcp = this.legalLaneChangePossibility.get(fromLane).get(lat);
574 LaneChangePossibility lcp = tsLcp.getObject();
575 return new TimeStampedObject<>(lcp.getDistance(lat), tsLcp.getTimestamp());
576 }
577
578
579
580
581
582
583
584
585 public final TimeStampedObject<Length> getTimeStampedPhysicalLaneChangePossibility(final RelativeLane fromLane,
586 final LateralDirectionality lat)
587 {
588 TimeStampedObject<LaneChangePossibility> tsLcp = this.physicalLaneChangePossibility.get(fromLane).get(lat);
589 LaneChangePossibility lcp = tsLcp.getObject();
590 return new TimeStampedObject<>(lcp.getDistance(lat), tsLcp.getTimestamp());
591 }
592
593
594
595
596
597 public final TimeStampedObject<SortedSet<RelativeLane>> getTimeStampedCrossSection()
598 {
599 return this.crossSection;
600 }
601
602
603 @Override
604 public final String toString()
605 {
606 return "DirectInfrastructurePerception";
607 }
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622 private class LaneChangePossibility
623 {
624
625
626 private final LaneStructureRecord record;
627
628
629 private final double dx;
630
631
632 private final boolean legal;
633
634
635
636
637
638
639 LaneChangePossibility(final LaneStructureRecord record, final Length dx, final boolean legal)
640 {
641 this.record = record;
642 this.dx = dx.si;
643 this.legal = legal;
644 }
645
646
647
648
649
650
651 final Length getDistance(final LateralDirectionality lat)
652 {
653 double d = this.record.getStartDistance().si + this.record.getLane().getLength().si - this.dx;
654 if ((lat.isLeft() && this.record.possibleLeft(this.legal))
655 || (lat.isRight() && this.record.possibleRight(this.legal)))
656 {
657 return Length.createSI(d);
658 }
659 return Length.createSI(-d);
660 }
661
662 }
663
664 }