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