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 if (ref.getGtuDirection().isPlus() && f < 1.0)
298 {
299 path = ref.getLane().getCenterLine().extractFractional(f, 1.0);
300 }
301 else if (ref.getGtuDirection().isMinus() && f > 0.0)
302 {
303 path = ref.getLane().getCenterLine().extractFractional(0.0, f).reverse();
304 }
305 LaneDirection prevFrom = null;
306 LaneDirection from = ref.getLaneDirection();
307 int n = 1;
308 while (path == null || path.getLength().si < distance.si + n * Lane.MARGIN.si)
309 {
310 n++;
311 prevFrom = from;
312 from = from.getNextLaneDirection(gtu);
313 if (path == null)
314 {
315 path = from.getDirection().isPlus() ? from.getLane().getCenterLine()
316 : from.getLane().getCenterLine().reverse();
317 }
318 else
319 {
320 try
321 {
322 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, from.getDirection().isPlus()
323 ? from.getLane().getCenterLine() : from.getLane().getCenterLine().reverse());
324 }
325 catch (NullPointerException nas)
326 {
327 prevFrom.getNextLaneDirection(gtu);
328 ref.getLaneDirection().getNextLaneDirection(gtu);
329 }
330 }
331 }
332 }
333 catch (GTUException exception)
334 {
335 throw new RuntimeException("Error during creation of path.", exception);
336 }
337 return path;
338 }
339
340 /**
341 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
342 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
343 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
344 * before completing the path, a truncated path that ends where the GTU stops is used instead.
345 * @param gtu LaneBasedGTU; the GTU for debugging purposes
346 * @param laneChangeDirectionality LateralDirectionality; direction of lane change (on initiation only, after that not
347 * important)
348 * @param startPosition DirectedPoint; current position
349 * @param startTime Time; the current time or a time in the future when the plan should start
350 * @param startSpeed Speed; the speed at the start of the path
351 * @param acceleration Acceleration; the acceleration to use
352 * @param timeStep Duration; time step for the plan
353 * @param laneChange LaneChange; lane change status
354 * @return the operational plan to accomplish the given end speed
355 * @throws OperationalPlanException when the construction of the operational path fails
356 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
357 * first lane
358 */
359 @SuppressWarnings("checkstyle:parameternumber")
360 public static LaneBasedOperationalPlan buildAccelerationLaneChangePlan(final LaneBasedGTU gtu,
361 final LateralDirectionality laneChangeDirectionality, final DirectedPoint startPosition, final Time startTime,
362 final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final LaneChange laneChange)
363 throws OperationalPlanException, OTSGeometryException
364 {
365
366 // on first call during lane change, use laneChangeDirectionality as laneChange.getDirection() is NONE
367 // on successive calls, use laneChange.getDirection() as laneChangeDirectionality is NONE (i.e. no LC initiated)
368 LateralDirectionality direction = laneChange.isChangingLane() ? laneChange.getDirection() : laneChangeDirectionality;
369
370 Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
371 Length planDistance =
372 Length.createSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
373 List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
374
375 try
376 {
377 // get position on from lane
378 Map<Lane, Length> positions = gtu.positions(gtu.getReference());
379 DirectedLanePosition ref = gtu.getReferencePosition();
380 Iterator<Lane> iterator = ref.getLane()
381 .accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection()).iterator();
382 Lane adjLane = iterator.hasNext() ? iterator.next() : null;
383 DirectedLanePosition from = null;
384 if (laneChange.getDirection() == null || (adjLane != null && positions.containsKey(adjLane)))
385 {
386 // reference lane is from lane, this is ok
387 from = ref;
388 }
389 else
390 {
391 // reference lane is to lane, this should be accounted for
392 for (Lane lane : positions.keySet())
393 {
394 if (lane.accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection())
395 .contains(ref.getLane()))
396 {
397 from = new DirectedLanePosition(lane, positions.get(lane), ref.getGtuDirection());
398 break;
399 }
400 }
401 }
402 Throw.when(from == null, RuntimeException.class, "From lane could not be determined during lane change.");
403
404 // get path and make plan
405 OTSLine3D path = laneChange.getPath(timeStep, gtu, from, startPosition, planDistance, direction);
406 LaneBasedOperationalPlan plan = new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, true);
407 return plan;
408 }
409 catch (GTUException exception)
410 {
411 throw new RuntimeException("Error during creation of lane change plan.", exception);
412 }
413 }
414
415 /**
416 * Makes the acceleration adhere to GTU capabilities.
417 * @param acceleration Acceleration; desired acceleration
418 * @param gtu LaneBasedGTU; gtu
419 * @return Acceleration; possible acceleration
420 */
421 private static Acceleration gtuCapabilities(final Acceleration acceleration, final LaneBasedGTU gtu)
422 {
423 return acceleration; // .gt(gtu.getMaximumDeceleration())
424 // ? (acceleration.lt(gtu.getMaximumAcceleration()) ? acceleration : gtu.getMaximumAcceleration())
425 // : gtu.getMaximumDeceleration();
426 }
427
428 /**
429 * Returns the effective braking time, which stops if stand-still is reached.
430 * @param acceleration Acceleration; acceleration
431 * @param startSpeed Speed; start speed
432 * @param time Duration; intended time step
433 * @return Duration; effective braking time
434 */
435 public static Duration brakingTime(final Acceleration acceleration, final Speed startSpeed, final Duration time)
436 {
437 if (acceleration.ge0())
438 {
439 return time;
440 }
441 double t = startSpeed.si / -acceleration.si;
442 if (t >= time.si)
443 {
444 return time;
445 }
446 return Duration.createSI(t);
447 }
448
449 /**
450 * Creates 1 or 2 segments in an operational plan. Two segments are returned of stand-still is reached within the time step.
451 * @param startSpeed Speed; start speed
452 * @param acceleration Acceleration; acceleration
453 * @param brakingTime Duration; braking time until stand-still
454 * @param timeStep Duration; time step
455 * @return 1 or 2 segments in an operational plan
456 */
457 private static List<Segment> createAccelerationSegments(final Speed startSpeed, final Acceleration acceleration,
458 final Duration brakingTime, final Duration timeStep)
459 {
460 List<Segment> segmentList = new ArrayList<>();
461 if (brakingTime.si < timeStep.si)
462 {
463 if (brakingTime.si > 0.0)
464 {
465 segmentList.add(new OperationalPlan.AccelerationSegment(brakingTime, acceleration));
466 }
467 segmentList.add(new OperationalPlan.SpeedSegment(timeStep.minus(brakingTime)));
468 }
469 else
470 {
471 segmentList.add(new OperationalPlan.AccelerationSegment(timeStep, acceleration));
472 }
473 return segmentList;
474 }
475
476 /**
477 * Build an operational plan based on a simple operational plan and status info.
478 * @param gtu LaneBasedGTU; gtu
479 * @param startTime Time; start time for plan
480 * @param simplePlan SimpleOperationalPlan; simple operational plan
481 * @param laneChange LaneChange; lane change status
482 * @return operational plan
483 * @throws ParameterException if parameter is not defined
484 * @throws GTUException gtu exception
485 * @throws NetworkException network exception
486 * @throws OperationalPlanException operational plan exeption
487 */
488 public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGTU gtu, final Time startTime,
489 final SimpleOperationalPlan simplePlan, final LaneChange laneChange)
490 throws ParameterException, GTUException, NetworkException, OperationalPlanException
491 {
492 Acceleration acc = gtu.getVehicleModel().boundAcceleration(simplePlan.getAcceleration(), gtu);
493
494 if (INSTANT_LANE_CHANGES)
495 {
496 if (simplePlan.isLaneChange())
497 {
498 gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
499 }
500 try
501 {
502 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
503 simplePlan.getDuration());
504 }
505 catch (OTSGeometryException exception)
506 {
507 throw new OperationalPlanException(exception);
508 }
509 }
510
511 // gradual lane change
512 try
513 {
514 if ((!simplePlan.isLaneChange() && !laneChange.isChangingLane()) || (gtu.getSpeed().si == 0.0 && acc.si <= 0.0))
515 {
516 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
517 simplePlan.getDuration());
518 }
519 return LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(gtu, simplePlan.getLaneChangeDirection(),
520 gtu.getLocation(), startTime, gtu.getSpeed(), acc, simplePlan.getDuration(), laneChange);
521 }
522 catch (OTSGeometryException exception)
523 {
524 throw new OperationalPlanException(exception);
525 }
526 }
527
528 /**
529 * Schedules a lane change finalization after the given distance is covered. This distance is known as the plan is created,
530 * but at that point no time can be derived as the plan is required for that. Hence, this method can be scheduled at the
531 * same time (sequentially after creation of the plan) to then schedule the actual finalization by deriving time from
532 * distance with the plan.
533 * @param gtu LaneBasedGTU; gtu
534 * @param distance Length; distance
535 * @param laneChangeDirection LateralDirectionality; lane change direction
536 * @throws SimRuntimeException on bad time
537 */
538 public static void scheduleLaneChangeFinalization(final LaneBasedGTU gtu, final Length distance,
539 final LateralDirectionality laneChangeDirection) throws SimRuntimeException
540 {
541 Time time = gtu.getOperationalPlan().timeAtDistance(distance);
542 if (Double.isNaN(time.si))
543 {
544 // rounding...
545 time = gtu.getOperationalPlan().getEndTime();
546 }
547 SimEventInterface<SimTimeDoubleUnit> event = gtu.getSimulator().scheduleEventAbs(time, (short) 6, gtu, gtu,
548 "finalizeLaneChange", new Object[] { laneChangeDirection });
549 gtu.setFinalizeLaneChangeEvent(event);
550 }
551
552 /**
553 * 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
554 * before completing the given path, a truncated path that ends where the GTU stops is used instead. There is no guarantee
555 * that the OperationalPlan will lead to a complete stop.
556 * @param gtu LaneBasedGTU; the GTU for debugging purposes
557 * @param distance Length; distance to drive for reaching the end speed
558 * @param startTime Time; the current time or a time in the future when the plan should start
559 * @param startSpeed Speed; the speed at the start of the path
560 * @param deceleration Acceleration; the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
561 * @return the operational plan to accomplish the given end speed
562 * @throws OperationalPlanException when construction of the operational path fails
563 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
564 * first lane
565 */
566 public static LaneBasedOperationalPlan buildStopPlan(final LaneBasedGTU gtu, final Length distance, final Time startTime,
567 final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
568 {
569 return buildMaximumAccelerationPlan(gtu, distance, startTime, startSpeed, new Speed(0.0, SpeedUnit.SI),
570 new Acceleration(1.0, AccelerationUnit.SI), deceleration);
571 }
572
573 }