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