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