1 package org.opentrafficsim.road.gtu.lane.plan.operational;
2
3 import java.util.ArrayList;
4 import java.util.Arrays;
5 import java.util.Iterator;
6 import java.util.List;
7 import java.util.Map;
8
9 import org.djunits.unit.AccelerationUnit;
10 import org.djunits.unit.DurationUnit;
11 import org.djunits.unit.LengthUnit;
12 import org.djunits.unit.SpeedUnit;
13 import org.djunits.value.ValueRuntimeException;
14 import org.djunits.value.vdouble.scalar.Acceleration;
15 import org.djunits.value.vdouble.scalar.Duration;
16 import org.djunits.value.vdouble.scalar.Length;
17 import org.djunits.value.vdouble.scalar.Speed;
18 import org.djunits.value.vdouble.scalar.Time;
19 import org.djutils.exceptions.Throw;
20 import org.djutils.logger.CategoryLogger;
21 import org.opentrafficsim.base.parameters.ParameterException;
22 import org.opentrafficsim.core.geometry.DirectedPoint;
23 import org.opentrafficsim.core.geometry.OTSGeometryException;
24 import org.opentrafficsim.core.geometry.OTSLine3D;
25 import org.opentrafficsim.core.geometry.OTSPoint3D;
26 import org.opentrafficsim.core.gtu.GTUException;
27 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
28 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
29 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.SpeedSegment;
30 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
31 import org.opentrafficsim.core.math.Solver;
32 import org.opentrafficsim.core.network.LateralDirectionality;
33 import org.opentrafficsim.core.network.NetworkException;
34 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
35 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
36 import org.opentrafficsim.road.network.lane.Lane;
37 import org.opentrafficsim.road.network.lane.LaneDirection;
38 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
39 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
40
41 import nl.tudelft.simulation.dsol.SimRuntimeException;
42 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58 public final class LaneOperationalPlanBuilder
59 {
60
61
62 private static final Acceleration MAX_ACCELERATION = new Acceleration(1E12, AccelerationUnit.SI);
63
64
65 private static final Acceleration MAX_DECELERATION = new Acceleration(-1E12, AccelerationUnit.SI);
66
67
68
69
70
71 private static final Length MINIMUM_CREDIBLE_PATH_LENGTH = new Length(0.001, LengthUnit.METER);
72
73
74 LaneOperationalPlanBuilder()
75 {
76
77 }
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97 public static LaneBasedOperationalPlan buildGradualAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
98 final Time startTime, final Speed startSpeed, final Speed endSpeed, final Acceleration maxAcceleration,
99 final Acceleration maxDeceleration) throws OperationalPlanException, OTSGeometryException
100 {
101 OTSLine3D path = createPathAlongCenterLine(gtu, distance);
102 Segment segment;
103 if (startSpeed.eq(endSpeed))
104 {
105 segment = new SpeedSegment(distance.divide(startSpeed));
106 }
107 else
108 {
109 try
110 {
111
112 Duration duration = distance.times(2.0).divide(endSpeed.plus(startSpeed));
113 Acceleration acceleration = endSpeed.minus(startSpeed).divide(duration);
114 if (acceleration.si < 0.0 && acceleration.lt(maxDeceleration))
115 {
116 acceleration = maxDeceleration;
117 duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
118 DurationUnit.SI);
119 }
120 if (acceleration.si > 0.0 && acceleration.gt(maxAcceleration))
121 {
122 acceleration = maxAcceleration;
123 duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
124 DurationUnit.SI);
125 }
126 segment = new OperationalPlan.AccelerationSegment(duration, acceleration);
127 }
128 catch (ValueRuntimeException ve)
129 {
130 throw new OperationalPlanException(ve);
131 }
132 }
133 ArrayList<Segment> segmentList = new ArrayList<>();
134 segmentList.add(segment);
135 return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, false);
136 }
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153 public static LaneBasedOperationalPlan buildGradualAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
154 final Time startTime, final Speed startSpeed, final Speed endSpeed)
155 throws OperationalPlanException, OTSGeometryException
156 {
157 return buildGradualAccelerationPlan(gtu, distance, startTime, startSpeed, endSpeed, MAX_ACCELERATION, MAX_DECELERATION);
158 }
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177 public static LaneBasedOperationalPlan buildMaximumAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
178 final Time startTime, final Speed startSpeed, final Speed endSpeed, final Acceleration acceleration,
179 final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
180 {
181 OTSLine3D path = createPathAlongCenterLine(gtu, distance);
182 ArrayList<Segment> segmentList = new ArrayList<>();
183 if (startSpeed.eq(endSpeed))
184 {
185 segmentList.add(new OperationalPlan.SpeedSegment(distance.divide(startSpeed)));
186 }
187 else
188 {
189 try
190 {
191 if (endSpeed.gt(startSpeed))
192 {
193 Duration t = endSpeed.minus(startSpeed).divide(acceleration);
194 Length x = startSpeed.times(t).plus(acceleration.times(0.5).times(t).times(t));
195 if (x.ge(distance))
196 {
197
198 Duration duration =
199 new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
200 DurationUnit.SI);
201 segmentList.add(new OperationalPlan.AccelerationSegment(duration, acceleration));
202 }
203 else
204 {
205
206 segmentList.add(new OperationalPlan.AccelerationSegment(t, acceleration));
207 Duration duration = distance.minus(x).divide(endSpeed);
208 segmentList.add(new OperationalPlan.SpeedSegment(duration));
209 }
210 }
211 else
212 {
213 Duration t = endSpeed.minus(startSpeed).divide(deceleration);
214 Length x = startSpeed.times(t).plus(deceleration.times(0.5).times(t).times(t));
215 if (x.ge(distance))
216 {
217
218 Duration duration =
219 new Duration(Solver.firstSolutionAfter(0, deceleration.si / 2, startSpeed.si, -distance.si),
220 DurationUnit.SI);
221 segmentList.add(new OperationalPlan.AccelerationSegment(duration, deceleration));
222 }
223 else
224 {
225 if (endSpeed.si == 0.0)
226 {
227
228 OTSLine3D partialPath = path.truncate(x.si);
229 segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
230 return new LaneBasedOperationalPlan(gtu, partialPath, startTime, startSpeed, segmentList, false);
231 }
232
233 segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
234 Duration duration = distance.minus(x).divide(endSpeed);
235 segmentList.add(new OperationalPlan.SpeedSegment(duration));
236 }
237 }
238 }
239 catch (ValueRuntimeException ve)
240 {
241 throw new OperationalPlanException(ve);
242 }
243
244 }
245 return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, false);
246 }
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264 public static LaneBasedOperationalPlan buildAccelerationPlan(final LaneBasedGTU gtu, final Time startTime,
265 final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final boolean deviative)
266 throws OperationalPlanException, OTSGeometryException
267 {
268 if (startSpeed.si <= OperationalPlan.DRIFTING_SPEED_SI && acceleration.le(Acceleration.ZERO))
269 {
270 return new LaneBasedOperationalPlan(gtu, gtu.getLocation(), startTime, timeStep, deviative);
271 }
272
273 Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
274 Length distance =
275 Length.instantiateSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
276 List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
277 if (distance.le(MINIMUM_CREDIBLE_PATH_LENGTH))
278 {
279 return new LaneBasedOperationalPlan(gtu, gtu.getLocation(), startTime, timeStep, deviative);
280 }
281 OTSLine3D path = createPathAlongCenterLine(gtu, distance);
282 return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, deviative);
283 }
284
285
286
287
288
289
290
291
292 public static OTSLine3D createPathAlongCenterLine(final LaneBasedGTU gtu, final Length distance) throws OTSGeometryException
293 {
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312 OTSLine3D path = null;
313 try
314 {
315 DirectedLanePosition ref = gtu.getReferencePosition();
316 double f = ref.getLane().fraction(ref.getPosition());
317 if (ref.getGtuDirection().isPlus() && f < 1.0)
318 {
319 if (f >= 0.0)
320 {
321 path = ref.getLane().getCenterLine().extractFractional(f, 1.0);
322 }
323 else
324 {
325 path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0);
326 }
327 }
328 else if (ref.getGtuDirection().isMinus() && f > 0.0)
329 {
330 if (f <= 1.0)
331 {
332 path = ref.getLane().getCenterLine().extractFractional(0.0, f).reverse();
333 }
334 else
335 {
336 path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0).reverse();
337 }
338 }
339
340
341
342
343 LaneDirection prevFrom = null;
344 LaneDirection from = ref.getLaneDirection();
345 int n = 1;
346 boolean alternativeTried = false;
347 while (path == null || path.getLength().si < distance.si + n * Lane.MARGIN.si)
348 {
349 n++;
350 prevFrom = from;
351 if (null == from)
352 {
353 CategoryLogger.always().warn("About to die: GTU {} has null from value", gtu.getId());
354 }
355 from = from.getNextLaneDirection(gtu);
356 if (from == null)
357 {
358
359 Length pos = prevFrom.getDirection().isPlus() ? prevFrom.getLength() : Length.ZERO;
360 for (SingleSensor sensor : prevFrom.getLane().getSensors(pos, pos, gtu.getGTUType(),
361 prevFrom.getDirection()))
362 {
363
364 if (sensor instanceof SinkSensor)
365 {
366
367 DirectedPoint end = path.getLocationExtendedSI(distance.si + n * Lane.MARGIN.si);
368 List<OTSPoint3D> points = new ArrayList<>(Arrays.asList(path.getPoints()));
369 points.add(new OTSPoint3D(end));
370 return new OTSLine3D(points);
371 }
372 }
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424 CategoryLogger.always().error("GTU {} has nowhere to go and no sink sensor either", gtu);
425
426 gtu.destroy();
427 return path;
428 }
429 if (path == null)
430 {
431 path = from.getDirection().isPlus() ? from.getLane().getCenterLine()
432 : from.getLane().getCenterLine().reverse();
433 }
434 else
435 {
436 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, from.getDirection().isPlus()
437 ? from.getLane().getCenterLine() : from.getLane().getCenterLine().reverse());
438 }
439 }
440 }
441 catch (GTUException exception)
442 {
443 throw new RuntimeException("Error during creation of path.", exception);
444 }
445 return path;
446 }
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467 @SuppressWarnings("checkstyle:parameternumber")
468 public static LaneBasedOperationalPlan buildAccelerationLaneChangePlan(final LaneBasedGTU gtu,
469 final LateralDirectionality laneChangeDirectionality, final DirectedPoint startPosition, final Time startTime,
470 final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final LaneChange laneChange)
471 throws OperationalPlanException, OTSGeometryException
472 {
473
474
475
476 LateralDirectionality direction = laneChange.isChangingLane() ? laneChange.getDirection() : laneChangeDirectionality;
477
478 Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
479 Length planDistance =
480 Length.instantiateSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
481 List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
482
483 try
484 {
485
486 Map<Lane, Length> positions = gtu.positions(gtu.getReference());
487 DirectedLanePosition ref = gtu.getReferencePosition();
488 Iterator<Lane> iterator = ref.getLane()
489 .accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection()).iterator();
490 Lane adjLane = iterator.hasNext() ? iterator.next() : null;
491 DirectedLanePosition from = null;
492 if (laneChange.getDirection() == null || (adjLane != null && positions.containsKey(adjLane)))
493 {
494
495 from = ref;
496 }
497 else
498 {
499
500 for (Lane lane : positions.keySet())
501 {
502 if (lane.accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection())
503 .contains(ref.getLane()))
504 {
505 from = new DirectedLanePosition(lane, positions.get(lane), ref.getGtuDirection());
506 break;
507 }
508 }
509 }
510 Throw.when(from == null, RuntimeException.class, "From lane could not be determined during lane change.");
511
512
513 OTSLine3D path = laneChange.getPath(timeStep, gtu, from, startPosition, planDistance, direction);
514 LaneBasedOperationalPlan plan = new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, true);
515 return plan;
516 }
517 catch (GTUException exception)
518 {
519 throw new RuntimeException("Error during creation of lane change plan.", exception);
520 }
521 }
522
523
524
525
526
527
528
529
530 public static Duration brakingTime(final Acceleration acceleration, final Speed startSpeed, final Duration time)
531 {
532 if (acceleration.ge0())
533 {
534 return time;
535 }
536 double t = startSpeed.si / -acceleration.si;
537 if (t >= time.si)
538 {
539 return time;
540 }
541 return Duration.instantiateSI(t);
542 }
543
544
545
546
547
548
549
550
551
552 private static List<Segment> createAccelerationSegments(final Speed startSpeed, final Acceleration acceleration,
553 final Duration brakingTime, final Duration timeStep)
554 {
555 List<Segment> segmentList = new ArrayList<>();
556 if (brakingTime.si < timeStep.si)
557 {
558 if (brakingTime.si > 0.0)
559 {
560 segmentList.add(new OperationalPlan.AccelerationSegment(brakingTime, acceleration));
561 }
562 segmentList.add(new OperationalPlan.SpeedSegment(timeStep.minus(brakingTime)));
563 }
564 else
565 {
566 segmentList.add(new OperationalPlan.AccelerationSegment(timeStep, acceleration));
567 }
568 return segmentList;
569 }
570
571
572
573
574
575
576
577
578
579
580
581
582
583 public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGTU gtu, final Time startTime,
584 final SimpleOperationalPlan simplePlan, final LaneChange laneChange)
585 throws ParameterException, GTUException, NetworkException, OperationalPlanException
586 {
587 Acceleration acc = gtu.getVehicleModel().boundAcceleration(simplePlan.getAcceleration(), gtu);
588
589 if (gtu.isInstantaneousLaneChange())
590 {
591 if (simplePlan.isLaneChange())
592 {
593 gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
594 }
595 try
596 {
597 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
598 simplePlan.getDuration(), false);
599 }
600 catch (OTSGeometryException exception)
601 {
602 throw new OperationalPlanException(exception);
603 }
604 }
605
606
607 try
608 {
609 if (!simplePlan.isLaneChange() && !laneChange.isChangingLane())
610 {
611 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
612 simplePlan.getDuration(), true);
613 }
614 if (gtu.getSpeed().si == 0.0 && acc.si <= 0.0)
615 {
616 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
617 simplePlan.getDuration(), false);
618 }
619 return LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(gtu, simplePlan.getLaneChangeDirection(),
620 gtu.getLocation(), startTime, gtu.getSpeed(), acc, simplePlan.getDuration(), laneChange);
621 }
622 catch (OTSGeometryException exception)
623 {
624 throw new OperationalPlanException(exception);
625 }
626 }
627
628
629
630
631
632
633
634
635
636
637
638 public static void scheduleLaneChangeFinalization(final LaneBasedGTU gtu, final Length distance,
639 final LateralDirectionality laneChangeDirection) throws SimRuntimeException
640 {
641 Time time = gtu.getOperationalPlan().timeAtDistance(distance);
642 if (Double.isNaN(time.si))
643 {
644
645 time = gtu.getOperationalPlan().getEndTime();
646 }
647 SimEventInterface<Duration> event = gtu.getSimulator().scheduleEventAbsTime(time, (short) 6, gtu, gtu,
648 "finalizeLaneChange", new Object[] { laneChangeDirection });
649 gtu.setFinalizeLaneChangeEvent(event);
650 }
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666 public static LaneBasedOperationalPlan buildStopPlan(final LaneBasedGTU gtu, final Length distance, final Time startTime,
667 final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
668 {
669 return buildMaximumAccelerationPlan(gtu, distance, startTime, startSpeed, new Speed(0.0, SpeedUnit.SI),
670 new Acceleration(1.0, AccelerationUnit.SI), deceleration);
671 }
672
673 }