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 private GTUErrorHandler errorHandler = GTUErrorHandler.THROW;
129
130
131
132
133
134
135
136
137 @SuppressWarnings("checkstyle:parameternumber")
138 public AbstractGTU(final String id, final GTUType gtuType, final OTSSimulatorInterface 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 HistoryManager historyManager = simulator.getReplication().getHistoryManager(simulator);
149 this.id = id;
150 this.uniqueNumber = ++staticUNIQUENUMBER;
151 this.gtuType = gtuType;
152 this.simulator = simulator;
153 this.odometer = new HistoricalValue<>(historyManager, Length.ZERO);
154 this.perceivableContext = perceivableContext;
155 this.perceivableContext.addGTU(this);
156 this.strategicalPlanner = new HistoricalValue<>(historyManager);
157 this.tacticalPlanner = new HistoricalValue<>(historyManager, null);
158 this.operationalPlan = new HistoricalValue<>(historyManager, null);
159 }
160
161
162
163
164
165
166
167
168 @SuppressWarnings("checkstyle:parameternumber")
169 public AbstractGTU(final IdGenerator idGenerator, final GTUType gtuType, final OTSSimulatorInterface simulator,
170 final PerceivableContext perceivableContext) 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() }, 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
260
261 @SuppressWarnings("checkstyle:designforextension")
262 protected boolean move(final DirectedPoint fromLocation)
263 throws SimRuntimeException, OperationalPlanException, GTUException, NetworkException, ParameterException
264 {
265 try
266 {
267 Time now = this.simulator.getSimulatorTime();
268
269
270
271 Length currentOdometer;
272 if (this.operationalPlan.get() != null)
273 {
274 currentOdometer = this.odometer.get().plus(this.operationalPlan.get().getTraveledDistance(now));
275 }
276 else
277 {
278 currentOdometer = this.odometer.get();
279 }
280
281
282
283 TacticalPlanner<?, ?> tactPlanner = this.tacticalPlanner.get();
284 if (tactPlanner == null)
285 {
286
287 tactPlanner = this.strategicalPlanner.get().getTacticalPlanner();
288 this.tacticalPlanner.set(tactPlanner);
289 }
290 tactPlanner.getPerception().perceive();
291 OperationalPlan newOperationalPlan = tactPlanner.generateOperationalPlan(now, fromLocation);
292 this.operationalPlan.set(newOperationalPlan);
293 this.cachedSpeedTime = Double.NaN;
294 this.cachedAccelerationTime = Double.NaN;
295 this.odometer.set(currentOdometer);
296
297
298 if (ALIGNED && newOperationalPlan.getTotalDuration().si == 0.5)
299 {
300
301
302 double tNext = Math.floor(2.0 * now.si + 1.0) / 2.0;
303 DirectedPoint p = (tNext - now.si < 0.5) ? newOperationalPlan.getEndLocation()
304 : newOperationalPlan.getLocation(new Duration(tNext - now.si, DurationUnit.SI));
305 this.nextMoveEvent = new SimEvent<>(new SimTimeDoubleUnit(new Time(tNext, TimeUnit.DEFAULT)), this, this, "move",
306 new Object[] { p });
307 ALIGN_COUNT++;
308 }
309 else
310 {
311
312
313 this.nextMoveEvent = new SimEvent<>(new SimTimeDoubleUnit(now.plus(newOperationalPlan.getTotalDuration())),
314 this, this, "move", new Object[] { newOperationalPlan.getEndLocation() });
315 }
316 this.simulator.scheduleEvent(this.nextMoveEvent);
317
318 fireTimedEvent(GTU.MOVE_EVENT, new Object[] { getId(), fromLocation, getSpeed(), getAcceleration(), getOdometer() },
319 this.simulator.getSimulatorTime());
320
321 return false;
322 }
323 catch (Exception ex)
324 {
325 try
326 {
327 this.errorHandler.handle(this, ex);
328 }
329 catch (Exception exception)
330 {
331 throw new GTUException(exception);
332 }
333 return true;
334 }
335 }
336
337
338
339
340
341
342
343
344
345
346
347 @SuppressWarnings("checkstyle:designforextension")
348 protected void interruptMove()
349 throws SimRuntimeException, OperationalPlanException, GTUException, NetworkException, ParameterException
350 {
351 this.simulator.cancelEvent(this.nextMoveEvent);
352 move(this.operationalPlan.get().getLocation(this.simulator.getSimulatorTime()));
353 }
354
355
356 @Override
357 public final String getId()
358 {
359 return this.id;
360 }
361
362
363 @SuppressWarnings("checkstyle:designforextension")
364 @Override
365 public GTUType getGTUType()
366 {
367 return this.gtuType;
368 }
369
370
371 @Override
372 public final RelativePosition getReference()
373 {
374 return RelativePosition.REFERENCE_POSITION;
375 }
376
377
378 @Override
379 public final OTSSimulatorInterface getSimulator()
380 {
381 return this.simulator;
382 }
383
384
385 @Override
386 public final Parameters getParameters()
387 {
388 return this.parameters;
389 }
390
391
392 @Override
393 public final void setParameters(final Parameters parameters)
394 {
395 this.parameters = parameters;
396 }
397
398
399 @Override
400 public StrategicalPlanner getStrategicalPlanner()
401 {
402 return this.strategicalPlanner.get();
403 }
404
405
406 @Override
407 public StrategicalPlanner getStrategicalPlanner(final Time time)
408 {
409 return this.strategicalPlanner.get(time);
410 }
411
412
413 @Override
414 public final OperationalPlan getOperationalPlan()
415 {
416 return this.operationalPlan.get();
417 }
418
419
420 @Override
421 public final OperationalPlan getOperationalPlan(final Time time)
422 {
423 return this.operationalPlan.get(time);
424 }
425
426
427 @Override
428 public final Length getOdometer()
429 {
430 return getOdometer(this.simulator.getSimulatorTime());
431 }
432
433
434 @Override
435 public final Length getOdometer(final Time time)
436 {
437 if (getOperationalPlan(time) == null)
438 {
439 return this.odometer.get(time);
440 }
441 try
442 {
443 return this.odometer.get(time).plus(getOperationalPlan(time).getTraveledDistance(time));
444 }
445 catch (OperationalPlanException ope)
446 {
447 return this.odometer.get(time);
448 }
449 }
450
451
452 @Override
453 public final Speed getSpeed()
454 {
455 return getSpeed(this.simulator.getSimulatorTime());
456 }
457
458
459 @Override
460 public final Speed getSpeed(final Time time)
461 {
462 if (this.cachedSpeedTime != time.si)
463 {
464 this.cachedSpeedTime = time.si;
465 OperationalPlan plan = getOperationalPlan(time);
466 if (plan == null)
467 {
468 this.cachedSpeed = Speed.ZERO;
469 }
470 else if (time.si < plan.getStartTime().si)
471 {
472 this.cachedSpeed = plan.getStartSpeed();
473 }
474 else if (time.si > plan.getEndTime().si)
475 {
476 if (time.si - plan.getEndTime().si < 1e-6)
477 {
478 this.cachedSpeed = Try.assign(() -> plan.getSpeed(plan.getEndTime()),
479 "getSpeed() could not derive a valid speed for the current operationalPlan");
480 }
481 else
482 {
483 throw new IllegalStateException("Requesting speed value beyond plan.");
484 }
485 }
486 else
487 {
488 this.cachedSpeed = Try.assign(() -> plan.getSpeed(time),
489 "getSpeed() could not derive a valid speed for the current operationalPlan");
490 }
491 }
492 return this.cachedSpeed;
493 }
494
495
496 @Override
497 public final Acceleration getAcceleration()
498 {
499 return getAcceleration(this.simulator.getSimulatorTime());
500 }
501
502
503 @Override
504 public final Acceleration getAcceleration(final Time time)
505 {
506 if (this.cachedAccelerationTime != time.si)
507 {
508 this.cachedAccelerationTime = time.si;
509 OperationalPlan plan = getOperationalPlan(time);
510 if (plan == null)
511 {
512 this.cachedAcceleration = Acceleration.ZERO;
513 }
514 else if (time.si < plan.getStartTime().si)
515 {
516 this.cachedAcceleration =
517 Try.assign(() -> plan.getAcceleration(plan.getStartTime()), "Exception obtaining acceleration.");
518 }
519 else if (time.si > plan.getEndTime().si)
520 {
521 if (time.si - plan.getEndTime().si < 1e-6)
522 {
523 this.cachedAcceleration = Try.assign(() -> plan.getAcceleration(plan.getEndTime()),
524 "getAcceleration() could not derive a valid acceleration for the current operationalPlan");
525 }
526 else
527 {
528 throw new IllegalStateException("Requesting acceleration value beyond plan.");
529 }
530 }
531 else
532 {
533 this.cachedAcceleration = Try.assign(() -> plan.getAcceleration(time),
534 "getAcceleration() could not derive a valid acceleration for the current operationalPlan");
535 }
536 }
537 return this.cachedAcceleration;
538 }
539
540
541
542
543 @Override
544 public final Acceleration getMaximumAcceleration()
545 {
546 return this.maximumAcceleration;
547 }
548
549
550
551
552 public final void setMaximumAcceleration(final Acceleration maximumAcceleration)
553 {
554 if (maximumAcceleration.le(Acceleration.ZERO))
555 {
556 throw new RuntimeException("Maximum acceleration of GTU " + this.id + " set to value <= 0");
557 }
558 this.maximumAcceleration = maximumAcceleration;
559 }
560
561
562
563
564 @Override
565 public final Acceleration getMaximumDeceleration()
566 {
567 return this.maximumDeceleration;
568 }
569
570
571
572
573 public final void setMaximumDeceleration(final Acceleration maximumDeceleration)
574 {
575 if (maximumDeceleration.ge(Acceleration.ZERO))
576 {
577 throw new RuntimeException("Maximum deceleration of GTU " + this.id + " set to value >= 0");
578 }
579 this.maximumDeceleration = maximumDeceleration;
580 }
581
582
583 private Time cacheLocationTime = new Time(Double.NaN, TimeUnit.DEFAULT);
584
585
586 private DirectedPoint cacheLocation = null;
587
588
589 @Override
590 @SuppressWarnings("checkstyle:designforextension")
591 public DirectedPoint getLocation()
592 {
593 if (this.operationalPlan.get() == null)
594 {
595 SimLogger.always()
596 .error("No operational plan for GTU " + this.id + " at t=" + this.getSimulator().getSimulatorTime());
597 return new DirectedPoint(0, 0, 0);
598 }
599 try
600 {
601
602 if (this.cacheLocationTime.si != this.simulator.getSimulatorTime().si)
603 {
604 this.cacheLocationTime = this.simulator.getSimulatorTime();
605 this.cacheLocation = this.operationalPlan.get().getLocation(this.cacheLocationTime);
606 }
607 return this.cacheLocation;
608 }
609 catch (OperationalPlanException exception)
610 {
611 return new DirectedPoint(0, 0, 0);
612 }
613 }
614
615
616 @Override
617 public final boolean isDestroyed()
618 {
619 return this.destroyed;
620 }
621
622
623 @Override
624 public PerceivableContext getPerceivableContext()
625 {
626 return this.perceivableContext;
627 }
628
629
630 @Override
631 public void addGtu(final GTU gtu) throws GTUException
632 {
633 this.children.add(gtu);
634 gtu.setParent(this);
635 }
636
637
638 @Override
639 public void removeGtu(final GTU gtu)
640 {
641 this.children.remove(gtu);
642 try
643 {
644 gtu.setParent(null);
645 }
646 catch (GTUException exception)
647 {
648
649 }
650 }
651
652
653 @Override
654 public void setParent(final GTU gtu) throws GTUException
655 {
656 Throw.when(gtu != null && this.parent != null, GTUException.class, "GTU %s already has a parent.", this);
657 this.parent = gtu;
658 }
659
660
661 @Override
662 public GTU getParent()
663 {
664 return this.parent;
665 }
666
667
668 @Override
669 public Set<GTU> getChildren()
670 {
671 return new LinkedHashSet<>(this.children);
672 }
673
674
675
676
677 protected GTUErrorHandler getErrorHandler()
678 {
679 return this.errorHandler;
680 }
681
682
683 @Override
684 public void setErrorHandler(final GTUErrorHandler errorHandler)
685 {
686 this.errorHandler = errorHandler;
687 }
688
689
690
691
692
693 public final SimEvent<SimTimeDoubleUnit> getNextMoveEvent()
694 {
695 return this.nextMoveEvent;
696 }
697
698
699 @Override
700 @SuppressWarnings("designforextension")
701 public int hashCode()
702 {
703 final int prime = 31;
704 int result = 1;
705 result = prime * result + ((this.id == null) ? 0 : this.id.hashCode());
706 result = prime * result + this.uniqueNumber;
707 return result;
708 }
709
710
711 @Override
712 @SuppressWarnings({ "designforextension", "needbraces" })
713 public boolean equals(final Object obj)
714 {
715 if (this == obj)
716 return true;
717 if (obj == null)
718 return false;
719 if (getClass() != obj.getClass())
720 return false;
721 AbstractGTU./../../org/opentrafficsim/core/gtu/AbstractGTU.html#AbstractGTU">AbstractGTU other = (AbstractGTU) obj;
722 if (this.id == null)
723 {
724 if (other.id != null)
725 return false;
726 }
727 else if (!this.id.equals(other.id))
728 return false;
729 if (this.uniqueNumber != other.uniqueNumber)
730 return false;
731 return true;
732 }
733
734 }