View Javadoc
1   package org.opentrafficsim.road.gtu.lane.tactical;
2   
3   import java.util.ArrayList;
4   import java.util.Collection;
5   import java.util.HashSet;
6   import java.util.List;
7   import java.util.Map;
8   import java.util.Set;
9   
10  import org.djunits.unit.AccelerationUnit;
11  import org.djunits.unit.DurationUnit;
12  import org.djunits.unit.LengthUnit;
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.base.parameters.ParameterTypeAcceleration;
21  import org.opentrafficsim.base.parameters.ParameterTypeDouble;
22  import org.opentrafficsim.base.parameters.ParameterTypeDuration;
23  import org.opentrafficsim.base.parameters.ParameterTypes;
24  import org.opentrafficsim.base.parameters.Parameters;
25  import org.opentrafficsim.core.gtu.GTUDirectionality;
26  import org.opentrafficsim.core.gtu.GTUException;
27  import org.opentrafficsim.core.gtu.TurnIndicatorStatus;
28  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
29  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
30  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
31  import org.opentrafficsim.core.network.LateralDirectionality;
32  import org.opentrafficsim.core.network.NetworkException;
33  import org.opentrafficsim.road.gtu.lane.AbstractLaneBasedGTU;
34  import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
35  import org.opentrafficsim.road.gtu.lane.perception.CategoricalLanePerception;
36  import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
37  import org.opentrafficsim.road.gtu.lane.perception.categories.DefaultSimplePerception;
38  import org.opentrafficsim.road.gtu.lane.perception.categories.DirectDefaultSimplePerception;
39  import org.opentrafficsim.road.gtu.lane.perception.headway.AbstractHeadwayGTU;
40  import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
41  import org.opentrafficsim.road.gtu.lane.tactical.directedlanechange.DirectedAltruistic;
42  import org.opentrafficsim.road.gtu.lane.tactical.directedlanechange.DirectedEgoistic;
43  import org.opentrafficsim.road.gtu.lane.tactical.directedlanechange.DirectedLaneChangeModel;
44  import org.opentrafficsim.road.gtu.lane.tactical.directedlanechange.DirectedLaneMovementStep;
45  import org.opentrafficsim.road.gtu.lane.tactical.following.AccelerationStep;
46  import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
47  import org.opentrafficsim.road.network.lane.Lane;
48  import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
49  import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
50  
51  import nl.tudelft.simulation.dsol.SimRuntimeException;
52  import nl.tudelft.simulation.language.d3.DirectedPoint;
53  
54  /**
55   * Lane-based tactical planner that implements car following behavior and rule-based lane change. This tactical planner
56   * retrieves the car following model from the strategical planner and will generate an operational plan for the GTU.
57   * <p>
58   * A lane change occurs when:
59   * <ol>
60   * <li>The route indicates that the current lane does not lead to the destination; main choices are the time when the GTU
61   * switches to the "right" lane, and what should happen when the split gets closer and the lane change has failed. Observations
62   * indicate that vehicles if necessary stop in their current lane until they can go to the desired lane. A lane drop is
63   * automatically part of this implementation, because the lane with a lane drop will not lead to the GTU's destination.</li>
64   * <li>The desired speed of the vehicle is a particular delta-speed higher than its predecessor, the headway to the predecessor
65   * in the current lane has exceeded a certain value, it is allowed to change to the target lane, the target lane lies on the
66   * GTU's route, and the gap in the target lane is acceptable (including the evaluation of the perceived speed of a following GTU
67   * in the target lane).</li>
68   * <li>The current lane is not the optimum lane given the traffic rules (for example, to keep right), the headway to the
69   * predecessor on the target lane is greater than a certain value, the speed of the predecessor on the target lane is greater
70   * than or equal to our speed, the target lane is on the route, it is allowed to switch to the target lane, and the gap at the
71   * target lane is acceptable (including the perceived speed of any vehicle in front or behind on the target lane).</li>
72   * </ol>
73   * <p>
74   * This lane-based tactical planner makes decisions based on headway (GTU following model). It can ask the strategic planner for
75   * assistance on the route to take when the network splits.
76   * <p>
77   * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
78   * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
79   * </p>
80   * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
81   * initial version Nov 25, 2015 <br>
82   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
83   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
84   */
85  public class LaneBasedGTUFollowingDirectedChangeTacticalPlanner extends AbstractLaneBasedTacticalPlanner
86  {
87      /** */
88      private static final long serialVersionUID = 20160129L;
89  
90      /** Acceleration parameter type. */
91      protected static final ParameterTypeAcceleration A = ParameterTypes.A;
92  
93      /** Desired headway parameter type. */
94      protected static final ParameterTypeDuration T = ParameterTypes.T;
95  
96      /** Speed limit adherance factor parameter type. */
97      protected static final ParameterTypeDouble FSPEED = ParameterTypes.FSPEED;
98  
99      /** Comfortable deceleration parameter type. */
100     protected static final ParameterTypeAcceleration B = ParameterTypes.B;
101 
102     /** Earliest next lane change time (unless we HAVE to change lanes). */
103     private Time earliestNextLaneChangeTime = Time.ZERO;
104 
105     /** Time a GTU should stay in its current lane after a lane change. */
106     private Duration durationInLaneAfterLaneChange = new Duration(15.0, DurationUnit.SECOND);
107 
108     /** Lane we changed to at instantaneous lane change. */
109     private Lane laneAfterLaneChange = null;
110 
111     /** Position on the reference lane. */
112     private Length posAfterLaneChange = null;
113 
114     /** When a failure in planning occurs, should we destroy the GTU to avoid halting of the model? */
115     private boolean destroyGtuOnFailure = false;
116 
117     /**
118      * Instantiated a tactical planner with just GTU following behavior and no lane changes.
119      * @param carFollowingModel GTUFollowingModelOld; Car-following model.
120      * @param gtu LaneBasedGTU; GTU
121      */
122     public LaneBasedGTUFollowingDirectedChangeTacticalPlanner(final GTUFollowingModelOld carFollowingModel,
123             final LaneBasedGTU gtu)
124     {
125         super(carFollowingModel, gtu, new CategoricalLanePerception(gtu));
126         getPerception().addPerceptionCategory(new DirectDefaultSimplePerception(getPerception()));
127         setNoLaneChange(new Duration(0.25, DurationUnit.SECOND));
128     }
129 
130     /**
131      * Returns the car-following model.
132      * @return The car-following model.
133      */
134     public final GTUFollowingModelOld getCarFollowingModelOld()
135     {
136         return (GTUFollowingModelOld) super.getCarFollowingModel();
137     }
138 
139     /**
140      * Indicate that no lane change should happen for the indicated duration.
141      * @param noLaneChangeDuration Duration; the duration for which no lane change should happen.
142      */
143     public final void setNoLaneChange(final Duration noLaneChangeDuration)
144     {
145         Throw.when(noLaneChangeDuration.lt0(), RuntimeException.class, "noLaneChangeDuration should be >= 0");
146         this.earliestNextLaneChangeTime = getGtu().getSimulator().getSimulatorTime().plus(noLaneChangeDuration);
147     }
148 
149     /**
150      * Headway for synchronization.
151      */
152     private Headway syncHeadway;
153 
154     /**
155      * Headway for cooperation.
156      */
157     private Headway coopHeadway;
158 
159     /**
160      * Time when (potential) dead-lock was first recognized.
161      */
162     private Time deadLock = null;
163 
164     /**
165      * Time after which situation is labeled a dead-lock.
166      */
167     private final Duration deadLockThreshold = new Duration(5.0, DurationUnit.SI);
168 
169     /**
170      * Headways that are causing the dead-lock.
171      */
172     private Collection<Headway> blockingHeadways = new HashSet<>();
173 
174     /** {@inheritDoc} */
175     @Override
176     @SuppressWarnings("checkstyle:methodlength")
177     public final OperationalPlan generateOperationalPlan(final Time startTime, final DirectedPoint locationAtStartTime)
178             throws OperationalPlanException, NetworkException, GTUException, ParameterException
179     {
180         try
181         {
182 
183             // ask Perception for the local situation
184             LaneBasedGTU laneBasedGTU = getGtu();
185             DefaultSimplePerception simplePerception = getPerception().getPerceptionCategory(DefaultSimplePerception.class);
186             Parameters parameters = laneBasedGTU.getParameters();
187             // This is the only interaction between the car-following model and the parameters
188             getCarFollowingModelOld().setA(parameters.getParameter(A));
189             getCarFollowingModelOld().setT(parameters.getParameter(T));
190             getCarFollowingModelOld().setFspeed(parameters.getParameter(FSPEED));
191 
192             // start with the turn indicator off -- this can change during the method
193             laneBasedGTU.setTurnIndicatorStatus(TurnIndicatorStatus.NONE);
194 
195             // if the GTU's maximum speed is zero (block), generate a stand still plan for one second
196             if (laneBasedGTU.getMaximumSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
197             {
198                 return new OperationalPlan(getGtu(), locationAtStartTime, startTime, new Duration(1.0, DurationUnit.SECOND));
199             }
200 
201             // perceive the forward headway, accessible lanes and speed limit.
202             simplePerception.updateForwardHeadwayGTU();
203             simplePerception.updateForwardHeadwayObject();
204             simplePerception.updateAccessibleAdjacentLanesLeft();
205             simplePerception.updateAccessibleAdjacentLanesRight();
206             simplePerception.updateSpeedLimit();
207 
208             // find out where we are going
209             Length forwardHeadway = parameters.getParameter(LOOKAHEAD);
210             LanePathInfo lanePathInfo = buildLanePathInfo(laneBasedGTU, forwardHeadway);
211             NextSplitInfo nextSplitInfo = determineNextSplit(laneBasedGTU, forwardHeadway);
212             Set<Lane> correctLanes = laneBasedGTU.positions(laneBasedGTU.getReference()).keySet();
213             correctLanes.retainAll(nextSplitInfo.getCorrectCurrentLanes());
214 
215             // Step 1: Do we want to change lanes because of the current lane not leading to our destination?
216             this.syncHeadway = null;
217             if (lanePathInfo.getPath().getLength().lt(forwardHeadway) && correctLanes.isEmpty())
218             {
219                 LateralDirectionality direction = determineLeftRight(laneBasedGTU, nextSplitInfo);
220                 if (direction != null)
221                 {
222                     getGtu().setTurnIndicatorStatus(direction.isLeft() ? TurnIndicatorStatus.LEFT : TurnIndicatorStatus.RIGHT);
223                     if (canChange(laneBasedGTU, getPerception(), lanePathInfo, direction))
224                     {
225                         DirectedPoint newLocation = changeLane(laneBasedGTU, direction);
226                         lanePathInfo = buildLanePathInfo(laneBasedGTU, forwardHeadway, this.laneAfterLaneChange,
227                                 this.posAfterLaneChange, laneBasedGTU.getDirection(this.laneAfterLaneChange));
228                         return currentLanePlan(laneBasedGTU, startTime, newLocation, lanePathInfo);
229                     }
230                     else
231                     {
232                         simplePerception.updateNeighboringHeadways(direction);
233                         Length minDistance = new Length(Double.MAX_VALUE, LengthUnit.SI);
234                         for (Headway headway : simplePerception.getNeighboringHeadways(direction))
235                         {
236                             if ((headway.isAhead() || headway.isParallel()) && (headway instanceof AbstractHeadwayGTU))
237                             {
238                                 if (headway.isParallel() || headway.getDistance().lt(minDistance))
239                                 {
240                                     this.syncHeadway = headway;
241                                     if (!headway.isParallel())
242                                     {
243                                         minDistance = headway.getDistance();
244                                     }
245                                 }
246                             }
247                         }
248                     }
249                 }
250             }
251             if (this.syncHeadway != null && this.syncHeadway.isParallel() && getGtu().getSpeed().si < 10)
252             {
253                 // do not sync at low speeds when being parallel
254                 this.syncHeadway = null;
255             }
256 
257             // Cooperation
258             this.coopHeadway = null;
259             for (LateralDirectionality direction : new LateralDirectionality[] { LateralDirectionality.LEFT,
260                     LateralDirectionality.RIGHT })
261             {
262                 simplePerception.updateNeighboringHeadways(direction);
263                 for (Headway headway : simplePerception.getNeighboringHeadways(direction))
264                 {
265                     // other vehicle ahead, its a vehicle, its the nearest, and its indicator is on
266                     if (headway.isAhead() && (headway instanceof AbstractHeadwayGTU)
267                             && (this.coopHeadway == null || headway.getDistance().lt(this.coopHeadway.getDistance()))
268                             && (direction.isLeft() ? ((AbstractHeadwayGTU) headway).isRightTurnIndicatorOn()
269                                     : ((AbstractHeadwayGTU) headway).isLeftTurnIndicatorOn()))
270                     {
271                         this.coopHeadway = headway;
272                     }
273                 }
274             }
275 
276             // Condition, if we have just changed lane, let's not change immediately again.
277             if (getGtu().getSimulator().getSimulatorTime().lt(this.earliestNextLaneChangeTime))
278             {
279                 return currentLanePlan(laneBasedGTU, startTime, locationAtStartTime, lanePathInfo);
280             }
281 
282             // Step 2. Do we want to change lanes to the left because of predecessor speed on the current lane?
283             // And does the lane left of us bring us to our destination as well?
284             Set<Lane> leftLanes = simplePerception.getAccessibleAdjacentLanesLeft().get(lanePathInfo.getReferenceLane());
285             if (nextSplitInfo.isSplit())
286             {
287                 leftLanes.retainAll(nextSplitInfo.getCorrectCurrentLanes());
288             }
289             if (!leftLanes.isEmpty()) // && laneBasedGTU.getSpeed().si > 4.0) // only if we are driving...
290             {
291                 simplePerception.updateBackwardHeadway();
292                 simplePerception.updateParallelHeadwaysLeft();
293                 simplePerception.updateNeighboringHeadwaysLeft();
294                 if (simplePerception.getParallelHeadwaysLeft().isEmpty())
295                 {
296                     Collection<Headway> sameLaneTraffic = new HashSet<>();
297                     // TODO should it be getObjectType().isGtu() or !getObjectType().isDistanceOnly() ?
298                     // XXX Object & GTU
299                     if (simplePerception.getForwardHeadwayGTU() != null
300                             && simplePerception.getForwardHeadwayGTU().getObjectType().isGtu())
301                     {
302                         sameLaneTraffic.add(simplePerception.getForwardHeadwayGTU());
303                     }
304                     if (simplePerception.getBackwardHeadway() != null
305                             && simplePerception.getBackwardHeadway().getObjectType().isGtu())
306                     {
307                         sameLaneTraffic.add(simplePerception.getBackwardHeadway());
308                     }
309                     DirectedLaneChangeModel dlcm = new DirectedAltruistic(getPerception());
310                     DirectedLaneMovementStep dlms = dlcm.computeLaneChangeAndAcceleration(laneBasedGTU,
311                             LateralDirectionality.LEFT, sameLaneTraffic, simplePerception.getNeighboringHeadwaysLeft(),
312                             parameters.getParameter(LOOKAHEAD), simplePerception.getSpeedLimit(),
313                             // changes 1.0 to 0.0, no bias to the left: changed 0.5 to 0.1 (threshold from MOBIL model)
314                             Acceleration.ZERO, new Acceleration(0.5, AccelerationUnit.SI),
315                             new Duration(0.5, DurationUnit.SECOND));
316                     if (dlms.getLaneChange() != null)
317                     {
318                         getGtu().setTurnIndicatorStatus(TurnIndicatorStatus.LEFT);
319                         if (canChange(laneBasedGTU, getPerception(), lanePathInfo, LateralDirectionality.LEFT))
320                         {
321                             DirectedPoint newLocation = changeLane(laneBasedGTU, LateralDirectionality.LEFT);
322                             lanePathInfo = buildLanePathInfo(laneBasedGTU, forwardHeadway, this.laneAfterLaneChange,
323                                     this.posAfterLaneChange, laneBasedGTU.getDirection(this.laneAfterLaneChange));
324                             return currentLanePlan(laneBasedGTU, startTime, newLocation, lanePathInfo);
325                         }
326                     }
327                 }
328             }
329 
330             // Step 3. Do we want to change lanes to the right because of TODO traffic rules?
331             Set<Lane> rightLanes = simplePerception.getAccessibleAdjacentLanesRight().get(lanePathInfo.getReferenceLane());
332             if (nextSplitInfo.isSplit())
333             {
334                 rightLanes.retainAll(nextSplitInfo.getCorrectCurrentLanes());
335             }
336             if (!rightLanes.isEmpty()) // && laneBasedGTU.getSpeed().si > 4.0) // only if we are driving...
337             {
338                 simplePerception.updateBackwardHeadway();
339                 simplePerception.updateParallelHeadwaysRight();
340                 simplePerception.updateNeighboringHeadwaysRight();
341                 if (simplePerception.getParallelHeadwaysRight().isEmpty())
342                 {
343                     Collection<Headway> sameLaneTraffic = new HashSet<>();
344                     // TODO should it be getObjectType().isGtu() or !getObjectType().isDistanceOnly() ?
345                     // XXX GTU & Object
346                     if (simplePerception.getForwardHeadwayGTU() != null
347                             && simplePerception.getForwardHeadwayGTU().getObjectType().isGtu())
348                     {
349                         sameLaneTraffic.add(simplePerception.getForwardHeadwayGTU());
350                     }
351                     if (simplePerception.getBackwardHeadway() != null
352                             && simplePerception.getBackwardHeadway().getObjectType().isGtu())
353                     {
354                         sameLaneTraffic.add(simplePerception.getBackwardHeadway());
355                     }
356                     DirectedLaneChangeModel dlcm = new DirectedAltruistic(getPerception());
357                     DirectedLaneMovementStep dlms = dlcm.computeLaneChangeAndAcceleration(laneBasedGTU,
358                             LateralDirectionality.RIGHT, sameLaneTraffic, simplePerception.getNeighboringHeadwaysRight(),
359                             parameters.getParameter(LOOKAHEAD), simplePerception.getSpeedLimit(),
360                             // 1.0 = bias?
361                             Acceleration.ZERO, new Acceleration(0.1, AccelerationUnit.SI),
362                             new Duration(0.5, DurationUnit.SECOND));
363                     if (dlms.getLaneChange() != null)
364                     {
365                         getGtu().setTurnIndicatorStatus(TurnIndicatorStatus.RIGHT);
366                         if (canChange(laneBasedGTU, getPerception(), lanePathInfo, LateralDirectionality.RIGHT))
367                         {
368                             DirectedPoint newLocation = changeLane(laneBasedGTU, LateralDirectionality.RIGHT);
369                             lanePathInfo = buildLanePathInfo(laneBasedGTU, forwardHeadway, this.laneAfterLaneChange,
370                                     this.posAfterLaneChange, laneBasedGTU.getDirection(this.laneAfterLaneChange));
371                             return currentLanePlan(laneBasedGTU, startTime, newLocation, lanePathInfo);
372                         }
373                     }
374                 }
375             }
376 
377             if (this.deadLock != null
378                     && getGtu().getSimulator().getSimulatorTime().minus(this.deadLock).ge(this.deadLockThreshold)
379                     && isDestroyGtuOnFailure())
380             {
381                 System.err.println("Deleting gtu " + getGtu().getId() + " to prevent dead-lock.");
382                 try
383                 {
384                     getGtu().getSimulator().scheduleEventRel(new Duration(0.001, DurationUnit.SI), this, getGtu(), "destroy",
385                             new Object[0]);
386                 }
387                 catch (SimRuntimeException exception)
388                 {
389                     throw new RuntimeException(exception);
390                 }
391             }
392 
393             return currentLanePlan(laneBasedGTU, startTime, locationAtStartTime, lanePathInfo);
394         }
395         catch (GTUException | NetworkException | OperationalPlanException exception)
396         {
397             if (isDestroyGtuOnFailure())
398             {
399                 System.err.println("LaneBasedGTUFollowingChange0TacticalPlanner.generateOperationalPlan() failed for "
400                         + getGtu() + " because of " + exception.getMessage() + " -- GTU destroyed");
401                 getGtu().destroy();
402                 return new OperationalPlan(getGtu(), locationAtStartTime, startTime, new Duration(1.0, DurationUnit.SECOND));
403             }
404             throw exception;
405         }
406     }
407 
408     /**
409      * Make a plan for the current lane.
410      * @param laneBasedGTU LaneBasedGTU; the gtu to generate the plan for
411      * @param startTime Time; the time from which the new operational plan has to be operational
412      * @param locationAtStartTime DirectedPoint; the location of the GTU at the start time of the new plan
413      * @param lanePathInfo LanePathInfo; the lane path for the current lane.
414      * @return An operation plan for staying in the current lane.
415      * @throws OperationalPlanException when there is a problem planning a path in the network
416      * @throws GTUException when there is a problem with the state of the GTU when planning a path
417      * @throws ParameterException in case LOOKAHEAD parameter cannot be found
418      * @throws NetworkException in case the headways to GTUs or objects cannot be calculated
419      */
420     private OperationalPlan currentLanePlan(final LaneBasedGTU laneBasedGTU, final Time startTime,
421             final DirectedPoint locationAtStartTime, final LanePathInfo lanePathInfo)
422             throws OperationalPlanException, GTUException, ParameterException, NetworkException
423     {
424         DefaultSimplePerception simplePerception = getPerception().getPerceptionCategory(DefaultSimplePerception.class);
425 
426         // No lane change. Continue on current lane.
427         AccelerationStep accelerationStep = mostLimitingAccelerationStep(lanePathInfo, simplePerception.getForwardHeadwayGTU(),
428                 simplePerception.getForwardHeadwayObject());
429 
430         // see if we have to continue standing still. In that case, generate a stand still plan
431         if (accelerationStep.getAcceleration().si < 1E-6 && laneBasedGTU.getSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
432         {
433             return new OperationalPlan(laneBasedGTU, locationAtStartTime, startTime, accelerationStep.getDuration());
434         }
435 
436         // build a list of lanes forward, with a maximum headway.
437         List<Segment> operationalPlanSegmentList = new ArrayList<>();
438         if (accelerationStep.getAcceleration().si == 0.0)
439         {
440             Segment segment = new OperationalPlan.SpeedSegment(accelerationStep.getDuration());
441             operationalPlanSegmentList.add(segment);
442         }
443         else
444         {
445             Segment segment =
446                     new OperationalPlan.AccelerationSegment(accelerationStep.getDuration(), accelerationStep.getAcceleration());
447             operationalPlanSegmentList.add(segment);
448         }
449         OperationalPlan op = new OperationalPlan(laneBasedGTU, lanePathInfo.getPath(), startTime, laneBasedGTU.getSpeed(),
450                 operationalPlanSegmentList);
451         return op;
452     }
453 
454     /**
455      * We are not on a lane that leads to our destination. Determine whether the lateral direction to go is left or right.
456      * @param laneBasedGTU LaneBasedGTU; the gtu
457      * @param nextSplitInfo NextSplitInfo; the information about the next split
458      * @return the lateral direction to go, or null if this cannot be determined
459      */
460     private LateralDirectionality determineLeftRight(final LaneBasedGTU laneBasedGTU, final NextSplitInfo nextSplitInfo)
461     {
462         // are the lanes in nextSplitInfo.getCorrectCurrentLanes() left or right of the current lane(s) of the GTU?
463         try
464         {
465             Set<Lane> lanes = laneBasedGTU.positions(laneBasedGTU.getReference()).keySet();
466             for (Lane correctLane : nextSplitInfo.getCorrectCurrentLanes())
467             {
468                 for (Lane currentLane : lanes)
469                 {
470                     if (correctLane.getParentLink().equals(currentLane.getParentLink()))
471                     {
472                         double deltaOffset =
473                                 correctLane.getDesignLineOffsetAtBegin().si - currentLane.getDesignLineOffsetAtBegin().si;
474                         if (laneBasedGTU.getDirection(currentLane).equals(GTUDirectionality.DIR_PLUS))
475                         {
476                             return deltaOffset > 0 ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT;
477                         }
478                         else
479                         {
480                             return deltaOffset < 0 ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT;
481                         }
482                     }
483                 }
484             }
485         }
486         catch (GTUException exception)
487         {
488             System.err.println(
489                     "Exception in LaneBasedGTUFollowingChange0TacticalPlanner.determineLeftRight: " + exception.getMessage());
490         }
491         // perhaps known from split info (if need to change away from all lanes on current link)
492         return nextSplitInfo.getRequiredDirection();
493     }
494 
495     /**
496      * See if a lane change in the given direction if possible.
497      * @param gtu LaneBasedGTU; the GTU that has to make the lane change
498      * @param perception LanePerception; the perception, where forward headway, accessible lanes and speed limit have been
499      *            assessed
500      * @param lanePathInfo LanePathInfo; the information for the path on the current lane
501      * @param direction LateralDirectionality; the lateral direction, either LEFT or RIGHT
502      * @return whether a lane change is possible.
503      * @throws NetworkException when there is a network inconsistency in updating the perception
504      * @throws GTUException when there is an issue retrieving GTU information for the perception update
505      * @throws ParameterException when there is a parameter problem.
506      * @throws OperationalPlanException in case a perception category is not present
507      */
508     private boolean canChange(final LaneBasedGTU gtu, final LanePerception perception, final LanePathInfo lanePathInfo,
509             final LateralDirectionality direction)
510             throws GTUException, NetworkException, ParameterException, OperationalPlanException
511     {
512 
513         // TODO remove this hack
514         if (!((AbstractLaneBasedGTU) gtu).isSafeToChange())
515         {
516             return false;
517         }
518 
519         // rear should be able to change
520         Map<Lane, Length> positions = getGtu().positions(getGtu().getRear());
521         for (Lane lane : positions.keySet())
522         {
523             Length pos = positions.get(lane);
524             if (pos.si > 0.0 && pos.si < lane.getLength().si && lane
525                     .accessibleAdjacentLanesLegal(direction, getGtu().getGTUType(), getGtu().getDirection(lane)).isEmpty())
526             {
527                 return false;
528             }
529         }
530 
531         Collection<Headway> otherLaneTraffic;
532         DefaultSimplePerception simplePerception = getPerception().getPerceptionCategory(DefaultSimplePerception.class);
533         simplePerception.updateForwardHeadwayGTU();
534         simplePerception.updateForwardHeadwayObject();
535         simplePerception.updateBackwardHeadway();
536         if (direction.isLeft())
537         {
538             simplePerception.updateParallelHeadwaysLeft();
539             this.blockingHeadways = simplePerception.getParallelHeadwaysLeft();
540             simplePerception.updateNeighboringHeadwaysLeft();
541             otherLaneTraffic = simplePerception.getNeighboringHeadwaysLeft();
542         }
543         else if (direction.isRight())
544         {
545             simplePerception.updateParallelHeadwaysRight();
546             this.blockingHeadways = simplePerception.getParallelHeadwaysRight();
547             simplePerception.updateNeighboringHeadwaysRight();
548             otherLaneTraffic = simplePerception.getNeighboringHeadwaysRight();
549         }
550         else
551         {
552             throw new GTUException("Lateral direction is neither LEFT nor RIGHT during a lane change");
553         }
554         if (!simplePerception.getParallelHeadways(direction).isEmpty())
555         {
556             return false;
557         }
558 
559         Collection<Headway> sameLaneTraffic = new HashSet<>();
560         // TODO should it be getObjectType().isGtu() or !getObjectType().isDistanceOnly() ?
561         // XXX Object & GTU
562         if (simplePerception.getForwardHeadwayGTU() != null && simplePerception.getForwardHeadwayGTU().getObjectType().isGtu())
563         {
564             sameLaneTraffic.add(simplePerception.getForwardHeadwayGTU());
565         }
566         if (simplePerception.getBackwardHeadway() != null && simplePerception.getBackwardHeadway().getObjectType().isGtu())
567         {
568             sameLaneTraffic.add(simplePerception.getBackwardHeadway());
569         }
570 
571         // TODO make type of plan (Egoistic, Altruistic) parameter of the class
572         DirectedLaneChangeModel dlcm = new DirectedEgoistic(getPerception());
573         // TODO make the elasticities 2.0 and 0.1 parameters of the class
574         DirectedLaneMovementStep dlms = dlcm.computeLaneChangeAndAcceleration(gtu, direction, sameLaneTraffic, otherLaneTraffic,
575                 gtu.getParameters().getParameter(LOOKAHEAD), simplePerception.getSpeedLimit(),
576                 new Acceleration(2.0, AccelerationUnit.SI), new Acceleration(0.1, AccelerationUnit.SI),
577                 new Duration(0.5, DurationUnit.SECOND));
578         if (dlms.getLaneChange() == null)
579         {
580             return false;
581         }
582 
583         return true;
584     }
585 
586     /**
587      * Change lanes instantaneously.
588      * @param gtu LaneBasedGTU; the gtu
589      * @param direction LateralDirectionality; the direction
590      * @return the new location of the GTU after the lane change
591      * @throws GTUException in case the enter lane fails
592      */
593     private DirectedPoint changeLane(final LaneBasedGTU gtu, final LateralDirectionality direction) throws GTUException
594     {
595         gtu.changeLaneInstantaneously(direction);
596 
597         // stay at a certain number of seconds in the current lane (unless we HAVE to change lanes)
598         this.earliestNextLaneChangeTime = gtu.getSimulator().getSimulatorTime().plus(this.durationInLaneAfterLaneChange);
599 
600         // make sure out turn indicator is on!
601         gtu.setTurnIndicatorStatus(direction.isLeft() ? TurnIndicatorStatus.LEFT : TurnIndicatorStatus.RIGHT);
602 
603         this.laneAfterLaneChange = gtu.getReferencePosition().getLane();
604         this.posAfterLaneChange = gtu.getReferencePosition().getPosition();
605         return gtu.getLocation();
606     }
607 
608     /**
609      * Calculate which Headway in front of us is leading to the most limiting acceleration step (i.e. to the lowest or most
610      * negative acceleration). There could, e.g. be a GTU in front of us, a speed sign in front of us, and a traffic light in
611      * front of the GTU and speed sign. This method will return the acceleration based on the headway that limits us most.<br>
612      * The method can e.g., be called with:
613      * <code>mostLimitingHeadway(simplePerception.getForwardHeadwayGTU(), simplePerception.getForwardHeadwayObject());</code>
614      * @param lanePathInfo LanePathInfo; the lane path info that was calculated for this GTU.
615      * @param headways Headway...; zero or more headways specifying possible limitations on our acceleration.
616      * @return the acceleration based on the most limiting headway.
617      * @throws OperationalPlanException in case the PerceptionCategory cannot be found
618      * @throws ParameterException in case LOOKAHEAD parameter cannot be found
619      * @throws GTUException in case the AccelerationStep cannot be calculated
620      * @throws NetworkException in case the headways to GTUs or objects cannot be calculated
621      */
622     private AccelerationStep mostLimitingAccelerationStep(final LanePathInfo lanePathInfo, final Headway... headways)
623             throws OperationalPlanException, ParameterException, GTUException, NetworkException
624     {
625         DefaultSimplePerception simplePerception = getPerception().getPerceptionCategory(DefaultSimplePerception.class);
626         simplePerception.updateForwardHeadwayGTU();
627         simplePerception.updateForwardHeadwayObject();
628         boolean sinkAtEnd = false;
629         for (SingleSensor sensor : (lanePathInfo.getLanes().get(lanePathInfo.getLanes().size() - 1).getSensors()))
630         {
631             if (sensor instanceof SinkSensor)
632             {
633                 sinkAtEnd = true;
634             }
635         }
636         boolean stopForEndOrSplit = !sinkAtEnd;
637         Parameters params = getGtu().getParameters();
638         Length maxDistance = sinkAtEnd ? new Length(Double.MAX_VALUE, LengthUnit.SI)
639                 : Length.min(getGtu().getParameters().getParameter(LOOKAHEAD),
640                         lanePathInfo.getPath().getLength().minus(getGtu().getLength().multiplyBy(2.0)));
641         // params.setParameter(B, params.getParameter(B0));
642         AccelerationStep mostLimitingAccelerationStep = getCarFollowingModelOld().computeAccelerationStepWithNoLeader(getGtu(),
643                 maxDistance, simplePerception.getSpeedLimit());
644         // bc.resetParameter(B);
645         Acceleration minB = params.getParameter(B).neg();
646         Acceleration numericallySafeB =
647                 Acceleration.max(minB, getGtu().getSpeed().divideBy(mostLimitingAccelerationStep.getDuration()).neg());
648         if ((this.syncHeadway != null || this.coopHeadway != null) && mostLimitingAccelerationStep.getAcceleration().gt(minB))
649         {
650             AccelerationStep sync;
651             if (this.syncHeadway == null)
652             {
653                 sync = null;
654             }
655             else if (this.syncHeadway.isParallel())
656             {
657                 sync = new AccelerationStep(numericallySafeB, mostLimitingAccelerationStep.getValidUntil(),
658                         mostLimitingAccelerationStep.getDuration());
659             }
660             else
661             {
662                 sync = getCarFollowingModelOld().computeAccelerationStep(getGtu(), this.syncHeadway.getSpeed(),
663                         this.syncHeadway.getDistance(), maxDistance, simplePerception.getSpeedLimit());
664             }
665             AccelerationStep coop;
666             if (this.coopHeadway == null)
667             {
668                 coop = null;
669             }
670             else
671             {
672                 coop = getCarFollowingModelOld().computeAccelerationStep(getGtu(), this.coopHeadway.getSpeed(),
673                         this.coopHeadway.getDistance(), maxDistance, simplePerception.getSpeedLimit());
674             }
675             AccelerationStep adjust;
676             if (sync == null)
677             {
678                 adjust = coop;
679             }
680             else if (coop == null)
681             {
682                 adjust = sync;
683             }
684             else
685             {
686                 adjust = sync.getAcceleration().lt(coop.getAcceleration()) ? sync : coop;
687             }
688             if (adjust.getAcceleration().lt(minB))
689             {
690                 mostLimitingAccelerationStep = new AccelerationStep(numericallySafeB,
691                         mostLimitingAccelerationStep.getValidUntil(), mostLimitingAccelerationStep.getDuration());
692             }
693             else
694             {
695                 mostLimitingAccelerationStep = adjust;
696             }
697         }
698 
699         for (Headway headway : headways)
700         {
701             if (headway != null && headway.getDistance().lt(maxDistance))
702             {
703                 AccelerationStep accelerationStep = getCarFollowingModelOld().computeAccelerationStep(getGtu(),
704                         headway.getSpeed(), headway.getDistance(), maxDistance, simplePerception.getSpeedLimit());
705                 if (accelerationStep.getAcceleration().lt(mostLimitingAccelerationStep.getAcceleration()))
706                 {
707                     stopForEndOrSplit = false;
708                     mostLimitingAccelerationStep = accelerationStep;
709                 }
710             }
711         }
712 
713         // recognize dead-lock
714         if (!this.blockingHeadways.isEmpty() && stopForEndOrSplit)
715         {
716             Speed maxSpeed = getGtu().getSpeed();
717             for (Headway headway : this.blockingHeadways)
718             {
719                 maxSpeed = Speed.max(maxSpeed, headway.getSpeed());
720             }
721             if (maxSpeed.si < OperationalPlan.DRIFTING_SPEED_SI)
722             {
723                 if (this.deadLock == null)
724                 {
725                     this.deadLock = getGtu().getSimulator().getSimulatorTime();
726                 }
727             }
728             else
729             {
730                 this.deadLock = null;
731             }
732         }
733         else
734         {
735             this.deadLock = null;
736         }
737 
738         return mostLimitingAccelerationStep;
739 
740     }
741 
742     /**
743      * @return destroyGtuOnFailure, indicating when a failure in planning occurs, whether we should destroy the GTU to avoid
744      *         halting of the model
745      */
746     public final boolean isDestroyGtuOnFailure()
747     {
748         return this.destroyGtuOnFailure;
749     }
750 
751     /**
752      * When a failure in planning occurs, should we destroy the GTU to avoid halting of the model?
753      * @param destroyGtuOnFailure boolean; set destroyGtuOnFailure to true or false
754      */
755     public final void setDestroyGtuOnFailure(final boolean destroyGtuOnFailure)
756     {
757         this.destroyGtuOnFailure = destroyGtuOnFailure;
758     }
759 
760     /**
761      * Get the duration to stay in a Lane after a lane change.
762      * @return Duration; durationInLaneAfterLaneChange
763      */
764     protected final Duration getDurationInLaneAfterLaneChange()
765     {
766         return this.durationInLaneAfterLaneChange;
767     }
768 
769     /**
770      * Set the duration to stay in a Lane after a lane change.
771      * @param durationInLaneAfterLaneChange Duration; set duration to stay in a Lane after a lane change
772      * @throws GTUException when durationInLaneAfterLaneChange less than zero
773      */
774     protected final void setDurationInLaneAfterLaneChange(final Duration durationInLaneAfterLaneChange) throws GTUException
775     {
776         Throw.when(durationInLaneAfterLaneChange.lt0(), GTUException.class, "durationInLaneAfterLaneChange should be >= 0");
777         this.durationInLaneAfterLaneChange = durationInLaneAfterLaneChange;
778     }
779 
780     /** {@inheritDoc} */
781     @Override
782     public final String toString()
783     {
784         return "LaneBasedGTUFollowingChange0TacticalPlanner [earliestNexLaneChangeTime=" + this.earliestNextLaneChangeTime
785                 + ", referenceLane=" + this.laneAfterLaneChange + ", referencePos=" + this.posAfterLaneChange
786                 + ", destroyGtuOnFailure=" + this.destroyGtuOnFailure + "]";
787     }
788 
789 }