1 package org.opentrafficsim.road.gtu.lane.plan.operational;
2
3 import java.util.ArrayList;
4 import java.util.Arrays;
5 import java.util.Iterator;
6 import java.util.List;
7 import java.util.Map;
8
9 import org.djunits.unit.AccelerationUnit;
10 import org.djunits.unit.DurationUnit;
11 import org.djunits.unit.LengthUnit;
12 import org.djunits.unit.SpeedUnit;
13 import org.djunits.value.ValueRuntimeException;
14 import org.djunits.value.vdouble.scalar.Acceleration;
15 import org.djunits.value.vdouble.scalar.Duration;
16 import org.djunits.value.vdouble.scalar.Length;
17 import org.djunits.value.vdouble.scalar.Speed;
18 import org.djunits.value.vdouble.scalar.Time;
19 import org.djutils.exceptions.Throw;
20 import org.djutils.logger.CategoryLogger;
21 import org.opentrafficsim.base.parameters.ParameterException;
22 import org.opentrafficsim.core.geometry.DirectedPoint;
23 import org.opentrafficsim.core.geometry.OTSGeometryException;
24 import org.opentrafficsim.core.geometry.OTSLine3D;
25 import org.opentrafficsim.core.geometry.OTSPoint3D;
26 import org.opentrafficsim.core.gtu.GTUException;
27 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
28 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
29 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.SpeedSegment;
30 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
31 import org.opentrafficsim.core.math.Solver;
32 import org.opentrafficsim.core.network.LateralDirectionality;
33 import org.opentrafficsim.core.network.NetworkException;
34 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
35 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
36 import org.opentrafficsim.road.network.lane.Lane;
37 import org.opentrafficsim.road.network.lane.LaneDirection;
38 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
39 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
40
41 import nl.tudelft.simulation.dsol.SimRuntimeException;
42 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
43
44 /**
45 * 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-2022 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 // if (gtu.getId().equals("1669") && gtu.getSimulator().getSimulatorTime().si >= 2508.9)
295 // {
296 // System.out.println("processing gtu " + gtu);
297 // try
298 // {
299 // for (Lane l : gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).keySet())
300 // {
301 // System.out.println("fractional position on lane " + l + ": "
302 // + gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).get(l));
303 // }
304 // System.out.println("reference position is " + gtu.getReferencePosition());
305 // System.out.println("operational plan path is " + gtu.getOperationalPlan().getPath());
306 // }
307 // catch (GTUException e)
308 // {
309 // e.printStackTrace();
310 // }
311 // }
312 OTSLine3D path = null;
313 try
314 {
315 DirectedLanePosition ref = gtu.getReferencePosition();
316 double f = ref.getLane().fraction(ref.getPosition());
317 if (ref.getGtuDirection().isPlus() && f < 1.0)
318 {
319 if (f >= 0.0)
320 {
321 path = ref.getLane().getCenterLine().extractFractional(f, 1.0);
322 }
323 else
324 {
325 path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0);
326 }
327 }
328 else if (ref.getGtuDirection().isMinus() && f > 0.0)
329 {
330 if (f <= 1.0)
331 {
332 path = ref.getLane().getCenterLine().extractFractional(0.0, f).reverse();
333 }
334 else
335 {
336 path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0).reverse();
337 }
338 }
339 // if (gtu.getId().equals("1669") && gtu.getSimulator().getSimulatorTime().si >= 2508.9)
340 // {
341 // System.out.println("First part of path is " + path);
342 // }
343 LaneDirection prevFrom = null;
344 LaneDirection from = ref.getLaneDirection();
345 int n = 1;
346 boolean alternativeTried = false;
347 while (path == null || path.getLength().si < distance.si + n * Lane.MARGIN.si)
348 {
349 n++;
350 prevFrom = from;
351 if (null == from)
352 {
353 CategoryLogger.always().warn("About to die: GTU {} has null from value", gtu.getId());
354 }
355 from = from.getNextLaneDirection(gtu);
356 if (from == null)
357 {
358 // check sink sensor
359 Length pos = prevFrom.getDirection().isPlus() ? prevFrom.getLength() : Length.ZERO;
360 for (SingleSensor sensor : prevFrom.getLane().getSensors(pos, pos, gtu.getGTUType(),
361 prevFrom.getDirection()))
362 {
363 // XXX for now, the same is not done for the DestinationSensor (e.g., decrease speed for parking)
364 if (sensor instanceof SinkSensor)
365 {
366 // just add some length so the GTU is happy to go to the sink
367 DirectedPoint end = path.getLocationExtendedSI(distance.si + n * Lane.MARGIN.si);
368 List<OTSPoint3D> points = new ArrayList<>(Arrays.asList(path.getPoints()));
369 points.add(new OTSPoint3D(end));
370 return new OTSLine3D(points);
371 }
372 }
373 // START CLEVER
374 /*-
375 if (!alternativeTried)
376 {
377 for (Lane l : gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).keySet())
378 {
379 if (ref.getLane().equals(l))
380 {
381 continue;
382 }
383 CategoryLogger.always().warn("GTU {} dead end on {}; but reference position is on {}; trying that",
384 gtu.getId(), ref, l);
385 // Figure out the driving direction and position on Lane l
386 // For now assume that lane l and ref are lanes on the same parent link. If not, chaos may occur
387 if (!l.getParentLink().equals(ref.getLane().getParentLink()))
388 {
389 CategoryLogger.always()
390 .error("Assumption that l and ref.getLane are on same Link does not hold");
391 }
392 from = new LaneDirection(l, ref.getGtuDirection());
393 if (ref.getGtuDirection().isPlus() && f < 1.0)
394 {
395 if (f >= 0.0)
396 {
397 path = l.getCenterLine().extractFractional(f, 1.0);
398 }
399 else
400 {
401 path = l.getCenterLine().extractFractional(0.0, 1.0);
402 }
403 }
404 else if (ref.getGtuDirection().isMinus() && f > 0.0)
405 {
406 if (f <= 1.0)
407 {
408 path = l.getCenterLine().extractFractional(0.0, f).reverse();
409 }
410 else
411 {
412 path = l.getCenterLine().extractFractional(0.0, 1.0).reverse();
413 }
414 }
415 alternativeTried = true;
416 }
417 if (null != from)
418 {
419 continue;
420 }
421 }
422 */
423 // END CLEVER
424 CategoryLogger.always().error("GTU {} has nowhere to go and no sink sensor either", gtu);
425 // gtu.getReferencePosition(); // CLEVER
426 gtu.destroy();
427 return path;
428 }
429 if (path == null)
430 {
431 path = from.getDirection().isPlus() ? from.getLane().getCenterLine()
432 : from.getLane().getCenterLine().reverse();
433 }
434 else
435 {
436 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, from.getDirection().isPlus()
437 ? from.getLane().getCenterLine() : from.getLane().getCenterLine().reverse());
438 }
439 }
440 }
441 catch (GTUException exception)
442 {
443 throw new RuntimeException("Error during creation of path.", exception);
444 }
445 return path;
446 }
447
448 /**
449 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
450 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
451 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
452 * before completing the path, a truncated path that ends where the GTU stops is used instead.
453 * @param gtu LaneBasedGTU; the GTU for debugging purposes
454 * @param laneChangeDirectionality LateralDirectionality; direction of lane change (on initiation only, after that not
455 * important)
456 * @param startPosition DirectedPoint; current position
457 * @param startTime Time; the current time or a time in the future when the plan should start
458 * @param startSpeed Speed; the speed at the start of the path
459 * @param acceleration Acceleration; the acceleration to use
460 * @param timeStep Duration; time step for the plan
461 * @param laneChange LaneChange; lane change status
462 * @return the operational plan to accomplish the given end speed
463 * @throws OperationalPlanException when the construction of the operational path fails
464 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
465 * first lane
466 */
467 @SuppressWarnings("checkstyle:parameternumber")
468 public static LaneBasedOperationalPlan buildAccelerationLaneChangePlan(final LaneBasedGTU gtu,
469 final LateralDirectionality laneChangeDirectionality, final DirectedPoint startPosition, final Time startTime,
470 final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final LaneChange laneChange)
471 throws OperationalPlanException, OTSGeometryException
472 {
473
474 // on first call during lane change, use laneChangeDirectionality as laneChange.getDirection() is NONE
475 // on successive calls, use laneChange.getDirection() as laneChangeDirectionality is NONE (i.e. no LC initiated)
476 LateralDirectionality direction = laneChange.isChangingLane() ? laneChange.getDirection() : laneChangeDirectionality;
477
478 Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
479 Length planDistance =
480 Length.instantiateSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
481 List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
482
483 try
484 {
485 // get position on from lane
486 Map<Lane, Length> positions = gtu.positions(gtu.getReference());
487 DirectedLanePosition ref = gtu.getReferencePosition();
488 Iterator<Lane> iterator = ref.getLane()
489 .accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection()).iterator();
490 Lane adjLane = iterator.hasNext() ? iterator.next() : null;
491 DirectedLanePosition from = null;
492 if (laneChange.getDirection() == null || (adjLane != null && positions.containsKey(adjLane)))
493 {
494 // reference lane is from lane, this is ok
495 from = ref;
496 }
497 else
498 {
499 // reference lane is to lane, this should be accounted for
500 for (Lane lane : positions.keySet())
501 {
502 if (lane.accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection())
503 .contains(ref.getLane()))
504 {
505 from = new DirectedLanePosition(lane, positions.get(lane), ref.getGtuDirection());
506 break;
507 }
508 }
509 }
510 Throw.when(from == null, RuntimeException.class, "From lane could not be determined during lane change.");
511
512 // get path and make plan
513 OTSLine3D path = laneChange.getPath(timeStep, gtu, from, startPosition, planDistance, direction);
514 LaneBasedOperationalPlan plan = new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, true);
515 return plan;
516 }
517 catch (GTUException exception)
518 {
519 throw new RuntimeException("Error during creation of lane change plan.", exception);
520 }
521 }
522
523 /**
524 * Returns the effective braking time, which stops if stand-still is reached.
525 * @param acceleration Acceleration; acceleration
526 * @param startSpeed Speed; start speed
527 * @param time Duration; intended time step
528 * @return Duration; effective braking time
529 */
530 public static Duration brakingTime(final Acceleration acceleration, final Speed startSpeed, final Duration time)
531 {
532 if (acceleration.ge0())
533 {
534 return time;
535 }
536 double t = startSpeed.si / -acceleration.si;
537 if (t >= time.si)
538 {
539 return time;
540 }
541 return Duration.instantiateSI(t);
542 }
543
544 /**
545 * Creates 1 or 2 segments in an operational plan. Two segments are returned of stand-still is reached within the time step.
546 * @param startSpeed Speed; start speed
547 * @param acceleration Acceleration; acceleration
548 * @param brakingTime Duration; braking time until stand-still
549 * @param timeStep Duration; time step
550 * @return 1 or 2 segments in an operational plan
551 */
552 private static List<Segment> createAccelerationSegments(final Speed startSpeed, final Acceleration acceleration,
553 final Duration brakingTime, final Duration timeStep)
554 {
555 List<Segment> segmentList = new ArrayList<>();
556 if (brakingTime.si < timeStep.si)
557 {
558 if (brakingTime.si > 0.0)
559 {
560 segmentList.add(new OperationalPlan.AccelerationSegment(brakingTime, acceleration));
561 }
562 segmentList.add(new OperationalPlan.SpeedSegment(timeStep.minus(brakingTime)));
563 }
564 else
565 {
566 segmentList.add(new OperationalPlan.AccelerationSegment(timeStep, acceleration));
567 }
568 return segmentList;
569 }
570
571 /**
572 * Build an operational plan based on a simple operational plan and status info.
573 * @param gtu LaneBasedGTU; gtu
574 * @param startTime Time; start time for plan
575 * @param simplePlan SimpleOperationalPlan; simple operational plan
576 * @param laneChange LaneChange; lane change status
577 * @return operational plan
578 * @throws ParameterException if parameter is not defined
579 * @throws GTUException gtu exception
580 * @throws NetworkException network exception
581 * @throws OperationalPlanException operational plan exeption
582 */
583 public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGTU gtu, final Time startTime,
584 final SimpleOperationalPlan simplePlan, final LaneChange laneChange)
585 throws ParameterException, GTUException, NetworkException, OperationalPlanException
586 {
587 Acceleration acc = gtu.getVehicleModel().boundAcceleration(simplePlan.getAcceleration(), gtu);
588
589 if (gtu.isInstantaneousLaneChange())
590 {
591 if (simplePlan.isLaneChange())
592 {
593 gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
594 }
595 try
596 {
597 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
598 simplePlan.getDuration(), false);
599 }
600 catch (OTSGeometryException exception)
601 {
602 throw new OperationalPlanException(exception);
603 }
604 }
605
606 // gradual lane change
607 try
608 {
609 if (!simplePlan.isLaneChange() && !laneChange.isChangingLane())
610 {
611 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
612 simplePlan.getDuration(), true);
613 }
614 if (gtu.getSpeed().si == 0.0 && acc.si <= 0.0)
615 {
616 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
617 simplePlan.getDuration(), false);
618 }
619 return LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(gtu, simplePlan.getLaneChangeDirection(),
620 gtu.getLocation(), startTime, gtu.getSpeed(), acc, simplePlan.getDuration(), laneChange);
621 }
622 catch (OTSGeometryException exception)
623 {
624 throw new OperationalPlanException(exception);
625 }
626 }
627
628 /**
629 * Schedules a lane change finalization after the given distance is covered. This distance is known as the plan is created,
630 * but at that point no time can be derived as the plan is required for that. Hence, this method can be scheduled at the
631 * same time (sequentially after creation of the plan) to then schedule the actual finalization by deriving time from
632 * distance with the plan.
633 * @param gtu LaneBasedGTU; gtu
634 * @param distance Length; distance
635 * @param laneChangeDirection LateralDirectionality; lane change direction
636 * @throws SimRuntimeException on bad time
637 */
638 public static void scheduleLaneChangeFinalization(final LaneBasedGTU gtu, final Length distance,
639 final LateralDirectionality laneChangeDirection) throws SimRuntimeException
640 {
641 Time time = gtu.getOperationalPlan().timeAtDistance(distance);
642 if (Double.isNaN(time.si))
643 {
644 // rounding...
645 time = gtu.getOperationalPlan().getEndTime();
646 }
647 SimEventInterface<Duration> event = gtu.getSimulator().scheduleEventAbsTime(time, (short) 6, gtu, gtu,
648 "finalizeLaneChange", new Object[] { laneChangeDirection });
649 gtu.setFinalizeLaneChangeEvent(event);
650 }
651
652 /**
653 * 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
654 * before completing the given path, a truncated path that ends where the GTU stops is used instead. There is no guarantee
655 * that the OperationalPlan will lead to a complete stop.
656 * @param gtu LaneBasedGTU; the GTU for debugging purposes
657 * @param distance Length; distance to drive for reaching the end speed
658 * @param startTime Time; the current time or a time in the future when the plan should start
659 * @param startSpeed Speed; the speed at the start of the path
660 * @param deceleration Acceleration; the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
661 * @return the operational plan to accomplish the given end speed
662 * @throws OperationalPlanException when construction of the operational path fails
663 * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
664 * first lane
665 */
666 public static LaneBasedOperationalPlan buildStopPlan(final LaneBasedGTU gtu, final Length distance, final Time startTime,
667 final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
668 {
669 return buildMaximumAccelerationPlan(gtu, distance, startTime, startSpeed, new Speed(0.0, SpeedUnit.SI),
670 new Acceleration(1.0, AccelerationUnit.SI), deceleration);
671 }
672
673 }