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