1 package org.opentrafficsim.core.gtu;
2
3 import java.awt.Color;
4 import java.util.LinkedHashSet;
5 import java.util.Set;
6
7 import org.djunits.unit.DurationUnit;
8 import org.djunits.unit.TimeUnit;
9 import org.djunits.value.vdouble.scalar.Acceleration;
10 import org.djunits.value.vdouble.scalar.Duration;
11 import org.djunits.value.vdouble.scalar.Length;
12 import org.djunits.value.vdouble.scalar.Speed;
13 import org.djunits.value.vdouble.scalar.Time;
14 import org.opentrafficsim.base.parameters.ParameterException;
15 import org.opentrafficsim.base.parameters.Parameters;
16 import org.opentrafficsim.core.gtu.animation.IDGTUColorer;
17 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
18 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
19 import org.opentrafficsim.core.gtu.plan.strategical.StrategicalPlanner;
20 import org.opentrafficsim.core.gtu.plan.tactical.TacticalPlanner;
21 import org.opentrafficsim.core.idgenerator.IdGenerator;
22 import org.opentrafficsim.core.network.NetworkException;
23 import org.opentrafficsim.core.perception.Historical;
24 import org.opentrafficsim.core.perception.HistoricalValue;
25 import org.opentrafficsim.core.perception.HistoryManager;
26 import org.opentrafficsim.core.perception.PerceivableContext;
27
28 import nl.tudelft.simulation.dsol.SimRuntimeException;
29 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEvent;
30 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
31 import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;
32 import nl.tudelft.simulation.event.EventProducer;
33 import nl.tudelft.simulation.language.Throw;
34 import nl.tudelft.simulation.language.d3.DirectedPoint;
35
36
37
38
39
40
41
42
43
44
45
46
47 public abstract class AbstractGTU extends EventProducer implements GTU
48 {
49
50 private static final long serialVersionUID = 20140822L;
51
52
53 private final String id;
54
55
56 private final int uniqueNumber;
57
58
59 private static int staticUNIQUENUMBER = 0;
60
61
62 private final GTUType gtuType;
63
64
65 private final DEVSSimulatorInterface.TimeDoubleUnit simulator;
66
67
68 private Parameters parameters;
69
70
71 private Acceleration maximumAcceleration;
72
73
74 private Acceleration maximumDeceleration;
75
76
77
78
79
80
81 private Historical<Length> odometer;
82
83
84 private final Historical<StrategicalPlanner> strategicalPlanner;
85
86
87 private final Historical<TacticalPlanner<?, ?>> tacticalPlanner;
88
89
90 protected final Historical<OperationalPlan> operationalPlan;
91
92
93 private SimEvent<SimTimeDoubleUnit> nextMoveEvent;
94
95
96 private PerceivableContext perceivableContext;
97
98
99 private boolean destroyed = false;
100
101
102 private Color baseColor = null;
103
104
105
106 public static boolean ALIGNED = true;
107
108
109
110 public static int ALIGN_COUNT = 0;
111
112
113 private double cachedSpeedTime = Double.NaN;
114
115
116 private Speed cachedSpeed = null;
117
118
119 private double cachedAccelerationTime = Double.NaN;
120
121
122 private Acceleration cachedAcceleration = null;
123
124
125 private GTU parent = null;
126
127
128 private Set<GTU> children = new LinkedHashSet<>();
129
130
131
132
133
134
135
136
137 @SuppressWarnings("checkstyle:parameternumber")
138 public AbstractGTU(final String id, final GTUType gtuType, final DEVSSimulatorInterface.TimeDoubleUnit simulator,
139 final PerceivableContext perceivableContext) throws GTUException
140 {
141 Throw.when(id == null, GTUException.class, "id is null");
142 Throw.when(gtuType == null, GTUException.class, "gtuType is null");
143 Throw.when(perceivableContext == null, GTUException.class, "perceivableContext is null for GTU with id %s", id);
144 Throw.when(perceivableContext.containsGtuId(id), GTUException.class,
145 "GTU with id %s already registered in perceivableContext %s", id, perceivableContext.getId());
146 Throw.when(simulator == null, GTUException.class, "simulator is null for GTU with id %s", id);
147
148 this.id = id;
149 this.uniqueNumber = ++staticUNIQUENUMBER;
150 this.gtuType = gtuType;
151 this.simulator = simulator;
152 this.odometer = new HistoricalValue<>(HistoryManager.get(simulator), Length.ZERO);
153 this.perceivableContext = perceivableContext;
154 this.perceivableContext.addGTU(this);
155 this.strategicalPlanner = new HistoricalValue<>(HistoryManager.get(simulator));
156 this.tacticalPlanner = new HistoricalValue<>(HistoryManager.get(simulator), null);
157 this.operationalPlan = new HistoricalValue<>(HistoryManager.get(simulator), null);
158 }
159
160
161
162
163
164
165
166
167 @SuppressWarnings("checkstyle:parameternumber")
168 public AbstractGTU(final IdGenerator idGenerator, final GTUType gtuType,
169 final DEVSSimulatorInterface.TimeDoubleUnit simulator, final PerceivableContext perceivableContext)
170 throws GTUException
171 {
172 this(generateId(idGenerator), gtuType, simulator, perceivableContext);
173 }
174
175
176
177
178
179
180
181
182
183
184
185 @SuppressWarnings({ "checkstyle:hiddenfield", "hiding", "checkstyle:designforextension" })
186 public void init(final StrategicalPlanner strategicalPlanner, final DirectedPoint initialLocation, final Speed initialSpeed)
187 throws SimRuntimeException, GTUException
188 {
189 Throw.when(strategicalPlanner == null, GTUException.class, "strategicalPlanner is null for GTU with id %s", this.id);
190 Throw.whenNull(initialLocation, "Initial location of GTU cannot be null");
191 Throw.when(Double.isNaN(initialLocation.x) || Double.isNaN(initialLocation.y) || Double.isNaN(initialLocation.z),
192 GTUException.class, "initialLocation %s invalid for GTU with id %s", initialLocation, this.id);
193 Throw.when(initialSpeed == null, GTUException.class, "initialSpeed is null for GTU with id %s", this.id);
194 Throw.when(!getId().equals(strategicalPlanner.getGtu().getId()), GTUException.class,
195 "GTU %s is initialized with a strategical planner for GTU %s", getId(), strategicalPlanner.getGtu().getId());
196
197 this.strategicalPlanner.set(strategicalPlanner);
198 this.tacticalPlanner.set(strategicalPlanner.getTacticalPlanner());
199 Time now = this.simulator.getSimulatorTime();
200
201 fireTimedEvent(GTU.INIT_EVENT, new Object[] { getId(), initialLocation, getLength(), getWidth(), getBaseColor() }, now);
202
203 try
204 {
205 move(initialLocation);
206 }
207 catch (OperationalPlanException | NetworkException | ParameterException exception)
208 {
209 throw new GTUException("Failed to create OperationalPlan for GTU " + this.id, exception);
210 }
211 }
212
213
214
215
216
217
218
219 private static String generateId(final IdGenerator idGenerator) throws GTUException
220 {
221 Throw.when(idGenerator == null, GTUException.class, "AbstractGTU.<init>: idGenerator is null");
222 return idGenerator.nextId();
223 }
224
225
226
227
228 @Override
229 @SuppressWarnings("checkstyle:designforextension")
230 public void destroy()
231 {
232 fireTimedEvent(GTU.DESTROY_EVENT, new Object[] { getId(), getLocation(), getOdometer() },
233 this.simulator.getSimulatorTime());
234
235
236 if (this.nextMoveEvent != null)
237 {
238 this.simulator.cancelEvent(this.nextMoveEvent);
239 this.nextMoveEvent = null;
240 }
241
242 this.perceivableContext.removeGTU(this);
243 this.destroyed = true;
244 }
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259 @SuppressWarnings("checkstyle:designforextension")
260 protected void move(final DirectedPoint fromLocation)
261 throws SimRuntimeException, OperationalPlanException, GTUException, NetworkException, ParameterException
262 {
263 Time now = this.simulator.getSimulatorTime();
264
265
266
267 Length currentOdometer;
268 if (this.operationalPlan.get() != null)
269 {
270 currentOdometer = this.odometer.get().plus(this.operationalPlan.get().getTraveledDistance(now));
271 }
272 else
273 {
274 currentOdometer = this.odometer.get();
275 }
276
277
278
279 TacticalPlanner<?, ?> tactPlanner = this.tacticalPlanner.get();
280 if (tactPlanner == null)
281 {
282
283 tactPlanner = this.strategicalPlanner.get().getTacticalPlanner();
284 this.tacticalPlanner.set(tactPlanner);
285 }
286 OperationalPlan newOperationalPlan = tactPlanner.generateOperationalPlan(now, fromLocation);
287 this.operationalPlan.set(newOperationalPlan);
288 this.cachedSpeedTime = Double.NaN;
289 this.cachedAccelerationTime = Double.NaN;
290 this.odometer.set(currentOdometer);
291 if (getOperationalPlan().getAcceleration(Duration.ZERO).si < -10
292 && getOperationalPlan().getSpeed(Duration.ZERO).si > 2.5)
293 {
294 System.err.println("(getOperationalPlan().getAcceleration(Duration.ZERO).si < -10)");
295
296 }
297
298
299 if (ALIGNED && newOperationalPlan.getTotalDuration().si == 0.5)
300 {
301
302
303 double tNext = Math.floor(2.0 * now.si + 1.0) / 2.0;
304 DirectedPoint p = (tNext - now.si < 0.5) ? newOperationalPlan.getEndLocation()
305 : newOperationalPlan.getLocation(new Duration(tNext - now.si, DurationUnit.SI));
306 this.nextMoveEvent = new SimEvent<>(new SimTimeDoubleUnit(new Time(tNext, TimeUnit.BASE)), this, this, "move",
307 new Object[] { p });
308 ALIGN_COUNT++;
309 }
310 else
311 {
312
313
314 this.nextMoveEvent = new SimEvent<>(new SimTimeDoubleUnit(now.plus(newOperationalPlan.getTotalDuration())), this,
315 this, "move", new Object[] { newOperationalPlan.getEndLocation() });
316 }
317 this.simulator.scheduleEvent(this.nextMoveEvent);
318
319 fireTimedEvent(GTU.MOVE_EVENT, new Object[] { getId(), fromLocation, getSpeed(), getAcceleration(), getOdometer() },
320 this.simulator.getSimulatorTime());
321 }
322
323
324
325
326
327
328
329
330
331
332
333 @SuppressWarnings("checkstyle:designforextension")
334 protected void interruptMove()
335 throws SimRuntimeException, OperationalPlanException, GTUException, NetworkException, ParameterException
336 {
337 this.simulator.cancelEvent(this.nextMoveEvent);
338 move(this.operationalPlan.get().getLocation(this.simulator.getSimulatorTime()));
339 }
340
341
342 @Override
343 public final String getId()
344 {
345 return this.id;
346 }
347
348
349 @SuppressWarnings("checkstyle:designforextension")
350 @Override
351 public GTUType getGTUType()
352 {
353 return this.gtuType;
354 }
355
356
357 @Override
358 public final RelativePosition getReference()
359 {
360 return RelativePosition.REFERENCE_POSITION;
361 }
362
363
364 @Override
365 public final DEVSSimulatorInterface.TimeDoubleUnit getSimulator()
366 {
367 return this.simulator;
368 }
369
370
371 @Override
372 public final Parameters getParameters()
373 {
374 return this.parameters;
375 }
376
377
378 @Override
379 public final void setParameters(final Parameters parameters)
380 {
381 this.parameters = parameters;
382 }
383
384
385 @Override
386 public StrategicalPlanner getStrategicalPlanner()
387 {
388 return this.strategicalPlanner.get();
389 }
390
391
392 @Override
393 public StrategicalPlanner getStrategicalPlanner(final Time time)
394 {
395 return this.strategicalPlanner.get(time);
396 }
397
398
399 @Override
400 public final OperationalPlan getOperationalPlan()
401 {
402 return this.operationalPlan.get();
403 }
404
405
406 @Override
407 public final OperationalPlan getOperationalPlan(final Time time)
408 {
409 return this.operationalPlan.get(time);
410 }
411
412
413 @Override
414 public final Length getOdometer()
415 {
416 return getOdometer(this.simulator.getSimulatorTime());
417 }
418
419
420 @Override
421 public final Length getOdometer(final Time time)
422 {
423 if (getOperationalPlan(time) == null)
424 {
425 return this.odometer.get(time);
426 }
427 try
428 {
429 return this.odometer.get(time).plus(getOperationalPlan(time).getTraveledDistance(time));
430 }
431 catch (@SuppressWarnings("unused") OperationalPlanException ope)
432 {
433 return this.odometer.get(time);
434 }
435 }
436
437
438 @Override
439 public final Speed getSpeed()
440 {
441 return getSpeed(this.simulator.getSimulatorTime());
442 }
443
444
445 @Override
446 public final Speed getSpeed(final Time time)
447 {
448 if (this.cachedSpeedTime != time.si)
449 {
450 this.cachedSpeedTime = time.si;
451 OperationalPlan plan = getOperationalPlan(time);
452 if (plan == null)
453 {
454 this.cachedSpeed = Speed.ZERO;
455 }
456 else if (time.si < plan.getStartTime().si)
457 {
458 this.cachedSpeed = plan.getStartSpeed();
459 }
460 else if (time.si > plan.getEndTime().si)
461 {
462 throw new IllegalStateException("Requesting speed value beyond plan.");
463 }
464 else
465 {
466 this.cachedSpeed = Try.assign(() -> plan.getSpeed(time),
467 "getSpeed() could not derive a valid speed for the current operationalPlan");
468 }
469 }
470 return this.cachedSpeed;
471 }
472
473
474 @Override
475 public final Acceleration getAcceleration()
476 {
477 return getAcceleration(this.simulator.getSimulatorTime());
478 }
479
480
481 @Override
482 public final Acceleration getAcceleration(final Time time)
483 {
484 if (this.cachedAccelerationTime != time.si)
485 {
486 this.cachedAccelerationTime = time.si;
487 OperationalPlan plan = getOperationalPlan(time);
488 if (plan == null)
489 {
490 this.cachedAcceleration = Acceleration.ZERO;
491 }
492 else if (time.si < plan.getStartTime().si)
493 {
494 this.cachedAcceleration =
495 Try.assign(() -> plan.getAcceleration(plan.getStartTime()), "Exception obtaining acceleration.");
496 }
497 else if (time.si > plan.getEndTime().si)
498 {
499 throw new IllegalStateException("Requesting acceleration value beyond plan.");
500 }
501 else
502 {
503 this.cachedAcceleration = Try.assign(() -> plan.getAcceleration(time),
504 "getAcceleration() could not derive a valid acceleration for the current operationalPlan");
505 }
506 }
507 return this.cachedAcceleration;
508 }
509
510
511
512
513 @Override
514 public final Acceleration getMaximumAcceleration()
515 {
516 return this.maximumAcceleration;
517 }
518
519
520
521
522 public final void setMaximumAcceleration(final Acceleration maximumAcceleration)
523 {
524 if (maximumAcceleration.le(Acceleration.ZERO))
525 {
526 throw new RuntimeException("Maximum acceleration of GTU " + this.id + " set to value <= 0");
527 }
528 this.maximumAcceleration = maximumAcceleration;
529 }
530
531
532
533
534 @Override
535 public final Acceleration getMaximumDeceleration()
536 {
537 return this.maximumDeceleration;
538 }
539
540
541
542
543 public final void setMaximumDeceleration(final Acceleration maximumDeceleration)
544 {
545 if (maximumDeceleration.ge(Acceleration.ZERO))
546 {
547 throw new RuntimeException("Maximum deceleration of GTU " + this.id + " set to value >= 0");
548 }
549 this.maximumDeceleration = maximumDeceleration;
550 }
551
552
553 private Time cacheLocationTime = new Time(Double.NaN, TimeUnit.BASE);
554
555
556 private DirectedPoint cacheLocation = null;
557
558
559 @Override
560 @SuppressWarnings("checkstyle:designforextension")
561 public DirectedPoint getLocation()
562 {
563 if (this.operationalPlan.get() == null)
564 {
565 System.err.println(
566 "No operational plan for GTU " + this.id + " at t=" + this.getSimulator().getSimulatorTime());
567 return new DirectedPoint(0, 0, 0);
568 }
569 try
570 {
571
572 if (this.cacheLocationTime.si != this.simulator.getSimulatorTime().si)
573 {
574 this.cacheLocationTime = this.simulator.getSimulatorTime();
575 this.cacheLocation = this.operationalPlan.get().getLocation(this.cacheLocationTime);
576 }
577 return this.cacheLocation;
578 }
579 catch (@SuppressWarnings("unused") OperationalPlanException exception)
580 {
581 return new DirectedPoint(0, 0, 0);
582 }
583 }
584
585
586 @Override
587 @SuppressWarnings("checkstyle:designforextension")
588 public Color getBaseColor()
589 {
590 if (this.baseColor == null)
591 {
592 this.baseColor = IDGTUColorer.LEGEND.get(this.uniqueNumber % IDGTUColorer.LEGEND.size()).getColor();
593 }
594 return this.baseColor;
595 }
596
597
598
599
600 public final boolean isDestroyed()
601 {
602 return this.destroyed;
603 }
604
605
606
607
608 public final PerceivableContext getPerceivableContext()
609 {
610 return this.perceivableContext;
611 }
612
613
614 @Override
615 public void addGtu(final GTU gtu) throws GTUException
616 {
617 this.children.add(gtu);
618 gtu.setParent(this);
619 }
620
621
622 @Override
623 public void removeGtu(final GTU gtu)
624 {
625 this.children.remove(gtu);
626 try
627 {
628 gtu.setParent(null);
629 }
630 catch (@SuppressWarnings("unused") GTUException exception)
631 {
632
633 }
634 }
635
636
637 @Override
638 public void setParent(final GTU gtu) throws GTUException
639 {
640 Throw.when(gtu != null && this.parent != null, GTUException.class, "GTU %s already has a parent.", this);
641 this.parent = gtu;
642 }
643
644
645 @Override
646 public GTU getParent()
647 {
648 return this.parent;
649 }
650
651
652 @Override
653 public Set<GTU> getChildren()
654 {
655 return new LinkedHashSet<>(this.children);
656 }
657
658
659 @Override
660 @SuppressWarnings("designforextension")
661 public int hashCode()
662 {
663 final int prime = 31;
664 int result = 1;
665 result = prime * result + ((this.id == null) ? 0 : this.id.hashCode());
666 result = prime * result + this.uniqueNumber;
667 return result;
668 }
669
670
671 @Override
672 @SuppressWarnings({ "designforextension", "needbraces" })
673 public boolean equals(final Object obj)
674 {
675 if (this == obj)
676 return true;
677 if (obj == null)
678 return false;
679 if (getClass() != obj.getClass())
680 return false;
681 AbstractGTU other = (AbstractGTU) obj;
682 if (this.id == null)
683 {
684 if (other.id != null)
685 return false;
686 }
687 else if (!this.id.equals(other.id))
688 return false;
689 if (this.uniqueNumber != other.uniqueNumber)
690 return false;
691 return true;
692 }
693
694 }