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