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.OTSGeometryException;
23 import org.opentrafficsim.core.geometry.OTSLine3D;
24 import org.opentrafficsim.core.geometry.OTSPoint3D;
25 import org.opentrafficsim.core.gtu.GTUDirectionality;
26 import org.opentrafficsim.core.gtu.GTUException;
27 import org.opentrafficsim.core.gtu.RelativePosition;
28 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
29 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
30 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.SpeedSegment;
31 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
32 import org.opentrafficsim.core.math.Solver;
33 import org.opentrafficsim.core.network.LateralDirectionality;
34 import org.opentrafficsim.core.network.NetworkException;
35 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
36 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
37 import org.opentrafficsim.road.network.lane.Lane;
38 import org.opentrafficsim.road.network.lane.LaneDirection;
39 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
40 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
41
42 import nl.tudelft.simulation.dsol.SimRuntimeException;
43 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
44 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
45 import nl.tudelft.simulation.language.d3.DirectedPoint;
46
47 /**
48 * Builder for several often used operational plans. E.g., decelerate to come to a full stop at the end of a shape; accelerate
49 * to reach a certain speed at the end of a curve; drive constant on a curve; decelerate or accelerate to reach a given end
50 * speed at the end of a curve, etc.<br>
51 * TODO driving with negative speeds (backward driving) is not yet supported.
52 * <p>
53 * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
54 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
55 * </p>
56 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
57 * initial version Nov 15, 2015 <br>
58 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
59 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
60 */
61 public final class LaneOperationalPlanBuilder // class package private for scheduling static method on an instance
62 {
63
64 /** Maximum acceleration for unbounded accelerations: 1E12 m/s2. */
65 private static final Acceleration MAX_ACCELERATION = new Acceleration(1E12, AccelerationUnit.SI);
66
67 /** Maximum deceleration for unbounded accelerations: -1E12 m/s2. */
68 private static final Acceleration MAX_DECELERATION = new Acceleration(-1E12, AccelerationUnit.SI);
69
70 /**
71 * Minimum distance of an operational plan path; anything shorter will be truncated to 0. <br>
72 * If objects related to e.g. molecular movements are simulated using this code, a setter for this parameter will be needed.
73 */
74 private static final Length MINIMUM_CREDIBLE_PATH_LENGTH = new Length(0.001, LengthUnit.METER);
75
76 /** Constructor. */
77 LaneOperationalPlanBuilder()
78 {
79 // class should not be instantiated
80 }
81
82 /**
83 * Build a plan with a path and a given start speed to try to reach a provided end speed, exactly at the end of the curve.
84 * The acceleration (and deceleration) are capped by maxAcceleration and maxDeceleration. Therefore, there is no guarantee
85 * that the end speed is actually reached by this plan.
86 * @param gtu LaneBasedGTU; the GTU for debugging purposes
87 * @param distance Length; distance to drive for reaching the end speed
88 * @param startTime Time; the current time or a time in the future when the plan should start
89 * @param startSpeed Speed; the speed at the start of the path
90 * @param endSpeed Speed; the required end speed
91 * @param maxAcceleration Acceleration; the maximum acceleration that can be applied, provided as a POSITIVE number
92 * @param maxDeceleration Acceleration; the maximum deceleration that can be applied, provided as a NEGATIVE number
93 * @return the operational plan to accomplish the given end speed
94 * @throws OperationalPlanException when the plan cannot be generated, e.g. because of a path that is too short
95 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
96 * constructed segment list differ more than a given threshold
97 * @throws OTSGeometryException in case the lanes are not connected or firstLanePosition is larger than the length of the
98 * first lane
99 */
100 public static LaneBasedOperationalPlan buildGradualAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
101 final Time startTime, final Speed startSpeed, final Speed endSpeed, final Acceleration maxAcceleration,
102 final Acceleration maxDeceleration) throws OperationalPlanException, OTSGeometryException
103 {
104 OTSLine3D path = createPathAlongCenterLine(gtu, distance);
105 Segment segment;
106 if (startSpeed.eq(endSpeed))
107 {
108 segment = new SpeedSegment(distance.divide(startSpeed));
109 }
110 else
111 {
112 try
113 {
114 // t = 2x / (vt + v0); a = (vt - v0) / t
115 Duration duration = distance.times(2.0).divide(endSpeed.plus(startSpeed));
116 Acceleration acceleration = endSpeed.minus(startSpeed).divide(duration);
117 if (acceleration.si < 0.0 && acceleration.lt(maxDeceleration))
118 {
119 acceleration = maxDeceleration;
120 duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
121 DurationUnit.SI);
122 }
123 if (acceleration.si > 0.0 && acceleration.gt(maxAcceleration))
124 {
125 acceleration = maxAcceleration;
126 duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
127 DurationUnit.SI);
128 }
129 segment = new OperationalPlan.AccelerationSegment(duration, acceleration);
130 }
131 catch (ValueRuntimeException ve)
132 {
133 throw new OperationalPlanException(ve);
134 }
135 }
136 ArrayList<Segment> segmentList = new ArrayList<>();
137 segmentList.add(segment);
138 return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, false);
139 }
140
141 /**
142 * Build a plan with a path and a given start speed to reach a provided end speed, exactly at the end of the curve.
143 * Acceleration and deceleration are virtually unbounded (1E12 m/s2) to reach the end speed (e.g., to come to a complete
144 * stop).
145 * @param gtu LaneBasedGTU; the GTU for debugging purposes
146 * @param distance Length; distance to drive for reaching the end speed
147 * @param startTime Time; the current time or a time in the future when the plan should start
148 * @param startSpeed Speed; the speed at the start of the path
149 * @param endSpeed Speed; the required end speed
150 * @return the operational plan to accomplish the given end speed
151 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
152 * constructed segment list differ more than a given threshold
153 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
154 * first lane
155 */
156 public static LaneBasedOperationalPlan buildGradualAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
157 final Time startTime, final Speed startSpeed, final Speed endSpeed)
158 throws OperationalPlanException, OTSGeometryException
159 {
160 return buildGradualAccelerationPlan(gtu, distance, startTime, startSpeed, endSpeed, MAX_ACCELERATION, MAX_DECELERATION);
161 }
162
163 /**
164 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
165 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
166 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
167 * before completing the path, a truncated path that ends where the GTU stops is used instead.
168 * @param gtu LaneBasedGTU; the GTU for debugging purposes
169 * @param distance Length; distance to drive for reaching the end speed
170 * @param startTime Time; the current time or a time in the future when the plan should start
171 * @param startSpeed Speed; the speed at the start of the path
172 * @param endSpeed Speed; the required end speed
173 * @param acceleration Acceleration; the acceleration to use if endSpeed > startSpeed, provided as a POSITIVE number
174 * @param deceleration Acceleration; the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
175 * @return the operational plan to accomplish the given end speed
176 * @throws OperationalPlanException when the construction of the operational path fails
177 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
178 * first lane
179 */
180 public static LaneBasedOperationalPlan buildMaximumAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
181 final Time startTime, final Speed startSpeed, final Speed endSpeed, final Acceleration acceleration,
182 final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
183 {
184 OTSLine3D path = createPathAlongCenterLine(gtu, distance);
185 ArrayList<Segment> segmentList = new ArrayList<>();
186 if (startSpeed.eq(endSpeed))
187 {
188 segmentList.add(new OperationalPlan.SpeedSegment(distance.divide(startSpeed)));
189 }
190 else
191 {
192 try
193 {
194 if (endSpeed.gt(startSpeed))
195 {
196 Duration t = endSpeed.minus(startSpeed).divide(acceleration);
197 Length x = startSpeed.times(t).plus(acceleration.times(0.5).times(t).times(t));
198 if (x.ge(distance))
199 {
200 // we cannot reach the end speed in the given distance with the given acceleration
201 Duration duration =
202 new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
203 DurationUnit.SI);
204 segmentList.add(new OperationalPlan.AccelerationSegment(duration, acceleration));
205 }
206 else
207 {
208 // we reach the (higher) end speed before the end of the segment. Make two segments.
209 segmentList.add(new OperationalPlan.AccelerationSegment(t, acceleration));
210 Duration duration = distance.minus(x).divide(endSpeed);
211 segmentList.add(new OperationalPlan.SpeedSegment(duration));
212 }
213 }
214 else
215 {
216 Duration t = endSpeed.minus(startSpeed).divide(deceleration);
217 Length x = startSpeed.times(t).plus(deceleration.times(0.5).times(t).times(t));
218 if (x.ge(distance))
219 {
220 // we cannot reach the end speed in the given distance with the given deceleration
221 Duration duration =
222 new Duration(Solver.firstSolutionAfter(0, deceleration.si / 2, startSpeed.si, -distance.si),
223 DurationUnit.SI);
224 segmentList.add(new OperationalPlan.AccelerationSegment(duration, deceleration));
225 }
226 else
227 {
228 if (endSpeed.si == 0.0)
229 {
230 // if endSpeed == 0, we cannot reach the end of the path. Therefore, build a partial path.
231 OTSLine3D partialPath = path.truncate(x.si);
232 segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
233 return new LaneBasedOperationalPlan(gtu, partialPath, startTime, startSpeed, segmentList, false);
234 }
235 // we reach the (lower) end speed, larger than zero, before the end of the segment. Make two segments.
236 segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
237 Duration duration = distance.minus(x).divide(endSpeed);
238 segmentList.add(new OperationalPlan.SpeedSegment(duration));
239 }
240 }
241 }
242 catch (ValueRuntimeException ve)
243 {
244 throw new OperationalPlanException(ve);
245 }
246
247 }
248 return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, false);
249 }
250
251 /**
252 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
253 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
254 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
255 * before completing the path, a truncated path that ends where the GTU stops is used instead.
256 * @param gtu LaneBasedGTU; the GTU for debugging purposes
257 * @param startTime Time; the current time or a time in the future when the plan should start
258 * @param startSpeed Speed; the speed at the start of the path
259 * @param acceleration Acceleration; the acceleration to use
260 * @param timeStep Duration; time step for the plan
261 * @param deviative boolean; whether the plan is deviative
262 * @return the operational plan to accomplish the given end speed
263 * @throws OperationalPlanException when the construction of the operational path fails
264 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
265 * first lane
266 */
267 public static LaneBasedOperationalPlan buildAccelerationPlan(final LaneBasedGTU gtu, final Time startTime,
268 final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final boolean deviative)
269 throws OperationalPlanException, OTSGeometryException
270 {
271 if (startSpeed.si <= OperationalPlan.DRIFTING_SPEED_SI && acceleration.le(Acceleration.ZERO))
272 {
273 return new LaneBasedOperationalPlan(gtu, gtu.getLocation(), startTime, timeStep, deviative);
274 }
275
276 Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
277 Length distance =
278 Length.instantiateSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
279 List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
280 if (distance.le(MINIMUM_CREDIBLE_PATH_LENGTH))
281 {
282 return new LaneBasedOperationalPlan(gtu, gtu.getLocation(), startTime, timeStep, deviative);
283 }
284 OTSLine3D path = createPathAlongCenterLine(gtu, distance);
285 return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, deviative);
286 }
287
288 /**
289 * Creates a path along lane center lines.
290 * @param gtu LaneBasedGTU; gtu
291 * @param distance Length; minimum distance
292 * @return OTSLine3D; path along lane center lines
293 * @throws OTSGeometryException when any of the OTSLine3D operations fails
294 */
295 public static OTSLine3D createPathAlongCenterLine(final LaneBasedGTU gtu, final Length distance) throws OTSGeometryException
296 {
297 // if (gtu.getId().equals("1669") && gtu.getSimulator().getSimulatorTime().si >= 2508.9)
298 // {
299 // System.out.println("processing gtu " + gtu);
300 // try
301 // {
302 // for (Lane l : gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).keySet())
303 // {
304 // System.out.println("fractional position on lane " + l + ": "
305 // + gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).get(l));
306 // }
307 // System.out.println("reference position is " + gtu.getReferencePosition());
308 // System.out.println("operational plan path is " + gtu.getOperationalPlan().getPath());
309 // }
310 // catch (GTUException e)
311 // {
312 // e.printStackTrace();
313 // }
314 // }
315 OTSLine3D path = null;
316 try
317 {
318 DirectedLanePosition ref = gtu.getReferencePosition();
319 double f = ref.getLane().fraction(ref.getPosition());
320 if (ref.getGtuDirection().isPlus() && f < 1.0)
321 {
322 if (f >= 0.0)
323 {
324 path = ref.getLane().getCenterLine().extractFractional(f, 1.0);
325 }
326 else
327 {
328 path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0);
329 }
330 }
331 else if (ref.getGtuDirection().isMinus() && f > 0.0)
332 {
333 if (f <= 1.0)
334 {
335 path = ref.getLane().getCenterLine().extractFractional(0.0, f).reverse();
336 }
337 else
338 {
339 path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0).reverse();
340 }
341 }
342 // if (gtu.getId().equals("1669") && gtu.getSimulator().getSimulatorTime().si >= 2508.9)
343 // {
344 // System.out.println("First part of path is " + path);
345 // }
346 LaneDirection prevFrom = null;
347 LaneDirection from = ref.getLaneDirection();
348 int n = 1;
349 boolean alternativeTried = false;
350 while (path == null || path.getLength().si < distance.si + n * Lane.MARGIN.si)
351 {
352 n++;
353 prevFrom = from;
354 if (null == from)
355 {
356 CategoryLogger.always().warn("About to die: GTU {} has null from value", gtu.getId());
357 }
358 from = from.getNextLaneDirection(gtu);
359 if (from == null)
360 {
361 // check sink sensor
362 Length pos = prevFrom.getDirection().isPlus() ? prevFrom.getLength() : Length.ZERO;
363 for (SingleSensor sensor : prevFrom.getLane().getSensors(pos, pos, gtu.getGTUType(),
364 prevFrom.getDirection()))
365 {
366 // XXX for now, the same is not done for the DestinationSensor (e.g., decrease speed for parking)
367 if (sensor instanceof SinkSensor)
368 {
369 // just add some length so the GTU is happy to go to the sink
370 DirectedPoint end = path.getLocationExtendedSI(distance.si + n * Lane.MARGIN.si);
371 List<OTSPoint3D> points = new ArrayList<>(Arrays.asList(path.getPoints()));
372 points.add(new OTSPoint3D(end));
373 return new OTSLine3D(points);
374 }
375 }
376 // START CLEVER
377 /*-
378 if (!alternativeTried)
379 {
380 for (Lane l : gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).keySet())
381 {
382 if (ref.getLane().equals(l))
383 {
384 continue;
385 }
386 CategoryLogger.always().warn("GTU {} dead end on {}; but reference position is on {}; trying that",
387 gtu.getId(), ref, l);
388 // Figure out the driving direction and position on Lane l
389 // For now assume that lane l and ref are lanes on the same parent link. If not, chaos may occur
390 if (!l.getParentLink().equals(ref.getLane().getParentLink()))
391 {
392 CategoryLogger.always()
393 .error("Assumption that l and ref.getLane are on same Link does not hold");
394 }
395 from = new LaneDirection(l, ref.getGtuDirection());
396 if (ref.getGtuDirection().isPlus() && f < 1.0)
397 {
398 if (f >= 0.0)
399 {
400 path = l.getCenterLine().extractFractional(f, 1.0);
401 }
402 else
403 {
404 path = l.getCenterLine().extractFractional(0.0, 1.0);
405 }
406 }
407 else if (ref.getGtuDirection().isMinus() && f > 0.0)
408 {
409 if (f <= 1.0)
410 {
411 path = l.getCenterLine().extractFractional(0.0, f).reverse();
412 }
413 else
414 {
415 path = l.getCenterLine().extractFractional(0.0, 1.0).reverse();
416 }
417 }
418 alternativeTried = true;
419 }
420 if (null != from)
421 {
422 continue;
423 }
424 }
425 */
426 // END CLEVER
427 CategoryLogger.always().error("GTU {} has nowhere to go and no sink sensor either", gtu);
428 // gtu.getReferencePosition(); // CLEVER
429 gtu.destroy();
430 return path;
431 }
432 if (path == null)
433 {
434 path = from.getDirection().isPlus() ? from.getLane().getCenterLine()
435 : from.getLane().getCenterLine().reverse();
436 }
437 else
438 {
439 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, from.getDirection().isPlus()
440 ? from.getLane().getCenterLine() : from.getLane().getCenterLine().reverse());
441 }
442 }
443 }
444 catch (GTUException exception)
445 {
446 throw new RuntimeException("Error during creation of path.", exception);
447 }
448 return path;
449 }
450
451 /**
452 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
453 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
454 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
455 * before completing the path, a truncated path that ends where the GTU stops is used instead.
456 * @param gtu LaneBasedGTU; the GTU for debugging purposes
457 * @param laneChangeDirectionality LateralDirectionality; direction of lane change (on initiation only, after that not
458 * important)
459 * @param startPosition DirectedPoint; current position
460 * @param startTime Time; the current time or a time in the future when the plan should start
461 * @param startSpeed Speed; the speed at the start of the path
462 * @param acceleration Acceleration; the acceleration to use
463 * @param timeStep Duration; time step for the plan
464 * @param laneChange LaneChange; lane change status
465 * @return the operational plan to accomplish the given end speed
466 * @throws OperationalPlanException when the construction of the operational path fails
467 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
468 * first lane
469 */
470 @SuppressWarnings("checkstyle:parameternumber")
471 public static LaneBasedOperationalPlan buildAccelerationLaneChangePlan(final LaneBasedGTU gtu,
472 final LateralDirectionality laneChangeDirectionality, final DirectedPoint startPosition, final Time startTime,
473 final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final LaneChange laneChange)
474 throws OperationalPlanException, OTSGeometryException
475 {
476
477 // on first call during lane change, use laneChangeDirectionality as laneChange.getDirection() is NONE
478 // on successive calls, use laneChange.getDirection() as laneChangeDirectionality is NONE (i.e. no LC initiated)
479 LateralDirectionality direction = laneChange.isChangingLane() ? laneChange.getDirection() : laneChangeDirectionality;
480
481 Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
482 Length planDistance =
483 Length.instantiateSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
484 List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
485
486 try
487 {
488 // get position on from lane
489 Map<Lane, Length> positions = gtu.positions(gtu.getReference());
490 DirectedLanePosition ref = gtu.getReferencePosition();
491 Iterator<Lane> iterator = ref.getLane()
492 .accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection()).iterator();
493 Lane adjLane = iterator.hasNext() ? iterator.next() : null;
494 DirectedLanePosition from = null;
495 if (laneChange.getDirection() == null || (adjLane != null && positions.containsKey(adjLane)))
496 {
497 // reference lane is from lane, this is ok
498 from = ref;
499 }
500 else
501 {
502 // reference lane is to lane, this should be accounted for
503 for (Lane lane : positions.keySet())
504 {
505 if (lane.accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection())
506 .contains(ref.getLane()))
507 {
508 from = new DirectedLanePosition(lane, positions.get(lane), ref.getGtuDirection());
509 break;
510 }
511 }
512 }
513 Throw.when(from == null, RuntimeException.class, "From lane could not be determined during lane change.");
514
515 // get path and make plan
516 OTSLine3D path = laneChange.getPath(timeStep, gtu, from, startPosition, planDistance, direction);
517 LaneBasedOperationalPlanan/operational/LaneBasedOperationalPlan.html#LaneBasedOperationalPlan">LaneBasedOperationalPlan plan = new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, true);
518 return plan;
519 }
520 catch (GTUException exception)
521 {
522 throw new RuntimeException("Error during creation of lane change plan.", exception);
523 }
524 }
525
526 /**
527 * Returns the effective braking time, which stops if stand-still is reached.
528 * @param acceleration Acceleration; acceleration
529 * @param startSpeed Speed; start speed
530 * @param time Duration; intended time step
531 * @return Duration; effective braking time
532 */
533 public static Duration brakingTime(final Acceleration acceleration, final Speed startSpeed, final Duration time)
534 {
535 if (acceleration.ge0())
536 {
537 return time;
538 }
539 double t = startSpeed.si / -acceleration.si;
540 if (t >= time.si)
541 {
542 return time;
543 }
544 return Duration.instantiateSI(t);
545 }
546
547 /**
548 * Creates 1 or 2 segments in an operational plan. Two segments are returned of stand-still is reached within the time step.
549 * @param startSpeed Speed; start speed
550 * @param acceleration Acceleration; acceleration
551 * @param brakingTime Duration; braking time until stand-still
552 * @param timeStep Duration; time step
553 * @return 1 or 2 segments in an operational plan
554 */
555 private static List<Segment> createAccelerationSegments(final Speed startSpeed, final Acceleration acceleration,
556 final Duration brakingTime, final Duration timeStep)
557 {
558 List<Segment> segmentList = new ArrayList<>();
559 if (brakingTime.si < timeStep.si)
560 {
561 if (brakingTime.si > 0.0)
562 {
563 segmentList.add(new OperationalPlan.AccelerationSegment(brakingTime, acceleration));
564 }
565 segmentList.add(new OperationalPlan.SpeedSegment(timeStep.minus(brakingTime)));
566 }
567 else
568 {
569 segmentList.add(new OperationalPlan.AccelerationSegment(timeStep, acceleration));
570 }
571 return segmentList;
572 }
573
574 /**
575 * Build an operational plan based on a simple operational plan and status info.
576 * @param gtu LaneBasedGTU; gtu
577 * @param startTime Time; start time for plan
578 * @param simplePlan SimpleOperationalPlan; simple operational plan
579 * @param laneChange LaneChange; lane change status
580 * @return operational plan
581 * @throws ParameterException if parameter is not defined
582 * @throws GTUException gtu exception
583 * @throws NetworkException network exception
584 * @throws OperationalPlanException operational plan exeption
585 */
586 public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGTU gtu, final Time startTime,
587 final SimpleOperationalPlan simplePlan, final LaneChange laneChange)
588 throws ParameterException, GTUException, NetworkException, OperationalPlanException
589 {
590 Acceleration acc = gtu.getVehicleModel().boundAcceleration(simplePlan.getAcceleration(), gtu);
591
592 if (gtu.isInstantaneousLaneChange())
593 {
594 if (simplePlan.isLaneChange())
595 {
596 gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
597 }
598 try
599 {
600 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
601 simplePlan.getDuration(), false);
602 }
603 catch (OTSGeometryException exception)
604 {
605 throw new OperationalPlanException(exception);
606 }
607 }
608
609 // gradual lane change
610 try
611 {
612 if (!simplePlan.isLaneChange() && !laneChange.isChangingLane())
613 {
614 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
615 simplePlan.getDuration(), true);
616 }
617 if (gtu.getSpeed().si == 0.0 && acc.si <= 0.0)
618 {
619 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
620 simplePlan.getDuration(), false);
621 }
622 return LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(gtu, simplePlan.getLaneChangeDirection(),
623 gtu.getLocation(), startTime, gtu.getSpeed(), acc, simplePlan.getDuration(), laneChange);
624 }
625 catch (OTSGeometryException exception)
626 {
627 throw new OperationalPlanException(exception);
628 }
629 }
630
631 /**
632 * Schedules a lane change finalization after the given distance is covered. This distance is known as the plan is created,
633 * but at that point no time can be derived as the plan is required for that. Hence, this method can be scheduled at the
634 * same time (sequentially after creation of the plan) to then schedule the actual finalization by deriving time from
635 * distance with the plan.
636 * @param gtu LaneBasedGTU; gtu
637 * @param distance Length; distance
638 * @param laneChangeDirection LateralDirectionality; lane change direction
639 * @throws SimRuntimeException on bad time
640 */
641 public static void scheduleLaneChangeFinalization(final LaneBasedGTU gtu, final Length distance,
642 final LateralDirectionality laneChangeDirection) throws SimRuntimeException
643 {
644 Time time = gtu.getOperationalPlan().timeAtDistance(distance);
645 if (Double.isNaN(time.si))
646 {
647 // rounding...
648 time = gtu.getOperationalPlan().getEndTime();
649 }
650 SimEventInterface<SimTimeDoubleUnit> event = gtu.getSimulator().scheduleEventAbs(time, (short) 6, gtu, gtu,
651 "finalizeLaneChange", new Object[] { laneChangeDirection });
652 gtu.setFinalizeLaneChangeEvent(event);
653 }
654
655 /**
656 * Build a plan with a path and a given start speed to try to come to a stop with a given deceleration. If the GTU can stop
657 * before completing the given path, a truncated path that ends where the GTU stops is used instead. There is no guarantee
658 * that the OperationalPlan will lead to a complete stop.
659 * @param gtu LaneBasedGTU; the GTU for debugging purposes
660 * @param distance Length; distance to drive for reaching the end speed
661 * @param startTime Time; the current time or a time in the future when the plan should start
662 * @param startSpeed Speed; the speed at the start of the path
663 * @param deceleration Acceleration; the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
664 * @return the operational plan to accomplish the given end speed
665 * @throws OperationalPlanException when construction of the operational path fails
666 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
667 * first lane
668 */
669 public static LaneBasedOperationalPlan buildStopPlan(final LaneBasedGTU gtu, final Length distance, final Time startTime,
670 final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
671 {
672 return buildMaximumAccelerationPlan(gtu, distance, startTime, startSpeed, new Speed(0.0, SpeedUnit.SI),
673 new Acceleration(1.0, AccelerationUnit.SI), deceleration);
674 }
675
676 }