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 tactPlanner.getPerception().perceive();
285 OperationalPlan newOperationalPlan = tactPlanner.generateOperationalPlan(now, fromLocation);
286 this.operationalPlan.set(newOperationalPlan);
287 this.cachedSpeedTime = Double.NaN;
288 this.cachedAccelerationTime = Double.NaN;
289 this.odometer.set(currentOdometer);
290 if (getOperationalPlan().getAcceleration(Duration.ZERO).si < -10
291 && getOperationalPlan().getSpeed(Duration.ZERO).si > 2.5)
292 {
293 SimLogger.always().error("(getOperationalPlan().getAcceleration(Duration.ZERO).si < -10)");
294
295 }
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.BASE)), 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())), this,
314 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
322
323
324
325
326
327
328
329
330
331
332 @SuppressWarnings("checkstyle:designforextension")
333 protected void interruptMove()
334 throws SimRuntimeException, OperationalPlanException, GTUException, NetworkException, ParameterException
335 {
336 this.simulator.cancelEvent(this.nextMoveEvent);
337 move(this.operationalPlan.get().getLocation(this.simulator.getSimulatorTime()));
338 }
339
340
341 @Override
342 public final String getId()
343 {
344 return this.id;
345 }
346
347
348 @SuppressWarnings("checkstyle:designforextension")
349 @Override
350 public GTUType getGTUType()
351 {
352 return this.gtuType;
353 }
354
355
356 @Override
357 public final RelativePosition getReference()
358 {
359 return RelativePosition.REFERENCE_POSITION;
360 }
361
362
363 @Override
364 public final OTSSimulatorInterface getSimulator()
365 {
366 return this.simulator;
367 }
368
369
370 @Override
371 public final Parameters getParameters()
372 {
373 return this.parameters;
374 }
375
376
377 @Override
378 public final void setParameters(final Parameters parameters)
379 {
380 this.parameters = parameters;
381 }
382
383
384 @Override
385 public StrategicalPlanner getStrategicalPlanner()
386 {
387 return this.strategicalPlanner.get();
388 }
389
390
391 @Override
392 public StrategicalPlanner getStrategicalPlanner(final Time time)
393 {
394 return this.strategicalPlanner.get(time);
395 }
396
397
398 @Override
399 public final OperationalPlan getOperationalPlan()
400 {
401 return this.operationalPlan.get();
402 }
403
404
405 @Override
406 public final OperationalPlan getOperationalPlan(final Time time)
407 {
408 return this.operationalPlan.get(time);
409 }
410
411
412 @Override
413 public final Length getOdometer()
414 {
415 return getOdometer(this.simulator.getSimulatorTime());
416 }
417
418
419 @Override
420 public final Length getOdometer(final Time time)
421 {
422 if (getOperationalPlan(time) == null)
423 {
424 return this.odometer.get(time);
425 }
426 try
427 {
428 return this.odometer.get(time).plus(getOperationalPlan(time).getTraveledDistance(time));
429 }
430 catch (OperationalPlanException ope)
431 {
432 return this.odometer.get(time);
433 }
434 }
435
436
437 @Override
438 public final Speed getSpeed()
439 {
440 return getSpeed(this.simulator.getSimulatorTime());
441 }
442
443
444 @Override
445 public final Speed getSpeed(final Time time)
446 {
447 if (this.cachedSpeedTime != time.si)
448 {
449 this.cachedSpeedTime = time.si;
450 OperationalPlan plan = getOperationalPlan(time);
451 if (plan == null)
452 {
453 this.cachedSpeed = Speed.ZERO;
454 }
455 else if (time.si < plan.getStartTime().si)
456 {
457 this.cachedSpeed = plan.getStartSpeed();
458 }
459 else if (time.si > plan.getEndTime().si)
460 {
461 if (time.si - plan.getEndTime().si < 1e-6)
462 {
463 this.cachedSpeed = Try.assign(() -> plan.getSpeed(plan.getEndTime()),
464 "getSpeed() could not derive a valid speed for the current operationalPlan");
465 }
466 else
467 {
468 throw new IllegalStateException("Requesting speed value beyond plan.");
469 }
470 }
471 else
472 {
473 this.cachedSpeed = Try.assign(() -> plan.getSpeed(time),
474 "getSpeed() could not derive a valid speed for the current operationalPlan");
475 }
476 }
477 return this.cachedSpeed;
478 }
479
480
481 @Override
482 public final Acceleration getAcceleration()
483 {
484 return getAcceleration(this.simulator.getSimulatorTime());
485 }
486
487
488 @Override
489 public final Acceleration getAcceleration(final Time time)
490 {
491 if (this.cachedAccelerationTime != time.si)
492 {
493 this.cachedAccelerationTime = time.si;
494 OperationalPlan plan = getOperationalPlan(time);
495 if (plan == null)
496 {
497 this.cachedAcceleration = Acceleration.ZERO;
498 }
499 else if (time.si < plan.getStartTime().si)
500 {
501 this.cachedAcceleration =
502 Try.assign(() -> plan.getAcceleration(plan.getStartTime()), "Exception obtaining acceleration.");
503 }
504 else if (time.si > plan.getEndTime().si)
505 {
506 if (time.si - plan.getEndTime().si < 1e-6)
507 {
508 this.cachedAcceleration = Try.assign(() -> plan.getAcceleration(plan.getEndTime()),
509 "getAcceleration() could not derive a valid acceleration for the current operationalPlan");
510 }
511 else
512 {
513 throw new IllegalStateException("Requesting acceleration value beyond plan.");
514 }
515 }
516 else
517 {
518 this.cachedAcceleration = Try.assign(() -> plan.getAcceleration(time),
519 "getAcceleration() could not derive a valid acceleration for the current operationalPlan");
520 }
521 }
522 return this.cachedAcceleration;
523 }
524
525
526
527
528 @Override
529 public final Acceleration getMaximumAcceleration()
530 {
531 return this.maximumAcceleration;
532 }
533
534
535
536
537 public final void setMaximumAcceleration(final Acceleration maximumAcceleration)
538 {
539 if (maximumAcceleration.le(Acceleration.ZERO))
540 {
541 throw new RuntimeException("Maximum acceleration of GTU " + this.id + " set to value <= 0");
542 }
543 this.maximumAcceleration = maximumAcceleration;
544 }
545
546
547
548
549 @Override
550 public final Acceleration getMaximumDeceleration()
551 {
552 return this.maximumDeceleration;
553 }
554
555
556
557
558 public final void setMaximumDeceleration(final Acceleration maximumDeceleration)
559 {
560 if (maximumDeceleration.ge(Acceleration.ZERO))
561 {
562 throw new RuntimeException("Maximum deceleration of GTU " + this.id + " set to value >= 0");
563 }
564 this.maximumDeceleration = maximumDeceleration;
565 }
566
567
568 private Time cacheLocationTime = new Time(Double.NaN, TimeUnit.BASE);
569
570
571 private DirectedPoint cacheLocation = null;
572
573
574 @Override
575 @SuppressWarnings("checkstyle:designforextension")
576 public DirectedPoint getLocation()
577 {
578 if (this.operationalPlan.get() == null)
579 {
580 SimLogger.always()
581 .error("No operational plan for GTU " + this.id + " at t=" + this.getSimulator().getSimulatorTime());
582 return new DirectedPoint(0, 0, 0);
583 }
584 try
585 {
586
587 if (this.cacheLocationTime.si != this.simulator.getSimulatorTime().si)
588 {
589 this.cacheLocationTime = this.simulator.getSimulatorTime();
590 this.cacheLocation = this.operationalPlan.get().getLocation(this.cacheLocationTime);
591 }
592 return this.cacheLocation;
593 }
594 catch (OperationalPlanException exception)
595 {
596 return new DirectedPoint(0, 0, 0);
597 }
598 }
599
600
601
602
603 public final boolean isDestroyed()
604 {
605 return this.destroyed;
606 }
607
608
609
610
611 public final PerceivableContext getPerceivableContext()
612 {
613 return this.perceivableContext;
614 }
615
616
617 @Override
618 public void addGtu(final GTU gtu) throws GTUException
619 {
620 this.children.add(gtu);
621 gtu.setParent(this);
622 }
623
624
625 @Override
626 public void removeGtu(final GTU gtu)
627 {
628 this.children.remove(gtu);
629 try
630 {
631 gtu.setParent(null);
632 }
633 catch (GTUException exception)
634 {
635
636 }
637 }
638
639
640 @Override
641 public void setParent(final GTU gtu) throws GTUException
642 {
643 Throw.when(gtu != null && this.parent != null, GTUException.class, "GTU %s already has a parent.", this);
644 this.parent = gtu;
645 }
646
647
648 @Override
649 public GTU getParent()
650 {
651 return this.parent;
652 }
653
654
655 @Override
656 public Set<GTU> getChildren()
657 {
658 return new LinkedHashSet<>(this.children);
659 }
660
661
662
663
664
665 public final SimEvent<SimTimeDoubleUnit> getNextMoveEvent()
666 {
667 return this.nextMoveEvent;
668 }
669
670
671 @Override
672 @SuppressWarnings("designforextension")
673 public int hashCode()
674 {
675 final int prime = 31;
676 int result = 1;
677 result = prime * result + ((this.id == null) ? 0 : this.id.hashCode());
678 result = prime * result + this.uniqueNumber;
679 return result;
680 }
681
682
683 @Override
684 @SuppressWarnings({ "designforextension", "needbraces" })
685 public boolean equals(final Object obj)
686 {
687 if (this == obj)
688 return true;
689 if (obj == null)
690 return false;
691 if (getClass() != obj.getClass())
692 return false;
693 AbstractGTU other = (AbstractGTU) obj;
694 if (this.id == null)
695 {
696 if (other.id != null)
697 return false;
698 }
699 else if (!this.id.equals(other.id))
700 return false;
701 if (this.uniqueNumber != other.uniqueNumber)
702 return false;
703 return true;
704 }
705
706 }