1 package org.opentrafficsim.road.network.lane.object.sensor;
2
3 import java.rmi.RemoteException;
4 import java.util.ArrayList;
5 import java.util.HashSet;
6 import java.util.List;
7 import java.util.Map;
8 import java.util.Set;
9
10 import javax.media.j3d.Bounds;
11
12 import org.djunits.value.vdouble.scalar.Length;
13 import org.djutils.exceptions.Throw;
14 import org.opentrafficsim.core.compatibility.Compatible;
15 import org.opentrafficsim.core.geometry.OTSGeometryException;
16 import org.opentrafficsim.core.geometry.OTSLine3D;
17 import org.opentrafficsim.core.geometry.OTSPoint3D;
18 import org.opentrafficsim.core.gtu.GTUDirectionality;
19 import org.opentrafficsim.core.gtu.GTUException;
20 import org.opentrafficsim.core.gtu.RelativePosition.TYPE;
21 import org.opentrafficsim.core.network.NetworkException;
22 import org.opentrafficsim.core.network.Node;
23 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
24 import org.opentrafficsim.road.network.lane.CrossSectionElement;
25 import org.opentrafficsim.road.network.lane.Lane;
26
27 import nl.tudelft.simulation.dsol.animation.Locatable;
28 import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;
29 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
30 import nl.tudelft.simulation.event.EventInterface;
31 import nl.tudelft.simulation.event.EventListenerInterface;
32 import nl.tudelft.simulation.event.EventProducer;
33 import nl.tudelft.simulation.event.EventProducerInterface;
34 import nl.tudelft.simulation.language.d3.DirectedPoint;
35
36
37
38
39
40
41
42
43
44
45
46
47
48 public class TrafficLightSensor extends EventProducer
49 implements EventListenerInterface, NonDirectionalOccupancySensor, EventProducerInterface, Locatable, Sensor
50 {
51
52 private static final long serialVersionUID = 20161103L;
53
54
55 private final String id;
56
57
58 private final FlankSensor entryA;
59
60
61 private final FlankSensor exitA;
62
63
64 private final FlankSensor entryB;
65
66
67 private final FlankSensor exitB;
68
69
70 private final Set<LaneBasedGTU> currentGTUs = new HashSet<>();
71
72
73 private final Set<Lane> lanes = new HashSet<>();
74
75
76 private final GTUDirectionality directionalityA;
77
78
79 private final GTUDirectionality directionalityB;
80
81
82 private final OTSLine3D path;
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99 @SuppressWarnings("checkstyle:parameternumber")
100 public TrafficLightSensor(final String id, final Lane laneA, final Length positionA, final Lane laneB,
101 final Length positionB, final List<Lane> intermediateLanes, final TYPE entryPosition, final TYPE exitPosition,
102 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final Compatible compatible) throws NetworkException
103 {
104 Throw.whenNull(id, "id may not be null");
105 this.id = id;
106 this.entryA = new FlankSensor(id + ".entryA", laneA, positionA, entryPosition, simulator, this, compatible);
107 this.exitA = new FlankSensor(id + ".exitA", laneA, positionA, exitPosition, simulator, this, compatible);
108 this.entryB = new FlankSensor(id + ".entryB", laneB, positionB, entryPosition, simulator, this, compatible);
109 this.exitB = new FlankSensor(id + ".exitB", laneB, positionB, exitPosition, simulator, this, compatible);
110
111
112 this.lanes.add(laneA);
113 if (null != intermediateLanes)
114 {
115 this.lanes.addAll(intermediateLanes);
116 }
117 this.lanes.add(laneB);
118 for (Lane lane : this.lanes)
119 {
120 lane.addListener(this, Lane.GTU_ADD_EVENT);
121 lane.addListener(this, Lane.GTU_REMOVE_EVENT);
122 }
123 if (laneA.equals(laneB))
124 {
125 this.directionalityA = positionA.le(positionB) ? GTUDirectionality.DIR_PLUS : GTUDirectionality.DIR_MINUS;
126 this.directionalityB = this.directionalityA;
127 }
128 else
129 {
130 this.directionalityA = findDirectionality(laneA, intermediateLanes);
131 this.directionalityB = GTUDirectionality.DIR_PLUS == findDirectionality(laneB, intermediateLanes)
132 ? GTUDirectionality.DIR_MINUS : GTUDirectionality.DIR_PLUS;
133
134 }
135 List<OTSPoint3D> outLine = new ArrayList<>();
136 outLine.add(fixElevation(this.entryA.getGeometry().getCentroid()));
137 if (null != intermediateLanes && intermediateLanes.size() > 0)
138 {
139 Lane prevLane = laneA;
140 List<Lane> remainingLanes = new ArrayList<>();
141 remainingLanes.addAll(intermediateLanes);
142 remainingLanes.add(laneB);
143 while (remainingLanes.size() > 0)
144 {
145 Lane continuingLane = null;
146 Node node = prevLane.getParentLink().getEndNode();
147 for (Lane nextLane : intermediateLanes)
148 {
149 if (nextLane.getParentLink().getStartNode().equals(node))
150 {
151 continuingLane = nextLane;
152 outLine.add(fixElevation(nextLane.getCenterLine().getFirst()));
153 break;
154 }
155 else if (nextLane.getParentLink().getEndNode().equals(node))
156 {
157 continuingLane = nextLane;
158 outLine.add(fixElevation(nextLane.getCenterLine().getLast()));
159 break;
160 }
161 }
162 if (null == continuingLane)
163 {
164 throw new NetworkException("Cannot find route from laneA to laneB using the provided intermediateLanes");
165 }
166 remainingLanes.remove(continuingLane);
167 }
168 }
169 outLine.add(fixElevation(this.exitB.getGeometry().getCentroid()));
170 try
171 {
172 this.path = OTSLine3D.createAndCleanOTSLine3D(outLine);
173 }
174 catch (OTSGeometryException exception)
175 {
176
177 throw new NetworkException(exception);
178 }
179 }
180
181
182
183
184
185
186 private OTSPoint3D fixElevation(final OTSPoint3D point)
187 {
188 return new OTSPoint3D(point.x, point.y, point.z + SingleSensor.DEFAULT_SENSOR_ELEVATION.si);
189 }
190
191
192
193
194
195
196
197
198
199 private GTUDirectionality findDirectionality(final Lane lane, final List<Lane> intermediateLanes) throws NetworkException
200 {
201 Node startNode = lane.getParentLink().getStartNode();
202 Node endNode = lane.getParentLink().getEndNode();
203 for (Lane otherLane : intermediateLanes)
204 {
205 if (lane.equals(otherLane))
206 {
207 continue;
208 }
209 Node intermediateNode = otherLane.getParentLink().getStartNode();
210 if (intermediateNode == startNode)
211 {
212 return GTUDirectionality.DIR_MINUS;
213 }
214 if (intermediateNode == endNode)
215 {
216 return GTUDirectionality.DIR_PLUS;
217 }
218 intermediateNode = otherLane.getParentLink().getEndNode();
219 if (intermediateNode == startNode)
220 {
221 return GTUDirectionality.DIR_MINUS;
222 }
223 if (intermediateNode == endNode)
224 {
225 return GTUDirectionality.DIR_PLUS;
226 }
227 }
228 throw new NetworkException("lane " + lane + " is not connected to any intermediate lane or the other lane");
229 }
230
231
232
233
234
235 protected final void addGTU(final LaneBasedGTU gtu)
236 {
237 if (this.currentGTUs.add(gtu) && this.currentGTUs.size() == 1)
238 {
239 fireTimedEvent(NonDirectionalOccupancySensor.NON_DIRECTIONAL_OCCUPANCY_SENSOR_TRIGGER_ENTRY_EVENT,
240 new Object[] { getId() }, getSimulator().getSimulatorTime());
241 }
242 }
243
244
245
246
247
248 protected final void removeGTU(final LaneBasedGTU gtu)
249 {
250 if (this.currentGTUs.remove(gtu) && this.currentGTUs.size() == 0)
251 {
252 fireTimedEvent(NonDirectionalOccupancySensor.NON_DIRECTIONAL_OCCUPANCY_SENSOR_TRIGGER_EXIT_EVENT,
253 new Object[] { getId() }, getSimulator().getSimulatorTime());
254 }
255 }
256
257
258 @Override
259 public final void notify(final EventInterface event) throws RemoteException
260 {
261
262 LaneBasedGTU gtu = (LaneBasedGTU) ((Object[]) event.getContent())[1];
263 if (Lane.GTU_REMOVE_EVENT.equals(event.getType()))
264 {
265 if (!this.currentGTUs.contains(gtu))
266 {
267 return;
268 }
269 try
270 {
271 Map<Lane, Length> frontPositions = gtu.positions(gtu.getRelativePositions().get(this.entryA.getPositionType()));
272 Set<Lane> remainingLanes = new HashSet<>(frontPositions.keySet());
273 remainingLanes.retainAll(this.lanes);
274 if (remainingLanes.size() == 0)
275 {
276 removeGTU(gtu);
277 }
278
279
280 return;
281 }
282 catch (GTUException exception)
283 {
284 System.err.println("Caught GTU exception trying to get the frontPositions");
285 exception.printStackTrace();
286 }
287 }
288 else if (Lane.GTU_ADD_EVENT.equals(event.getType()))
289 {
290 if (this.currentGTUs.contains(gtu))
291 {
292 return;
293 }
294
295 try
296 {
297 Map<Lane, Length> frontPositions = gtu.positions(gtu.getRelativePositions().get(this.entryA.getPositionType()));
298 Set<Lane> remainingLanes = new HashSet<>(frontPositions.keySet());
299 remainingLanes.retainAll(this.lanes);
300 if (remainingLanes.size() == 0)
301 {
302 System.err.println("GTU is not in any or our lanes - CANNOT HAPPEN");
303 }
304 Map<Lane, Length> rearPositions = gtu.positions(gtu.getRelativePositions().get(this.exitA.getPositionType()));
305 for (Lane remainingLane : remainingLanes)
306 {
307 Length frontPosition = frontPositions.get(remainingLane);
308 Length rearPosition = rearPositions.get(remainingLane);
309 Length laneLength = remainingLane.getLength();
310
311
312
313 if (frontPosition.lt0() && rearPosition.lt0()
314 || frontPosition.gt(laneLength) && rearPosition.gt(laneLength))
315 {
316 continue;
317 }
318
319 if (this.entryA.getLane() != remainingLane && this.entryB.getLane() != remainingLane)
320 {
321
322 addGTU(gtu);
323 return;
324 }
325
326 GTUDirectionality drivingDirection = gtu.getDirection(remainingLane);
327 if (this.entryA.getLane().equals(this.entryB.getLane()))
328 {
329
330
331 if (GTUDirectionality.DIR_PLUS == drivingDirection)
332 {
333
334 if (this.directionalityA == GTUDirectionality.DIR_PLUS)
335 {
336 if (frontPosition.ge(this.entryA.getLongitudinalPosition())
337 && rearPosition.lt(this.exitB.getLongitudinalPosition()))
338 {
339 addGTU(gtu);
340 return;
341 }
342 }
343 else
344 {
345 if (frontPosition.le(this.entryB.getLongitudinalPosition())
346 && rearPosition.gt(this.exitA.getLongitudinalPosition()))
347 {
348 addGTU(gtu);
349 return;
350 }
351 }
352 }
353 else
354 {
355
356 if (this.directionalityA == GTUDirectionality.DIR_MINUS)
357 {
358 if (frontPosition.le(this.entryB.getLongitudinalPosition())
359 && rearPosition.gt(this.entryA.getLongitudinalPosition()))
360 {
361 addGTU(gtu);
362 return;
363 }
364 }
365 else
366 {
367 if (frontPosition.le(this.entryB.getLongitudinalPosition())
368 && rearPosition.gt(this.exitA.getLongitudinalPosition()))
369 {
370 addGTU(gtu);
371 return;
372 }
373 }
374 }
375 }
376 else
377
378 {
379 Length detectionPosition;
380 GTUDirectionality detectorDirectionality;
381 if (this.entryA.getLane() == remainingLane)
382 {
383 detectionPosition = this.entryA.getLongitudinalPosition();
384 detectorDirectionality = this.directionalityA;
385 }
386 else
387 {
388 detectionPosition = this.entryB.getLongitudinalPosition();
389 detectorDirectionality = this.directionalityB;
390 }
391 if (GTUDirectionality.DIR_PLUS == drivingDirection)
392 {
393 if (GTUDirectionality.DIR_PLUS == detectorDirectionality)
394 {
395 if (frontPosition.ge(detectionPosition))
396 {
397 addGTU(gtu);
398 return;
399 }
400 }
401 else
402 {
403 if (rearPosition.le(detectionPosition))
404 {
405 addGTU(gtu);
406 return;
407 }
408 }
409 }
410 else
411 {
412
413 if (GTUDirectionality.DIR_PLUS == detectorDirectionality)
414 {
415 if (rearPosition.ge(detectionPosition))
416 {
417 addGTU(gtu);
418 return;
419 }
420 }
421 else
422 {
423 if (frontPosition.le(detectionPosition))
424 {
425 addGTU(gtu);
426 return;
427 }
428 }
429 }
430 }
431 }
432 return;
433 }
434 catch (GTUException exception)
435 {
436 System.err.println("Caught GTU exception trying to get the frontPositions");
437 exception.printStackTrace();
438 }
439 }
440 else
441 {
442 System.err.println("Unexpected event: " + event);
443 }
444 }
445
446
447 @Override
448 public final TYPE getPositionTypeEntry()
449 {
450 return this.entryA.getPositionType();
451 }
452
453
454 @Override
455 public final TYPE getPositionTypeExit()
456 {
457 return this.exitA.getPositionType();
458 }
459
460
461 @Override
462 public final Length getLanePositionA()
463 {
464 return this.entryA.getLongitudinalPosition();
465 }
466
467
468 @Override
469 public final Length getLanePositionB()
470 {
471 return this.entryB.getLongitudinalPosition();
472 }
473
474
475
476
477
478
479 final void signalDetection(final FlankSensor sensor, final LaneBasedGTU gtu)
480 {
481 GTUDirectionality gtuDirection = null;
482 try
483 {
484 gtuDirection = gtu.getDirection(sensor.getLane());
485 }
486 catch (GTUException exception)
487 {
488 exception.printStackTrace();
489 }
490
491
492
493
494
495 if (this.entryA == sensor && gtuDirection == this.directionalityA
496 || this.entryB == sensor && gtuDirection != this.directionalityB)
497 {
498 addGTU(gtu);
499 }
500 else if (this.exitA == sensor && gtuDirection != this.directionalityA
501 || this.exitB == sensor && gtuDirection == this.directionalityB)
502
503 {
504 removeGTU(gtu);
505 }
506
507
508
509
510 }
511
512
513 @Override
514 public final String getId()
515 {
516 return this.id;
517 }
518
519
520 @Override
521 public final DEVSSimulatorInterface.TimeDoubleUnit getSimulator()
522 {
523 return this.entryA.getSimulator();
524 }
525
526
527 @Override
528 public final DirectedPoint getLocation() throws RemoteException
529 {
530 return this.path.getLocation();
531 }
532
533
534 @Override
535 public final Bounds getBounds() throws RemoteException
536 {
537 return this.path.getBounds();
538 }
539
540
541
542
543
544 public final OTSLine3D getPath()
545 {
546 return this.path;
547 }
548
549
550
551
552
553 public final boolean getOccupancy()
554 {
555 return this.currentGTUs.size() > 0;
556 }
557
558
559 @Override
560 public final String toString()
561 {
562 return "TrafficLightSensor [id=" + this.id + ", entryA=" + this.entryA + ", exitA=" + this.exitA + ", entryB="
563 + this.entryB + ", exitB=" + this.exitB + ", currentGTUs=" + this.currentGTUs + ", lanes=" + this.lanes
564 + ", directionalityA=" + this.directionalityA + ", directionalityB=" + this.directionalityB + ", path="
565 + this.path + "]";
566 }
567
568 }
569
570
571
572
573 class FlankSensor extends AbstractSensor
574 {
575
576 private static final long serialVersionUID = 20161104L;
577
578
579 private final TrafficLightSensor parent;
580
581
582
583
584
585
586
587
588
589
590
591
592 FlankSensor(final String id, final Lane lane, final Length longitudinalPosition, final TYPE positionType,
593 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final TrafficLightSensor parent, final Compatible compatible)
594 throws NetworkException
595 {
596 super(id, lane, longitudinalPosition, positionType, simulator, compatible);
597 this.parent = parent;
598 }
599
600
601 @Override
602 protected void triggerResponse(final LaneBasedGTU gtu)
603 {
604 this.parent.signalDetection(this, gtu);
605 }
606
607
608 @Override
609 public FlankSensor clone(final CrossSectionElement newCSE, final SimulatorInterface.TimeDoubleUnit newSimulator)
610 throws NetworkException
611 {
612 Throw.when(!(newCSE instanceof Lane), NetworkException.class, "sensors can only be cloned for Lanes");
613 Throw.when(!(newSimulator instanceof DEVSSimulatorInterface.TimeDoubleUnit), NetworkException.class,
614 "simulator should be a DEVSSimulator");
615
616
617 return new FlankSensor(getId(), (Lane) newCSE, getLongitudinalPosition(), getPositionType(),
618 (DEVSSimulatorInterface.TimeDoubleUnit) newSimulator, this.parent, super.getDetectedGTUTypes());
619 }
620
621
622 @Override
623 public final String toString()
624 {
625 return "FlankSensor [parent=" + this.parent.getId() + "]";
626 }
627
628 }