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.LinkedHashMap;
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.value.ValueRuntimeException;
12  import org.djunits.value.storage.StorageType;
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.djunits.value.vdouble.vector.AccelerationVector;
19  import org.djutils.draw.point.OrientedPoint2d;
20  import org.opentrafficsim.base.parameters.ParameterException;
21  import org.opentrafficsim.base.parameters.ParameterTypeLength;
22  import org.opentrafficsim.base.parameters.ParameterTypes;
23  import org.opentrafficsim.core.geometry.OtsLine2d;
24  import org.opentrafficsim.core.gtu.GtuException;
25  import org.opentrafficsim.core.gtu.GtuType;
26  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
27  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
28  import org.opentrafficsim.core.gtu.plan.operational.Segments;
29  import org.opentrafficsim.core.network.LateralDirectionality;
30  import org.opentrafficsim.core.network.Link;
31  import org.opentrafficsim.core.network.NetworkException;
32  import org.opentrafficsim.core.network.Node;
33  import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
34  import org.opentrafficsim.road.gtu.lane.perception.CategoricalLanePerception;
35  import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
36  import org.opentrafficsim.road.gtu.lane.perception.categories.DefaultSimplePerception;
37  import org.opentrafficsim.road.gtu.lane.perception.categories.DirectDefaultSimplePerception;
38  import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
39  import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
40  import org.opentrafficsim.road.gtu.lane.tactical.following.GtuFollowingModelOld;
41  import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
42  import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneMovementStep;
43  import org.opentrafficsim.road.network.lane.CrossSectionElement;
44  import org.opentrafficsim.road.network.lane.CrossSectionLink;
45  import org.opentrafficsim.road.network.lane.Lane;
46  import org.opentrafficsim.road.network.lane.LanePosition;
47  import org.opentrafficsim.road.network.lane.object.detector.LaneDetector;
48  import org.opentrafficsim.road.network.lane.object.detector.SinkDetector;
49  
50  /**
51   * Lane-based tactical planner that implements car following and lane change behavior. This lane-based tactical planner makes
52   * decisions based on headway (GTU following model) and lane change (Lane Change model), and will generate an operational plan
53   * for the GTU. It can ask the strategic planner for assistance on the route to take when the network splits.
54   * <p>
55   * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
56   * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
57   * </p>
58   * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
59   * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
60   */
61  public class LaneBasedCfLcTacticalPlanner extends AbstractLaneBasedTacticalPlanner
62  {
63      /** */
64      private static final long serialVersionUID = 20151125L;
65  
66      /** Look back parameter type. */
67      protected static final ParameterTypeLength LOOKBACKOLD = ParameterTypes.LOOKBACKOLD;
68  
69      /** Standard incentive to stay in the current lane. */
70      private static final Acceleration STAYINCURRENTLANEINCENTIVE = new Acceleration(0.1, AccelerationUnit.METER_PER_SECOND_2);
71  
72      /** Standard incentive to stay in the current lane. */
73      private static final Acceleration PREFERREDLANEINCENTIVE = new Acceleration(0.3, AccelerationUnit.METER_PER_SECOND_2);
74  
75      /** Standard incentive to stay in the current lane. */
76      private static final Acceleration NONPREFERREDLANEINCENTIVE = new Acceleration(-0.3, AccelerationUnit.METER_PER_SECOND_2);
77  
78      /** Return value of suitability when no lane change is required within the time horizon. */
79      public static final Length NOLANECHANGENEEDED = new Length(Double.MAX_VALUE, LengthUnit.SI);
80  
81      /** Return value of suitability when a lane change is required <i>right now</i>. */
82      public static final Length GETOFFTHISLANENOW = Length.ZERO;
83  
84      /** Standard time horizon for route choices. */
85      private static final Duration TIMEHORIZON = new Duration(90, DurationUnit.SECOND);
86  
87      /** Lane change model for this tactical planner. */
88      private LaneChangeModel laneChangeModel;
89  
90      /**
91       * Instantiated a tactical planner with GTU following and lane change behavior.
92       * @param carFollowingModel GtuFollowingModelOld; Car-following model.
93       * @param laneChangeModel LaneChangeModel; Lane change model.
94       * @param gtu LaneBasedGtu; GTU
95       */
96      public LaneBasedCfLcTacticalPlanner(final GtuFollowingModelOld carFollowingModel, final LaneChangeModel laneChangeModel,
97              final LaneBasedGtu gtu)
98      {
99          super(carFollowingModel, gtu, new CategoricalLanePerception(gtu));
100         this.laneChangeModel = laneChangeModel;
101         getPerception().addPerceptionCategory(new DirectDefaultSimplePerception(getPerception()));
102     }
103 
104     /** {@inheritDoc} */
105     @Override
106     public final OperationalPlan generateOperationalPlan(final Time startTime, final OrientedPoint2d locationAtStartTime)
107             throws OperationalPlanException, NetworkException, GtuException, ParameterException
108     {
109         try
110         {
111             // define some basic variables
112             LaneBasedGtu laneBasedGTU = getGtu();
113             LanePerception perception = getPerception();
114 
115             // if the GTU's maximum speed is zero (block), generate a stand still plan for one second
116             if (laneBasedGTU.getMaximumSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
117             {
118                 return OperationalPlan.standStill(getGtu(), getGtu().getLocation(), startTime, Duration.ONE);
119             }
120 
121             Length maximumForwardHeadway = laneBasedGTU.getParameters().getParameter(LOOKAHEAD);
122             DefaultSimplePerception simplePerception = perception.getPerceptionCategory(DefaultSimplePerception.class);
123             simplePerception.updateSpeedLimit();
124             simplePerception.updateForwardHeadwayGtu();
125             simplePerception.updateForwardHeadwayObject();
126             simplePerception.updateBackwardHeadway();
127             simplePerception.updateNeighboringHeadwaysLeft();
128             simplePerception.updateNeighboringHeadwaysRight();
129             Speed speedLimit = simplePerception.getSpeedLimit();
130 
131             // look at the conditions for headway on the current lane
132             Headway sameLaneLeader = simplePerception.getForwardHeadwayGtu();
133             // TODO how to handle objects on this lane or another lane???
134             Headway sameLaneFollower = simplePerception.getBackwardHeadway();
135             Collection<Headway> sameLaneTraffic = new ArrayList<>();
136             if (sameLaneLeader.getObjectType().isGtu())
137             {
138                 sameLaneTraffic.add(sameLaneLeader);
139             }
140             if (sameLaneFollower.getObjectType().isGtu())
141             {
142                 sameLaneTraffic.add(sameLaneFollower);
143             }
144 
145             // Are we in the right lane for the route?
146             LanePathInfo lanePathInfo = buildLanePathInfo(laneBasedGTU, maximumForwardHeadway);
147 
148             // TODO these two info's are not used
149             NextSplitInfo nextSplitInfo = determineNextSplit(laneBasedGTU, maximumForwardHeadway);
150             boolean currentLaneFine = nextSplitInfo.correctCurrentLanes().contains(lanePathInfo.getReferenceLane());
151 
152             // calculate the lane change step
153             // TODO skip if:
154             // - we are in the right lane and drive at max speed or we accelerate maximally
155             // - there are no other lanes
156             Collection<Headway> leftLaneTraffic = simplePerception.getNeighboringHeadwaysLeft();
157             Collection<Headway> rightLaneTraffic = simplePerception.getNeighboringHeadwaysRight();
158 
159             // FIXME: whether we drive on the right should be stored in some central place.
160             final LateralDirectionality preferred = LateralDirectionality.RIGHT;
161             final Acceleration defaultLeftLaneIncentive =
162                     preferred.isLeft() ? PREFERREDLANEINCENTIVE : NONPREFERREDLANEINCENTIVE;
163             final Acceleration defaultRightLaneIncentive =
164                     preferred.isRight() ? PREFERREDLANEINCENTIVE : NONPREFERREDLANEINCENTIVE;
165 
166             AccelerationVector defaultLaneIncentives = new AccelerationVector(new double[] {defaultLeftLaneIncentive.getSI(),
167                     STAYINCURRENTLANEINCENTIVE.getSI(), defaultRightLaneIncentive.getSI()}, AccelerationUnit.SI);
168             AccelerationVector laneIncentives = laneIncentives(laneBasedGTU, defaultLaneIncentives);
169             LaneMovementStep lcmr = this.laneChangeModel.computeLaneChangeAndAcceleration(laneBasedGTU, sameLaneTraffic,
170                     rightLaneTraffic, leftLaneTraffic, speedLimit,
171                     new Acceleration(laneIncentives.get(preferred.isRight() ? 2 : 0)), new Acceleration(laneIncentives.get(1)),
172                     new Acceleration(laneIncentives.get(preferred.isRight() ? 0 : 2)));
173             Duration duration = lcmr.getGfmr().getValidUntil().minus(getGtu().getSimulator().getSimulatorAbsTime());
174             if (lcmr.getLaneChangeDirection() != null)
175             {
176                 laneBasedGTU.changeLaneInstantaneously(lcmr.getLaneChangeDirection());
177 
178                 // create the path to drive in this timestep.
179                 lanePathInfo = buildLanePathInfo(laneBasedGTU, maximumForwardHeadway);
180             }
181 
182             // incorporate traffic light
183             Headway object = simplePerception.getForwardHeadwayObject();
184             Acceleration a = lcmr.getGfmr().getAcceleration();
185             if (object instanceof HeadwayTrafficLight)
186             {
187                 // if it was perceived, it was red, or yellow and judged as requiring to stop
188                 a = Acceleration.min(a, ((GtuFollowingModelOld) getCarFollowingModel()).computeAcceleration(getGtu().getSpeed(),
189                         getGtu().getMaximumSpeed(), Speed.ZERO, object.getDistance(), speedLimit));
190             }
191 
192             // incorporate dead-end/split
193             Length dist = lanePathInfo.path().getLength().minus(getGtu().getFront().dx());
194             a = Acceleration.min(a, ((GtuFollowingModelOld) getCarFollowingModel()).computeAcceleration(getGtu().getSpeed(),
195                     getGtu().getMaximumSpeed(), Speed.ZERO, dist, speedLimit));
196 
197             // build a list of lanes forward, with a maximum headway.
198             if (a.si < 1E-6 && laneBasedGTU.getSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
199             {
200                 return OperationalPlan.standStill(getGtu(), getGtu().getLocation(), startTime, Duration.ONE);
201             }
202             OtsLine2d path = lanePathInfo.path();
203             OperationalPlan op = new OperationalPlan(getGtu(), path, startTime, Segments.off(getGtu().getSpeed(), duration, a));
204             return op;
205         }
206         catch (ValueRuntimeException exception)
207         {
208             throw new GtuException(exception);
209         }
210     }
211 
212     /**
213      * TODO: move laneIncentives to LanePerception? Figure out if the default lane incentives are OK, or override them with
214      * values that should keep this GTU on the intended route.
215      * @param gtu LaneBasedGtu; the GTU for which to calculate the incentives
216      * @param defaultLaneIncentives AccelerationVector; the three lane incentives for the next left adjacent lane, the current
217      *            lane and the next right adjacent lane
218      * @return AccelerationVector; the (possibly adjusted) lane incentives
219      * @throws NetworkException on network inconsistency
220      * @throws ValueRuntimeException cannot happen
221      * @throws GtuException when the position of the GTU cannot be correctly determined
222      * @throws OperationalPlanException if DefaultAlexander perception category is not present
223      */
224     private AccelerationVector laneIncentives(final LaneBasedGtu gtu, final AccelerationVector defaultLaneIncentives)
225             throws NetworkException, ValueRuntimeException, GtuException, OperationalPlanException
226     {
227         Length leftSuitability = suitability(gtu, LateralDirectionality.LEFT);
228         Length currentSuitability = suitability(gtu, null);
229         Length rightSuitability = suitability(gtu, LateralDirectionality.RIGHT);
230         if (leftSuitability == NOLANECHANGENEEDED && currentSuitability == NOLANECHANGENEEDED
231                 && rightSuitability == NOLANECHANGENEEDED)
232         {
233             return checkLaneDrops(gtu, defaultLaneIncentives);
234         }
235         if ((leftSuitability == NOLANECHANGENEEDED || leftSuitability == GETOFFTHISLANENOW)
236                 && currentSuitability == NOLANECHANGENEEDED
237                 && (rightSuitability == NOLANECHANGENEEDED || rightSuitability == GETOFFTHISLANENOW))
238         {
239             return checkLaneDrops(gtu, new AccelerationVector(new double[] {acceleration(gtu, leftSuitability),
240                     defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability)}, AccelerationUnit.SI));
241         }
242         if (currentSuitability == NOLANECHANGENEEDED)
243         {
244             return new AccelerationVector(new double[] {acceleration(gtu, leftSuitability),
245                     defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability)}, AccelerationUnit.SI);
246         }
247         return new AccelerationVector(new double[] {acceleration(gtu, leftSuitability), acceleration(gtu, currentSuitability),
248                 acceleration(gtu, rightSuitability)}, AccelerationUnit.SI);
249     }
250 
251     /**
252      * Figure out if the default lane incentives are OK, or override them with values that should keep this GTU from running out
253      * of road at an upcoming lane drop.
254      * @param gtu LaneBasedGtu; the GTU for which to check the lane drops
255      * @param defaultLaneIncentives AccelerationVector; DoubleVector.Rel.Dense&lt;AccelerationUnit&gt; the three lane incentives
256      *            for the next left adjacent lane, the current lane and the next right adjacent lane
257      * @return DoubleVector.Rel.Dense&lt;AccelerationUnit&gt;; the (possibly adjusted) lane incentives
258      * @throws NetworkException on network inconsistency
259      * @throws ValueRuntimeException cannot happen
260      * @throws GtuException when the positions of the GTU cannot be determined
261      * @throws OperationalPlanException if DefaultAlexander perception category is not present
262      */
263     private AccelerationVector checkLaneDrops(final LaneBasedGtu gtu, final AccelerationVector defaultLaneIncentives)
264             throws NetworkException, ValueRuntimeException, GtuException, OperationalPlanException
265     {
266         // FIXME: these comparisons to -10 is ridiculous.
267         Length leftSuitability = Double.isNaN(defaultLaneIncentives.get(0).si) || defaultLaneIncentives.get(0).si < -10
268                 ? GETOFFTHISLANENOW : laneDrop(gtu, LateralDirectionality.LEFT);
269         Length currentSuitability = laneDrop(gtu, null);
270         Length rightSuitability = Double.isNaN(defaultLaneIncentives.get(2).si) || defaultLaneIncentives.get(2).si < -10
271                 ? GETOFFTHISLANENOW : laneDrop(gtu, LateralDirectionality.RIGHT);
272         // @formatter:off
273         if ((leftSuitability == NOLANECHANGENEEDED || leftSuitability == GETOFFTHISLANENOW)
274                 && currentSuitability == NOLANECHANGENEEDED
275                 && (rightSuitability == NOLANECHANGENEEDED || rightSuitability == GETOFFTHISLANENOW))
276         {
277             return defaultLaneIncentives;
278         }
279         // @formatter:on
280         if (currentSuitability == NOLANECHANGENEEDED)
281         {
282             return new AccelerationVector(new double[] {acceleration(gtu, leftSuitability),
283                     defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability)}, AccelerationUnit.SI);
284         }
285         if (currentSuitability.le(leftSuitability))
286         {
287             return new AccelerationVector(
288                     new double[] {PREFERREDLANEINCENTIVE.getSI(), NONPREFERREDLANEINCENTIVE.getSI(), GETOFFTHISLANENOW.getSI()},
289                     AccelerationUnit.SI);
290         }
291         if (currentSuitability.le(rightSuitability))
292         {
293             return new AccelerationVector(
294                     new double[] {GETOFFTHISLANENOW.getSI(), NONPREFERREDLANEINCENTIVE.getSI(), PREFERREDLANEINCENTIVE.getSI()},
295                     AccelerationUnit.SI, StorageType.DENSE);
296         }
297         return new AccelerationVector(new double[] {acceleration(gtu, leftSuitability), acceleration(gtu, currentSuitability),
298                 acceleration(gtu, rightSuitability)}, AccelerationUnit.SI);
299     }
300 
301     /**
302      * Return the distance until the next lane drop in the specified (nearby) lane.
303      * @param gtu LaneBasedGtu; the GTU to determine the next lane drop for
304      * @param direction LateralDirectionality; one of the values <cite>LateralDirectionality.LEFT</cite> (use the left-adjacent
305      *            lane), or <cite>LateralDirectionality.RIGHT</cite> (use the right-adjacent lane), or <cite>null</cite> (use
306      *            the current lane)
307      * @return DoubleScalar.Rel&lt;LengthUnit&gt;; distance until the next lane drop if it occurs within the TIMEHORIZON, or
308      *         LaneBasedRouteNavigator.NOLANECHANGENEEDED if this lane can be followed until the next split junction or until
309      *         beyond the TIMEHORIZON
310      * @throws NetworkException on network inconsistency
311      * @throws GtuException when the positions of the GTU cannot be determined
312      * @throws OperationalPlanException if DefaultAlexander perception category is not present
313      */
314     private Length laneDrop(final LaneBasedGtu gtu, final LateralDirectionality direction)
315             throws NetworkException, GtuException, OperationalPlanException
316     {
317         LanePosition dlp = gtu.getReferencePosition();
318         Lane lane = dlp.lane();
319         Length longitudinalPosition = dlp.position();
320         if (null != direction)
321         {
322             lane = getPerception().getPerceptionCategory(DefaultSimplePerception.class).bestAccessibleAdjacentLane(lane,
323                     direction, longitudinalPosition);
324         }
325         if (null == lane)
326         {
327             return GETOFFTHISLANENOW;
328         }
329         double remainingLength = lane.getLength().getSI() - longitudinalPosition.getSI();
330         double remainingTimeSI = TIMEHORIZON.getSI() - remainingLength / lane.getSpeedLimit(gtu.getType()).getSI();
331         while (remainingTimeSI >= 0)
332         {
333             for (LaneDetector detector : lane.getDetectors())
334             {
335                 if (detector instanceof SinkDetector)
336                 {
337                     return NOLANECHANGENEEDED;
338                 }
339             }
340             int branching = lane.nextLanes(gtu.getType()).size();
341             if (branching == 0)
342             {
343                 return new Length(remainingLength, LengthUnit.SI);
344             }
345             if (branching > 1)
346             {
347                 return NOLANECHANGENEEDED;
348             }
349             lane = lane.nextLanes(gtu.getType()).iterator().next();
350             remainingTimeSI -= lane.getLength().getSI() / lane.getSpeedLimit(gtu.getType()).getSI();
351             remainingLength += lane.getLength().getSI();
352         }
353         return NOLANECHANGENEEDED;
354     }
355 
356     /**
357      * TODO: move suitability to LanePerception? Return the suitability for the current lane, left adjacent lane or right
358      * adjacent lane.
359      * @param gtu LaneBasedGtu; the GTU for which to calculate the incentives
360      * @param direction LateralDirectionality; one of the values <cite>null</cite>, <cite>LateralDirectionality.LEFT</cite>, or
361      *            <cite>LateralDirectionality.RIGHT</cite>
362      * @return DoubleScalar.Rel&lt;LengthUnit&gt;; the suitability of the lane for reaching the (next) destination
363      * @throws NetworkException on network inconsistency
364      * @throws GtuException when position cannot be determined
365      * @throws OperationalPlanException if DefaultAlexander perception category is not present
366      */
367     private Length suitability(final LaneBasedGtu gtu, final LateralDirectionality direction)
368             throws NetworkException, GtuException, OperationalPlanException
369     {
370         LanePosition dlp = gtu.getReferencePosition();
371         Lane lane = dlp.lane();
372         Length longitudinalPosition = dlp.position().plus(gtu.getFront().dx());
373         if (null != direction)
374         {
375             lane = getPerception().getPerceptionCategory(DefaultSimplePerception.class).bestAccessibleAdjacentLane(lane,
376                     direction, longitudinalPosition);
377         }
378         if (null == lane)
379         {
380             return GETOFFTHISLANENOW;
381         }
382         try
383         {
384             return suitability(lane, longitudinalPosition, gtu, TIMEHORIZON);
385             // return suitability(lane, lane.getLength().minus(longitudinalPosition), gtu, TIMEHORIZON);
386         }
387         catch (NetworkException ne)
388         {
389             System.err.println(gtu + " has a route problem in suitability: " + ne.getMessage());
390             return NOLANECHANGENEEDED;
391         }
392     }
393 
394     /**
395      * Compute deceleration needed to stop at a specified distance.
396      * @param gtu LaneBasedGtu; the GTU for which to calculate the acceleration to come to a full stop at the distance
397      * @param stopDistance Length; the distance
398      * @return double; the acceleration (deceleration) needed to stop at the specified distance in m/s/s
399      */
400     private double acceleration(final LaneBasedGtu gtu, final Length stopDistance)
401     {
402         // What is the deceleration that will bring this GTU to a stop at exactly the suitability distance?
403         // Answer: a = -v^2 / 2 / suitabilityDistance
404         double v = gtu.getSpeed().getSI();
405         double a = -v * v / 2 / stopDistance.getSI();
406         return a;
407     }
408 
409     /**
410      * Determine the suitability of being at a particular longitudinal position in a particular Lane for following this Route.
411      * <br>
412      * @param lane Lane; the lane to consider
413      * @param longitudinalPosition Length; the longitudinal position in the lane
414      * @param gtu LaneBasedGtu; the GTU (used to check lane compatibility of lanes, and current lane the GTU is on)
415      * @param timeHorizon Duration; the maximum time that a driver may want to look ahead
416      * @return DoubleScalar.Rel&lt;LengthUnit&gt;; a value that indicates within what distance the GTU should try to vacate this
417      *         lane.
418      * @throws NetworkException on network inconsistency, or when the continuation Link at a branch cannot be determined
419      */
420     private Length suitability(final Lane lane, final Length longitudinalPosition, final LaneBasedGtu gtu,
421             final Duration timeHorizon) throws NetworkException
422     {
423         double remainingDistance = lane.getLength().getSI() - longitudinalPosition.getSI();
424         double spareTime = timeHorizon.getSI() - remainingDistance / lane.getSpeedLimit(gtu.getType()).getSI();
425         // Find the first upcoming Node where there is a branch
426         Node nextNode = lane.getLink().getEndNode();
427         Link lastLink = lane.getLink();
428         Node nextSplitNode = null;
429         Lane currentLane = lane;
430         CrossSectionLink linkBeforeBranch = lane.getLink();
431         while (null != nextNode)
432         {
433             if (spareTime <= 0)
434             {
435                 return NOLANECHANGENEEDED; // It is not yet time to worry; this lane will do as well as any other
436             }
437             int laneCount = countCompatibleLanes(linkBeforeBranch, gtu.getType());
438             if (0 == laneCount)
439             {
440                 throw new NetworkException("No compatible Lanes on Link " + linkBeforeBranch);
441             }
442             if (1 == laneCount)
443             {
444                 return NOLANECHANGENEEDED; // Only one compatible lane available; we'll get there "automatically";
445                 // i.e. without influence from the Route
446             }
447             int branching = nextNode.getLinks().size();
448             if (branching > 2)
449             { // Found a split
450                 nextSplitNode = nextNode;
451                 break;
452             }
453             else if (1 == branching)
454             {
455                 return NOLANECHANGENEEDED; // dead end; no more choices to make
456             }
457             else
458             { // Look beyond this nextNode
459                 Link nextLink = gtu.getStrategicalPlanner().nextLink(lastLink, gtu.getType());
460                 if (nextLink instanceof CrossSectionLink)
461                 {
462                     nextNode = nextLink.getEndNode();
463                     // Oops: wrong code added the length of linkBeforeBranch in stead of length of nextLink
464                     remainingDistance += nextLink.getLength().getSI();
465                     linkBeforeBranch = (CrossSectionLink) nextLink;
466                     // Figure out the new currentLane
467                     if (currentLane.nextLanes(gtu.getType()).size() == 0)
468                     {
469                         // Lane drop; our lane disappears. This is a compulsory lane change; which is not controlled
470                         // by the Route. Perform the forced lane change.
471                         if (currentLane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtu.getType()).size() > 0)
472                         {
473                             for (Lane adjacentLane : currentLane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT,
474                                     gtu.getType()))
475                             {
476                                 if (adjacentLane.nextLanes(gtu.getType()).size() > 0)
477                                 {
478                                     currentLane = adjacentLane;
479                                     break;
480                                 }
481                                 // If there are several adjacent lanes that have non empty nextLanes, we simple take the
482                                 // first in the set
483                             }
484                         }
485                         for (Lane adjacentLane : currentLane.accessibleAdjacentLanesLegal(LateralDirectionality.LEFT,
486                                 gtu.getType()))
487                         {
488                             if (adjacentLane.nextLanes(gtu.getType()).size() > 0)
489                             {
490                                 currentLane = adjacentLane;
491                                 break;
492                             }
493                             // If there are several adjacent lanes that have non empty nextLanes, we simple take the
494                             // first in the set
495                         }
496                         if (currentLane.nextLanes(gtu.getType()).size() == 0)
497                         {
498                             throw new NetworkException(
499                                     "Lane ends and there is not a compatible adjacent lane that does " + "not end");
500                         }
501                     }
502                     // Any compulsory lane change(s) have been performed and there is guaranteed a compatible next lane.
503                     for (Lane nextLane : currentLane.nextLanes(gtu.getType()))
504                     {
505                         currentLane = currentLane.nextLanes(gtu.getType()).iterator().next();
506                         break;
507                     }
508                     spareTime -= currentLane.getLength().getSI() / currentLane.getSpeedLimit(gtu.getType()).getSI();
509                 }
510                 else
511                 {
512                     // There is a non-CrossSectionLink on the path to the next branch. A non-CrossSectionLink does not
513                     // have identifiable Lanes, therefore we can't aim for a particular Lane
514                     return NOLANECHANGENEEDED; // Any Lane will do equally well
515                 }
516                 lastLink = nextLink;
517             }
518         }
519         if (null == nextNode)
520         {
521             throw new NetworkException("Cannot find the next branch or sink node");
522         }
523         // We have now found the first upcoming branching Node
524         // Which continuing link is the one we need?
525         Map<Lane, Length> suitabilityOfLanesBeforeBranch = new LinkedHashMap<>();
526         Link linkAfterBranch = gtu.getStrategicalPlanner().nextLink(lastLink, gtu.getType());
527         for (CrossSectionElement cse : linkBeforeBranch.getCrossSectionElementList())
528         {
529             if (cse instanceof Lane)
530             {
531                 Lane l = (Lane) cse;
532                 if (l.getType().isCompatible(gtu.getType()))
533                 {
534                     for (Lane connectingLane : l.nextLanes(gtu.getType()))
535                     {
536                         if (connectingLane.getLink() == linkAfterBranch && connectingLane.getType().isCompatible(gtu.getType()))
537                         {
538                             Length currentValue = suitabilityOfLanesBeforeBranch.get(l);
539                             // Use recursion to find out HOW suitable this continuation lane is, but don't revert back
540                             // to the maximum time horizon (or we could end up in infinite recursion when there are
541                             // loops in the network).
542                             Length value = suitability(connectingLane, new Length(0, LengthUnit.SI), gtu,
543                                     new Duration(spareTime, DurationUnit.SI));
544                             // This line was missing...
545                             value = value.plus(new Length(remainingDistance, LengthUnit.SI));
546                             // Use the minimum of the value computed for the first split junction (if there is one)
547                             // and the value computed for the second split junction.
548                             suitabilityOfLanesBeforeBranch.put(l,
549                                     null == currentValue || value.le(currentValue) ? value : currentValue);
550                         }
551                     }
552                 }
553             }
554         }
555         if (suitabilityOfLanesBeforeBranch.size() == 0)
556         {
557             throw new NetworkException("No lanes available on Link " + linkBeforeBranch);
558         }
559         Length currentLaneSuitability = suitabilityOfLanesBeforeBranch.get(currentLane);
560         if (null != currentLaneSuitability)
561         {
562             return currentLaneSuitability; // Following the current lane will keep us on the Route
563         }
564         // Performing one or more lane changes (left or right) is required.
565         int totalLanes = countCompatibleLanes(currentLane.getLink(), gtu.getType());
566         Length leftSuitability = computeSuitabilityWithLaneChanges(currentLane, remainingDistance,
567                 suitabilityOfLanesBeforeBranch, totalLanes, LateralDirectionality.LEFT, gtu.getType());
568         Length rightSuitability = computeSuitabilityWithLaneChanges(currentLane, remainingDistance,
569                 suitabilityOfLanesBeforeBranch, totalLanes, LateralDirectionality.RIGHT, gtu.getType());
570         if (leftSuitability.ge(rightSuitability))
571         {
572             return leftSuitability;
573         }
574         else if (rightSuitability.ge(leftSuitability))
575         {
576             // TODO
577             return rightSuitability;
578         }
579         if (leftSuitability.le(GETOFFTHISLANENOW))
580         {
581             throw new NetworkException("Changing lanes in any direction does not get the GTU on a suitable lane");
582         }
583         return leftSuitability; // left equals right; this is odd but topologically possible
584     }
585 
586     /**
587      * Compute the suitability of a lane from which lane changes are required to get to the next point on the Route.<br>
588      * This method weighs the suitability of the nearest suitable lane by (m - n) / m where n is the number of lane changes
589      * required and m is the total number of lanes in the CrossSectionLink.
590      * @param startLane Lane; the current lane of the GTU
591      * @param remainingDistance double; distance in m of GTU to first branch
592      * @param suitabilities Map&lt;Lane, Length&gt;; the set of suitable lanes and their suitability
593      * @param totalLanes int; total number of lanes compatible with the GTU type
594      * @param direction LateralDirectionality; the direction of the lane changes to attempt
595      * @param gtuType GtuType; the type of the GTU
596      * @return double; the suitability of the <cite>startLane</cite> for following the Route
597      */
598     protected final Length computeSuitabilityWithLaneChanges(final Lane startLane, final double remainingDistance,
599             final Map<Lane, Length> suitabilities, final int totalLanes, final LateralDirectionality direction,
600             final GtuType gtuType)
601     {
602         /*-
603          * The time per required lane change seems more relevant than distance per required lane change.
604          * Total time required does not grow linearly with the number of required lane changes. Logarithmic, arc tangent 
605          * is more like it.
606          * Rijkswaterstaat appears to use a fixed time for ANY number of lane changes (about 60s). 
607          * TomTom navigation systems give more time (about 90s).
608          * In this method the returned suitability decreases linearly with the number of required lane changes. This
609          * ensures that there is a gradient that coaches the GTU towards the most suitable lane.
610          */
611         int laneChangesUsed = 0;
612         Lane currentLane = startLane;
613         Length currentSuitability = null;
614         while (null == currentSuitability)
615         {
616             laneChangesUsed++;
617             if (currentLane.accessibleAdjacentLanesLegal(direction, gtuType).size() == 0)
618             {
619                 return GETOFFTHISLANENOW;
620             }
621             currentLane = currentLane.accessibleAdjacentLanesLegal(direction, gtuType).iterator().next();
622             currentSuitability = suitabilities.get(currentLane);
623         }
624         double fraction = currentSuitability == NOLANECHANGENEEDED ? 0 : 0.5;
625         int notSuitableLaneCount = totalLanes - suitabilities.size();
626         return new Length(
627                 remainingDistance * (notSuitableLaneCount - laneChangesUsed + 1 + fraction) / (notSuitableLaneCount + fraction),
628                 LengthUnit.SI);
629     }
630 
631     /**
632      * Determine how many lanes on a CrossSectionLink are compatible with a particular GTU type.<br>
633      * TODO: this method should probably be moved into the CrossSectionLink class
634      * @param link CrossSectionLink; the link
635      * @param gtuType GtuType; the GTU type
636      * @return integer; the number of lanes on the link that are compatible with the GTU type
637      */
638     protected final int countCompatibleLanes(final CrossSectionLink link, final GtuType gtuType)
639     {
640         int result = 0;
641         for (CrossSectionElement cse : link.getCrossSectionElementList())
642         {
643             if (cse instanceof Lane)
644             {
645                 Lane l = (Lane) cse;
646                 if (l.getType().isCompatible(gtuType))
647                 {
648                     result++;
649                 }
650             }
651         }
652         return result;
653     }
654 
655     /** {@inheritDoc} */
656     @Override
657     public final String toString()
658     {
659         return "LaneBasedCFLCTacticalPlanner [laneChangeModel=" + this.laneChangeModel + "]";
660     }
661 
662 }