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.opentrafficsim.base.parameters.ParameterException;
21 import org.opentrafficsim.core.geometry.OTSGeometryException;
22 import org.opentrafficsim.core.geometry.OTSLine3D;
23 import org.opentrafficsim.core.geometry.OTSPoint3D;
24 import org.opentrafficsim.core.gtu.GTUException;
25 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
26 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
27 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.SpeedSegment;
28 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
29 import org.opentrafficsim.core.math.Solver;
30 import org.opentrafficsim.core.network.LateralDirectionality;
31 import org.opentrafficsim.core.network.NetworkException;
32 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
33 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
34 import org.opentrafficsim.road.network.lane.Lane;
35 import org.opentrafficsim.road.network.lane.LaneDirection;
36 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
37 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
38
39 import nl.tudelft.simulation.dsol.SimRuntimeException;
40 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
41 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
42 import nl.tudelft.simulation.language.d3.DirectedPoint;
43
44 /**
45 * Builder for several often used operational plans. E.g., decelerate to come to a full stop at the end of a shape; accelerate
46 * to reach a certain speed at the end of a curve; drive constant on a curve; decelerate or accelerate to reach a given end
47 * speed at the end of a curve, etc.<br>
48 * TODO driving with negative speeds (backward driving) is not yet supported.
49 * <p>
50 * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
51 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
52 * </p>
53 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
54 * initial version Nov 15, 2015 <br>
55 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
56 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
57 */
58 public final class LaneOperationalPlanBuilder // class package private for scheduling static method on an instance
59 {
60
61 /** Maximum acceleration for unbounded accelerations: 1E12 m/s2. */
62 private static final Acceleration MAX_ACCELERATION = new Acceleration(1E12, AccelerationUnit.SI);
63
64 /** Maximum deceleration for unbounded accelerations: -1E12 m/s2. */
65 private static final Acceleration MAX_DECELERATION = new Acceleration(-1E12, AccelerationUnit.SI);
66
67 /**
68 * Minimum distance of an operational plan path; anything shorter will be truncated to 0. <br>
69 * If objects related to e.g. molecular movements are simulated using this code, a setter for this parameter will be needed.
70 */
71 private static final Length MINIMUM_CREDIBLE_PATH_LENGTH = new Length(0.001, LengthUnit.METER);
72
73 /** Constructor. */
74 LaneOperationalPlanBuilder()
75 {
76 // class should not be instantiated
77 }
78
79 /**
80 * 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.
81 * The acceleration (and deceleration) are capped by maxAcceleration and maxDeceleration. Therefore, there is no guarantee
82 * that the end speed is actually reached by this plan.
83 * @param gtu LaneBasedGTU; the GTU for debugging purposes
84 * @param distance Length; distance to drive for reaching the end speed
85 * @param startTime Time; the current time or a time in the future when the plan should start
86 * @param startSpeed Speed; the speed at the start of the path
87 * @param endSpeed Speed; the required end speed
88 * @param maxAcceleration Acceleration; the maximum acceleration that can be applied, provided as a POSITIVE number
89 * @param maxDeceleration Acceleration; the maximum deceleration that can be applied, provided as a NEGATIVE number
90 * @return the operational plan to accomplish the given end speed
91 * @throws OperationalPlanException when the plan cannot be generated, e.g. because of a path that is too short
92 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
93 * constructed segment list differ more than a given threshold
94 * @throws OTSGeometryException in case the lanes are not connected or firstLanePosition is larger than the length of the
95 * first lane
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 // t = 2x / (vt + v0); a = (vt - v0) / t
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 * Build a plan with a path and a given start speed to reach a provided end speed, exactly at the end of the curve.
140 * Acceleration and deceleration are virtually unbounded (1E12 m/s2) to reach the end speed (e.g., to come to a complete
141 * stop).
142 * @param gtu LaneBasedGTU; the GTU for debugging purposes
143 * @param distance Length; distance to drive for reaching the end speed
144 * @param startTime Time; the current time or a time in the future when the plan should start
145 * @param startSpeed Speed; the speed at the start of the path
146 * @param endSpeed Speed; the required end speed
147 * @return the operational plan to accomplish the given end speed
148 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
149 * constructed segment list differ more than a given threshold
150 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
151 * first lane
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 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
162 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
163 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
164 * before completing the path, a truncated path that ends where the GTU stops is used instead.
165 * @param gtu LaneBasedGTU; the GTU for debugging purposes
166 * @param distance Length; distance to drive for reaching the end speed
167 * @param startTime Time; the current time or a time in the future when the plan should start
168 * @param startSpeed Speed; the speed at the start of the path
169 * @param endSpeed Speed; the required end speed
170 * @param acceleration Acceleration; the acceleration to use if endSpeed > startSpeed, provided as a POSITIVE number
171 * @param deceleration Acceleration; the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
172 * @return the operational plan to accomplish the given end speed
173 * @throws OperationalPlanException when the construction of the operational path fails
174 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
175 * first lane
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 // we cannot reach the end speed in the given distance with the given acceleration
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 // we reach the (higher) end speed before the end of the segment. Make two segments.
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 // we cannot reach the end speed in the given distance with the given deceleration
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 // if endSpeed == 0, we cannot reach the end of the path. Therefore, build a partial path.
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 // we reach the (lower) end speed, larger than zero, before the end of the segment. Make two segments.
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 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
250 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
251 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
252 * before completing the path, a truncated path that ends where the GTU stops is used instead.
253 * @param gtu LaneBasedGTU; the GTU for debugging purposes
254 * @param startTime Time; the current time or a time in the future when the plan should start
255 * @param startSpeed Speed; the speed at the start of the path
256 * @param acceleration Acceleration; the acceleration to use
257 * @param timeStep Duration; time step for the plan
258 * @param deviative boolean; whether the plan is deviative
259 * @return the operational plan to accomplish the given end speed
260 * @throws OperationalPlanException when the construction of the operational path fails
261 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
262 * first lane
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 * Creates a path along lane center lines.
287 * @param gtu LaneBasedGTU; gtu
288 * @param distance Length; minimum distance
289 * @return OTSLine3D; path along lane center lines
290 * @throws OTSGeometryException when any of the OTSLine3D operations fails
291 */
292 public static OTSLine3D createPathAlongCenterLine(final LaneBasedGTU gtu, final Length distance) throws OTSGeometryException
293 {
294 OTSLine3D path = null;
295 try
296 {
297 DirectedLanePosition ref = gtu.getReferencePosition();
298 double f = ref.getLane().fraction(ref.getPosition());
299 if (ref.getGtuDirection().isPlus() && f < 1.0)
300 {
301 if (f >= 0.0)
302 {
303 path = ref.getLane().getCenterLine().extractFractional(f, 1.0);
304 }
305 else
306 {
307 path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0);
308 }
309 }
310 else if (ref.getGtuDirection().isMinus() && f > 0.0)
311 {
312 if (f <= 1.0)
313 {
314 path = ref.getLane().getCenterLine().extractFractional(0.0, f).reverse();
315 }
316 else
317 {
318 path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0).reverse();
319 }
320 }
321 LaneDirection prevFrom = null;
322 LaneDirection from = ref.getLaneDirection();
323 int n = 1;
324 while (path == null || path.getLength().si < distance.si + n * Lane.MARGIN.si)
325 {
326 n++;
327 prevFrom = from;
328 from = from.getNextLaneDirection(gtu);
329 if (from == null)
330 {
331 // check sink sensor
332 Length pos = prevFrom.getDirection().isPlus() ? prevFrom.getLength() : Length.ZERO;
333 for (SingleSensor sensor : prevFrom.getLane().getSensors(pos, pos, gtu.getGTUType(),
334 prevFrom.getDirection()))
335 {
336 // XXX for now, the same is not done for the DestinationSensor (e.g., decrease speed for parking)
337 if (sensor instanceof SinkSensor)
338 {
339 // just add some length so the GTU is happy to go to the sink
340 DirectedPoint end = path.getLocationExtendedSI(distance.si + n * Lane.MARGIN.si);
341 List<OTSPoint3D> points = new ArrayList<>(Arrays.asList(path.getPoints()));
342 points.add(new OTSPoint3D(end));
343 return new OTSLine3D(points);
344 }
345 }
346 }
347 if (path == null)
348 {
349 path = from.getDirection().isPlus() ? from.getLane().getCenterLine()
350 : from.getLane().getCenterLine().reverse();
351 }
352 else
353 {
354 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, from.getDirection().isPlus()
355 ? from.getLane().getCenterLine() : from.getLane().getCenterLine().reverse());
356 }
357 }
358 }
359 catch (GTUException exception)
360 {
361 throw new RuntimeException("Error during creation of path.", exception);
362 }
363 return path;
364 }
365
366 /**
367 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
368 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
369 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
370 * before completing the path, a truncated path that ends where the GTU stops is used instead.
371 * @param gtu LaneBasedGTU; the GTU for debugging purposes
372 * @param laneChangeDirectionality LateralDirectionality; direction of lane change (on initiation only, after that not
373 * important)
374 * @param startPosition DirectedPoint; current position
375 * @param startTime Time; the current time or a time in the future when the plan should start
376 * @param startSpeed Speed; the speed at the start of the path
377 * @param acceleration Acceleration; the acceleration to use
378 * @param timeStep Duration; time step for the plan
379 * @param laneChange LaneChange; lane change status
380 * @return the operational plan to accomplish the given end speed
381 * @throws OperationalPlanException when the construction of the operational path fails
382 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
383 * first lane
384 */
385 @SuppressWarnings("checkstyle:parameternumber")
386 public static LaneBasedOperationalPlan buildAccelerationLaneChangePlan(final LaneBasedGTU gtu,
387 final LateralDirectionality laneChangeDirectionality, final DirectedPoint startPosition, final Time startTime,
388 final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final LaneChange laneChange)
389 throws OperationalPlanException, OTSGeometryException
390 {
391
392 // on first call during lane change, use laneChangeDirectionality as laneChange.getDirection() is NONE
393 // on successive calls, use laneChange.getDirection() as laneChangeDirectionality is NONE (i.e. no LC initiated)
394 LateralDirectionality direction = laneChange.isChangingLane() ? laneChange.getDirection() : laneChangeDirectionality;
395
396 Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
397 Length planDistance =
398 Length.instantiateSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
399 List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
400
401 try
402 {
403 // get position on from lane
404 Map<Lane, Length> positions = gtu.positions(gtu.getReference());
405 DirectedLanePosition ref = gtu.getReferencePosition();
406 Iterator<Lane> iterator = ref.getLane()
407 .accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection()).iterator();
408 Lane adjLane = iterator.hasNext() ? iterator.next() : null;
409 DirectedLanePosition from = null;
410 if (laneChange.getDirection() == null || (adjLane != null && positions.containsKey(adjLane)))
411 {
412 // reference lane is from lane, this is ok
413 from = ref;
414 }
415 else
416 {
417 // reference lane is to lane, this should be accounted for
418 for (Lane lane : positions.keySet())
419 {
420 if (lane.accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection())
421 .contains(ref.getLane()))
422 {
423 from = new DirectedLanePosition(lane, positions.get(lane), ref.getGtuDirection());
424 break;
425 }
426 }
427 }
428 Throw.when(from == null, RuntimeException.class, "From lane could not be determined during lane change.");
429
430 // get path and make plan
431 OTSLine3D path = laneChange.getPath(timeStep, gtu, from, startPosition, planDistance, direction);
432 LaneBasedOperationalPlanan/operational/LaneBasedOperationalPlan.html#LaneBasedOperationalPlan">LaneBasedOperationalPlan plan = new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, true);
433 return plan;
434 }
435 catch (GTUException exception)
436 {
437 throw new RuntimeException("Error during creation of lane change plan.", exception);
438 }
439 }
440
441 /**
442 * Returns the effective braking time, which stops if stand-still is reached.
443 * @param acceleration Acceleration; acceleration
444 * @param startSpeed Speed; start speed
445 * @param time Duration; intended time step
446 * @return Duration; effective braking time
447 */
448 public static Duration brakingTime(final Acceleration acceleration, final Speed startSpeed, final Duration time)
449 {
450 if (acceleration.ge0())
451 {
452 return time;
453 }
454 double t = startSpeed.si / -acceleration.si;
455 if (t >= time.si)
456 {
457 return time;
458 }
459 return Duration.instantiateSI(t);
460 }
461
462 /**
463 * Creates 1 or 2 segments in an operational plan. Two segments are returned of stand-still is reached within the time step.
464 * @param startSpeed Speed; start speed
465 * @param acceleration Acceleration; acceleration
466 * @param brakingTime Duration; braking time until stand-still
467 * @param timeStep Duration; time step
468 * @return 1 or 2 segments in an operational plan
469 */
470 private static List<Segment> createAccelerationSegments(final Speed startSpeed, final Acceleration acceleration,
471 final Duration brakingTime, final Duration timeStep)
472 {
473 List<Segment> segmentList = new ArrayList<>();
474 if (brakingTime.si < timeStep.si)
475 {
476 if (brakingTime.si > 0.0)
477 {
478 segmentList.add(new OperationalPlan.AccelerationSegment(brakingTime, acceleration));
479 }
480 segmentList.add(new OperationalPlan.SpeedSegment(timeStep.minus(brakingTime)));
481 }
482 else
483 {
484 segmentList.add(new OperationalPlan.AccelerationSegment(timeStep, acceleration));
485 }
486 return segmentList;
487 }
488
489 /**
490 * Build an operational plan based on a simple operational plan and status info.
491 * @param gtu LaneBasedGTU; gtu
492 * @param startTime Time; start time for plan
493 * @param simplePlan SimpleOperationalPlan; simple operational plan
494 * @param laneChange LaneChange; lane change status
495 * @return operational plan
496 * @throws ParameterException if parameter is not defined
497 * @throws GTUException gtu exception
498 * @throws NetworkException network exception
499 * @throws OperationalPlanException operational plan exeption
500 */
501 public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGTU gtu, final Time startTime,
502 final SimpleOperationalPlan simplePlan, final LaneChange laneChange)
503 throws ParameterException, GTUException, NetworkException, OperationalPlanException
504 {
505 Acceleration acc = gtu.getVehicleModel().boundAcceleration(simplePlan.getAcceleration(), gtu);
506
507 if (gtu.isInstantaneousLaneChange())
508 {
509 if (simplePlan.isLaneChange())
510 {
511 gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
512 }
513 try
514 {
515 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
516 simplePlan.getDuration(), false);
517 }
518 catch (OTSGeometryException exception)
519 {
520 throw new OperationalPlanException(exception);
521 }
522 }
523
524 // gradual lane change
525 try
526 {
527 if (!simplePlan.isLaneChange() && !laneChange.isChangingLane())
528 {
529 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
530 simplePlan.getDuration(), true);
531 }
532 if (gtu.getSpeed().si == 0.0 && acc.si <= 0.0)
533 {
534 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
535 simplePlan.getDuration(), false);
536 }
537 return LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(gtu, simplePlan.getLaneChangeDirection(),
538 gtu.getLocation(), startTime, gtu.getSpeed(), acc, simplePlan.getDuration(), laneChange);
539 }
540 catch (OTSGeometryException exception)
541 {
542 throw new OperationalPlanException(exception);
543 }
544 }
545
546 /**
547 * Schedules a lane change finalization after the given distance is covered. This distance is known as the plan is created,
548 * but at that point no time can be derived as the plan is required for that. Hence, this method can be scheduled at the
549 * same time (sequentially after creation of the plan) to then schedule the actual finalization by deriving time from
550 * distance with the plan.
551 * @param gtu LaneBasedGTU; gtu
552 * @param distance Length; distance
553 * @param laneChangeDirection LateralDirectionality; lane change direction
554 * @throws SimRuntimeException on bad time
555 */
556 public static void scheduleLaneChangeFinalization(final LaneBasedGTU gtu, final Length distance,
557 final LateralDirectionality laneChangeDirection) throws SimRuntimeException
558 {
559 Time time = gtu.getOperationalPlan().timeAtDistance(distance);
560 if (Double.isNaN(time.si))
561 {
562 // rounding...
563 time = gtu.getOperationalPlan().getEndTime();
564 }
565 SimEventInterface<SimTimeDoubleUnit> event = gtu.getSimulator().scheduleEventAbs(time, (short) 6, gtu, gtu,
566 "finalizeLaneChange", new Object[] { laneChangeDirection });
567 gtu.setFinalizeLaneChangeEvent(event);
568 }
569
570 /**
571 * 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
572 * before completing the given path, a truncated path that ends where the GTU stops is used instead. There is no guarantee
573 * that the OperationalPlan will lead to a complete stop.
574 * @param gtu LaneBasedGTU; the GTU for debugging purposes
575 * @param distance Length; distance to drive for reaching the end speed
576 * @param startTime Time; the current time or a time in the future when the plan should start
577 * @param startSpeed Speed; the speed at the start of the path
578 * @param deceleration Acceleration; the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
579 * @return the operational plan to accomplish the given end speed
580 * @throws OperationalPlanException when construction of the operational path fails
581 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
582 * first lane
583 */
584 public static LaneBasedOperationalPlan buildStopPlan(final LaneBasedGTU gtu, final Length distance, final Time startTime,
585 final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
586 {
587 return buildMaximumAccelerationPlan(gtu, distance, startTime, startSpeed, new Speed(0.0, SpeedUnit.SI),
588 new Acceleration(1.0, AccelerationUnit.SI), deceleration);
589 }
590
591 }