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