LaneBasedCFLCTacticalPlanner.java
package org.opentrafficsim.road.gtu.lane.tactical;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
import nl.tudelft.simulation.language.d3.DirectedPoint;
import org.djunits.unit.AccelerationUnit;
import org.djunits.unit.LengthUnit;
import org.djunits.unit.TimeUnit;
import org.djunits.value.StorageType;
import org.djunits.value.ValueException;
import org.djunits.value.vdouble.scalar.Acceleration;
import org.djunits.value.vdouble.scalar.Duration;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Speed;
import org.djunits.value.vdouble.scalar.Time;
import org.djunits.value.vdouble.vector.AccelerationVector;
import org.opentrafficsim.core.geometry.OTSLine3D;
import org.opentrafficsim.core.gtu.GTU;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.GTUType;
import org.opentrafficsim.core.gtu.RelativePosition;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypes;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.core.network.Link;
import org.opentrafficsim.core.network.NetworkException;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.perception.Headway;
import org.opentrafficsim.road.gtu.lane.perception.HeadwayGTU;
import org.opentrafficsim.road.gtu.lane.perception.LanePerceptionFull;
import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneMovementStep;
import org.opentrafficsim.road.network.lane.CrossSectionElement;
import org.opentrafficsim.road.network.lane.CrossSectionLink;
import org.opentrafficsim.road.network.lane.Lane;
import org.opentrafficsim.road.network.lane.Sensor;
import org.opentrafficsim.road.network.lane.SinkSensor;
/**
* Lane-based tactical planner that implements car following and lane change behavior. This lane-based tactical planner makes
* decisions based on headway (GTU following model) and lane change (Lane Change model), and will generate an operational plan
* for the GTU. It can ask the strategic planner for assistance on the route to take when the network splits.
* <p>
* Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
* </p>
* $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
* initial version Nov 25, 2015 <br>
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
*/
public class LaneBasedCFLCTacticalPlanner extends AbstractLaneBasedTacticalPlanner
{
/** */
private static final long serialVersionUID = 20151125L;
/** Standard incentive to stay in the current lane. */
private static final Acceleration STAYINCURRENTLANEINCENTIVE = new Acceleration(0.1, AccelerationUnit.METER_PER_SECOND_2);
/** Standard incentive to stay in the current lane. */
private static final Acceleration PREFERREDLANEINCENTIVE = new Acceleration(0.3, AccelerationUnit.METER_PER_SECOND_2);
/** Standard incentive to stay in the current lane. */
private static final Acceleration NONPREFERREDLANEINCENTIVE = new Acceleration(-0.3, AccelerationUnit.METER_PER_SECOND_2);
/** Return value of suitability when no lane change is required within the time horizon. */
public static final Length NOLANECHANGENEEDED = new Length(Double.MAX_VALUE, LengthUnit.SI);
/** Return value of suitability when a lane change is required <i>right now</i>. */
public static final Length GETOFFTHISLANENOW = Length.ZERO;
/** Standard time horizon for route choices. */
private static final Duration TIMEHORIZON = new Duration(90, TimeUnit.SECOND);
/** Lane change model for this tactical planner. */
private LaneChangeModel laneChangeModel;
/**
* Instantiated a tactical planner with GTU following and lane change behavior.
* @param carFollowingModel Car-following model.
* @param laneChangeModel Lane change model.
*/
public LaneBasedCFLCTacticalPlanner(final GTUFollowingModelOld carFollowingModel, final LaneChangeModel laneChangeModel)
{
super(carFollowingModel);
this.laneChangeModel = laneChangeModel;
}
/** {@inheritDoc} */
@Override
public OperationalPlan generateOperationalPlan(final GTU gtu, final Time startTime, final DirectedPoint locationAtStartTime)
throws OperationalPlanException, NetworkException, GTUException, ParameterException
{
try
{
// define some basic variables
LaneBasedGTU laneBasedGTU = (LaneBasedGTU) gtu;
LanePerceptionFull perception = laneBasedGTU.getPerception();
// if the GTU's maximum speed is zero (block), generate a stand still plan for one second
if (laneBasedGTU.getMaximumVelocity().si < OperationalPlan.DRIFTING_SPEED_SI)
{
return new OperationalPlan(gtu, locationAtStartTime, startTime, new Duration(1.0, TimeUnit.SECOND));
}
// perceive every time step... This is the 'classical' way of tactical planning.
// NOTE: delete this if perception takes place independent of the tactical planning (different frequency)
perception.perceive();
Length maximumForwardHeadway = laneBasedGTU.getBehavioralCharacteristics().getParameter(ParameterTypes.LOOKAHEAD);
Length maximumReverseHeadway = laneBasedGTU.getBehavioralCharacteristics().getParameter(ParameterTypes.LOOKBACKOLD);
Time now = gtu.getSimulator().getSimulatorTime().getTime();
Speed speedLimit = perception.getSpeedLimit();
// look at the conditions for headway on the current lane
Headway sameLaneLeader = perception.getForwardHeadway();
Headway sameLaneFollower = perception.getBackwardHeadway();
Collection<Headway> sameLaneTraffic = new ArrayList<Headway>();
if (sameLaneLeader.getObjectType().isGtu())
{
sameLaneTraffic.add(sameLaneLeader);
}
if (sameLaneFollower.getObjectType().isGtu())
{
sameLaneTraffic.add(sameLaneFollower);
}
// Are we in the right lane for the route?
LanePathInfo lanePathInfo = buildLanePathInfo(laneBasedGTU, maximumForwardHeadway);
NextSplitInfo nextSplitInfo = determineNextSplit(laneBasedGTU, maximumForwardHeadway);
boolean currentLaneFine = nextSplitInfo.getCorrectCurrentLanes().contains(lanePathInfo.getReferenceLane());
// calculate the lane change step
// TODO skip if:
// - we are in the right lane and drive at max speed or we accelerate maximally
// - there are no other lanes
Collection<Headway> leftLaneTraffic = perception.getNeighboringHeadwaysLeft();
Collection<Headway> rightLaneTraffic = perception.getNeighboringHeadwaysRight();
// FIXME: whether we drive on the right should be stored in some central place.
final LateralDirectionality preferred = LateralDirectionality.RIGHT;
final Acceleration defaultLeftLaneIncentive =
preferred.isLeft() ? PREFERREDLANEINCENTIVE : NONPREFERREDLANEINCENTIVE;
final Acceleration defaultRightLaneIncentive =
preferred.isRight() ? PREFERREDLANEINCENTIVE : NONPREFERREDLANEINCENTIVE;
AccelerationVector defaultLaneIncentives =
new AccelerationVector(new double[] { defaultLeftLaneIncentive.getSI(), STAYINCURRENTLANEINCENTIVE.getSI(),
defaultRightLaneIncentive.getSI() }, AccelerationUnit.SI, StorageType.DENSE);
AccelerationVector laneIncentives = laneIncentives(laneBasedGTU, defaultLaneIncentives);
LaneMovementStep lcmr =
this.laneChangeModel.computeLaneChangeAndAcceleration(laneBasedGTU, sameLaneTraffic, rightLaneTraffic,
leftLaneTraffic, speedLimit, new Acceleration(laneIncentives.get(preferred.isRight() ? 2 : 0)),
new Acceleration(laneIncentives.get(1)),
new Acceleration(laneIncentives.get(preferred.isRight() ? 0 : 2)));
Duration duration = lcmr.getGfmr().getValidUntil().minus(gtu.getSimulator().getSimulatorTime().getTime());
// if ("1".equals(gtu.getId()))
// {
// System.out.println(String.format("%s: laneIncentives %s, lcmr %s", gtu, laneIncentives, lcmr));
// }
if (lcmr.getLaneChange() != null)
{
Collection<Lane> oldLaneSet = new HashSet<Lane>(laneBasedGTU.getLanes().keySet());
Collection<Lane> newLaneSet = new HashSet<Lane>(2);
Map<Lane, Double> oldFractionalPositions = new LinkedHashMap<Lane, Double>();
for (Lane l : laneBasedGTU.getLanes().keySet())
{
oldFractionalPositions.put(l, laneBasedGTU.fractionalPosition(l, gtu.getReference(), now));
if (lcmr.getLaneChange().equals(LateralDirectionality.LEFT))
{
newLaneSet.addAll(laneBasedGTU.getPerception().getAccessibleAdjacentLanesLeft().get(l));
}
else
{
newLaneSet.addAll(laneBasedGTU.getPerception().getAccessibleAdjacentLanesRight().get(l));
}
}
// Add this GTU to the lanes in newLaneSet.
// This could be rewritten to be more efficient.
for (Lane newLane : newLaneSet)
{
Double fractionalPosition = null;
// find ONE lane in oldLaneSet that has l as neighbor
Lane foundOldLane = null;
for (Lane oldLane : oldLaneSet)
{
if ((lcmr.getLaneChange().equals(LateralDirectionality.LEFT) && laneBasedGTU.getPerception()
.getAccessibleAdjacentLanesLeft().get(oldLane).contains(newLane))
|| (lcmr.getLaneChange().equals(LateralDirectionality.RIGHT) && laneBasedGTU.getPerception()
.getAccessibleAdjacentLanesRight().get(oldLane).contains(newLane)))
{
fractionalPosition = oldFractionalPositions.get(oldLane);
foundOldLane = oldLane;
break;
}
}
if (null == fractionalPosition)
{
throw new Error("Program error: Cannot find an oldLane that has newLane " + newLane + " as "
+ lcmr.getLaneChange() + " neighbor");
}
laneBasedGTU.enterLane(newLane, newLane.getLength().multiplyBy(fractionalPosition), laneBasedGTU.getLanes()
.get(foundOldLane));
}
// if ("41".equals(gtu.getId()) && oldLaneSet.size() > 1)
// {
// System.out.println("Problem");
// AbstractLaneBasedGTU lbg = (AbstractLaneBasedGTU) gtu;
// for (Lane l : lbg.getLanes().keySet())
// {
// System.out.println("reference on lane " + l + " is "
// + lbg.position(l, RelativePosition.REFERENCE_POSITION) + " length of lane is " + l.getLength());
// }
// }
System.out.println(gtu + " changes lane from " + oldLaneSet + " to " + newLaneSet + " at time "
+ gtu.getSimulator().getSimulatorTime().get());
// Remove this GTU from all of the Lanes that it is on and remember the fractional position on each
// one
for (Lane l : oldFractionalPositions.keySet())
{
laneBasedGTU.leaveLane(l);
}
// create the path to drive in this timestep.
lanePathInfo = buildLanePathInfo(laneBasedGTU, maximumForwardHeadway);
// System.out.println("lane incentives: " + laneIncentives);
// build a list of lanes forward, with a maximum headway.
if (lcmr.getGfmr().getAcceleration().si < 1E-6
&& laneBasedGTU.getSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
{
// TODO Make a 100% lateral move from standing still...
return new OperationalPlan(gtu, locationAtStartTime, startTime, duration);
}
// TODO make a gradual lateral move
OTSLine3D path = lanePathInfo.getPath();
List<Segment> operationalPlanSegmentList = new ArrayList<>();
if (lcmr.getGfmr().getAcceleration().si == 0.0)
{
Segment segment = new OperationalPlan.SpeedSegment(duration);
operationalPlanSegmentList.add(segment);
}
else
{
Segment segment = new OperationalPlan.AccelerationSegment(duration, lcmr.getGfmr().getAcceleration());
operationalPlanSegmentList.add(segment);
}
OperationalPlan op = new OperationalPlan(gtu, path, startTime, gtu.getSpeed(), operationalPlanSegmentList);
return op;
}
else
// NO LANE CHANGE
{
// see if we have to continue standing still. In that case, generate a stand still plan
if (lcmr.getGfmr().getAcceleration().si < 1E-6
&& laneBasedGTU.getSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
{
return new OperationalPlan(gtu, locationAtStartTime, startTime, duration);
}
// build a list of lanes forward, with a maximum headway.
OTSLine3D path = lanePathInfo.getPath();
List<Segment> operationalPlanSegmentList = new ArrayList<>();
if (lcmr.getGfmr().getAcceleration().si == 0.0)
{
Segment segment = new OperationalPlan.SpeedSegment(duration);
operationalPlanSegmentList.add(segment);
}
else
{
Segment segment = new OperationalPlan.AccelerationSegment(duration, lcmr.getGfmr().getAcceleration());
operationalPlanSegmentList.add(segment);
}
OperationalPlan op = new OperationalPlan(gtu, path, startTime, gtu.getSpeed(), operationalPlanSegmentList);
return op;
}
}
catch (ValueException exception)
{
throw new GTUException(exception);
}
}
/**
* TODO: move laneIncentives to LanePerception? Figure out if the default lane incentives are OK, or override them with
* values that should keep this GTU on the intended route.
* @param gtu the GTU for which to calculate the incentives
* @param defaultLaneIncentives AccelerationVector; the three lane incentives for the next left adjacent lane, the current
* lane and the next right adjacent lane
* @return AccelerationVector; the (possibly adjusted) lane incentives
* @throws NetworkException on network inconsistency
* @throws ValueException cannot happen
* @throws GTUException when the position of the GTU cannot be correctly determined
*/
private AccelerationVector laneIncentives(final LaneBasedGTU gtu, final AccelerationVector defaultLaneIncentives)
throws NetworkException, ValueException, GTUException
{
Length leftSuitability = suitability(gtu, LateralDirectionality.LEFT);
Length currentSuitability = suitability(gtu, null);
Length rightSuitability = suitability(gtu, LateralDirectionality.RIGHT);
if (leftSuitability == NOLANECHANGENEEDED && currentSuitability == NOLANECHANGENEEDED
&& rightSuitability == NOLANECHANGENEEDED)
{
return checkLaneDrops(gtu, defaultLaneIncentives);
}
if ((leftSuitability == NOLANECHANGENEEDED || leftSuitability == GETOFFTHISLANENOW)
&& currentSuitability == NOLANECHANGENEEDED
&& (rightSuitability == NOLANECHANGENEEDED || rightSuitability == GETOFFTHISLANENOW))
{
return checkLaneDrops(gtu, new AccelerationVector(new double[] { acceleration(gtu, leftSuitability),
defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability) }, AccelerationUnit.SI,
StorageType.DENSE));
}
if (currentSuitability == NOLANECHANGENEEDED)
{
return new AccelerationVector(new double[] { acceleration(gtu, leftSuitability),
defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability) }, AccelerationUnit.SI,
StorageType.DENSE);
}
return new AccelerationVector(new double[] { acceleration(gtu, leftSuitability), acceleration(gtu, currentSuitability),
acceleration(gtu, rightSuitability) }, AccelerationUnit.SI, StorageType.DENSE);
}
/**
* Figure out if the default lane incentives are OK, or override them with values that should keep this GTU from running out
* of road at an upcoming lane drop.
* @param gtu the GTU for which to check the lane drops
* @param defaultLaneIncentives DoubleVector.Rel.Dense<AccelerationUnit> the three lane incentives for the next left
* adjacent lane, the current lane and the next right adjacent lane
* @return DoubleVector.Rel.Dense<AccelerationUnit>; the (possibly adjusted) lane incentives
* @throws NetworkException on network inconsistency
* @throws ValueException cannot happen
* @throws GTUException when the positions of the GTU cannot be determined
*/
private AccelerationVector checkLaneDrops(final LaneBasedGTU gtu, final AccelerationVector defaultLaneIncentives)
throws NetworkException, ValueException, GTUException
{
// FIXME: these comparisons to -10 is ridiculous.
Length leftSuitability =
Double.isNaN(defaultLaneIncentives.get(0).si) || defaultLaneIncentives.get(0).si < -10 ? GETOFFTHISLANENOW
: laneDrop(gtu, LateralDirectionality.LEFT);
Length currentSuitability = laneDrop(gtu, null);
Length rightSuitability =
Double.isNaN(defaultLaneIncentives.get(2).si) || defaultLaneIncentives.get(2).si < -10 ? GETOFFTHISLANENOW
: laneDrop(gtu, LateralDirectionality.RIGHT);
// @formatter:off
if ((leftSuitability == NOLANECHANGENEEDED || leftSuitability == GETOFFTHISLANENOW)
&& currentSuitability == NOLANECHANGENEEDED
&& (rightSuitability == NOLANECHANGENEEDED || rightSuitability == GETOFFTHISLANENOW))
{
return defaultLaneIncentives;
}
// @formatter:on
if (currentSuitability == NOLANECHANGENEEDED)
{
return new AccelerationVector(new double[] { acceleration(gtu, leftSuitability),
defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability) }, AccelerationUnit.SI,
StorageType.DENSE);
}
if (currentSuitability.le(leftSuitability))
{
return new AccelerationVector(new double[] { PREFERREDLANEINCENTIVE.getSI(), NONPREFERREDLANEINCENTIVE.getSI(),
GETOFFTHISLANENOW.getSI() }, AccelerationUnit.SI, StorageType.DENSE);
}
if (currentSuitability.le(rightSuitability))
{
return new AccelerationVector(new double[] { GETOFFTHISLANENOW.getSI(), NONPREFERREDLANEINCENTIVE.getSI(),
PREFERREDLANEINCENTIVE.getSI() }, AccelerationUnit.SI, StorageType.DENSE);
}
return new AccelerationVector(new double[] { acceleration(gtu, leftSuitability), acceleration(gtu, currentSuitability),
acceleration(gtu, rightSuitability) }, AccelerationUnit.SI, StorageType.DENSE);
}
/**
* Return the distance until the next lane drop in the specified (nearby) lane.
* @param gtu the GTU to determine the next lane drop for
* @param direction LateralDirectionality; one of the values <cite>LateralDirectionality.LEFT</cite> (use the left-adjacent
* lane), or <cite>LateralDirectionality.RIGHT</cite> (use the right-adjacent lane), or <cite>null</cite> (use
* the current lane)
* @return DoubleScalar.Rel<LengthUnit>; distance until the next lane drop if it occurs within the TIMEHORIZON, or
* LaneBasedRouteNavigator.NOLANECHANGENEEDED if this lane can be followed until the next split junction or until
* beyond the TIMEHORIZON
* @throws NetworkException on network inconsistency
* @throws GTUException when the positions of the GTU cannot be determined
*/
private Length laneDrop(final LaneBasedGTU gtu, final LateralDirectionality direction) throws NetworkException,
GTUException
{
Lane lane = null;
Length longitudinalPosition = null;
Map<Lane, Length> positions = gtu.positions(RelativePosition.REFERENCE_POSITION);
if (null == direction)
{
for (Lane l : gtu.getLanes().keySet())
{
if (l.getLaneType().isCompatible(gtu.getGTUType()))
{
lane = l;
}
}
if (null == lane)
{
throw new NetworkException(this + " is not on any compatible lane");
}
longitudinalPosition = positions.get(lane);
}
else
{
lane = positions.keySet().iterator().next();
longitudinalPosition = positions.get(lane);
lane = gtu.getPerception().bestAccessibleAdjacentLane(lane, direction, longitudinalPosition); // XXX correct??
}
if (null == lane)
{
return GETOFFTHISLANENOW;
}
double remainingLength = lane.getLength().getSI() - longitudinalPosition.getSI();
double remainingTimeSI = TIMEHORIZON.getSI() - remainingLength / lane.getSpeedLimit(gtu.getGTUType()).getSI();
while (remainingTimeSI >= 0)
{
for (Sensor s : lane.getSensors())
{
if (s instanceof SinkSensor)
{
return NOLANECHANGENEEDED;
}
}
int branching = lane.nextLanes(gtu.getGTUType()).size();
if (branching == 0)
{
return new Length(remainingLength, LengthUnit.SI);
}
if (branching > 1)
{
return NOLANECHANGENEEDED;
}
lane = lane.nextLanes(gtu.getGTUType()).keySet().iterator().next();
remainingTimeSI -= lane.getLength().getSI() / lane.getSpeedLimit(gtu.getGTUType()).getSI();
remainingLength += lane.getLength().getSI();
}
return NOLANECHANGENEEDED;
}
/**
* TODO: move suitability to LanePerception? Return the suitability for the current lane, left adjacent lane or right
* adjacent lane.
* @param gtu the GTU for which to calculate the incentives
* @param direction LateralDirectionality; one of the values <cite>null</cite>, <cite>LateralDirectionality.LEFT</cite>, or
* <cite>LateralDirectionality.RIGHT</cite>
* @return DoubleScalar.Rel<LengthUnit>; the suitability of the lane for reaching the (next) destination
* @throws NetworkException on network inconsistency
* @throws GTUException when position cannot be determined
*/
private Length suitability(final LaneBasedGTU gtu, final LateralDirectionality direction) throws NetworkException,
GTUException
{
Lane lane = null;
Length longitudinalPosition = null;
Map<Lane, Length> positions = gtu.positions(RelativePosition.REFERENCE_POSITION);
if (null == direction)
{
for (Lane l : gtu.getLanes().keySet())
{
if (l.getLaneType().isCompatible(gtu.getGTUType()))
{
lane = l;
}
}
if (null == lane)
{
throw new NetworkException(this + " is not on any compatible lane");
}
longitudinalPosition = positions.get(lane);
}
else
{
lane = positions.keySet().iterator().next();
longitudinalPosition = positions.get(lane);
// if ("1051".equals(gtu.getId()) && "Lane lane.1 of FirstVia to SecondVia".equals(lane.toString())
// && longitudinalPosition.si >= -1.85699 && longitudinalPosition.si < 0)
// {
// System.out.println(gtu + " direction " + direction + " position on " + lane + " is " + longitudinalPosition);
// }
lane = gtu.getPerception().bestAccessibleAdjacentLane(lane, direction, longitudinalPosition); // XXX correct??
// These nested if statements can be combined, but that would reduce readability of the code
if (null != lane)
{
// Cancel lane change opportunity if front or rear of the GTU is not able to make the lane change
if ((!canChangeLane(gtu.positions(gtu.getRear()), positions, gtu, direction))
|| (!canChangeLane(gtu.positions(gtu.getFront()), positions, gtu, direction)))
{
// System.out.println("Canceling " + direction + " lane change opportunity for " + gtu);
lane = null;
}
}
}
if (null == lane)
{
return GETOFFTHISLANENOW;
}
try
{
return suitability(lane, longitudinalPosition, gtu, TIMEHORIZON);
}
catch (NetworkException ne)
{
System.err.println(gtu + " has a route problem in suitability: " + ne.getMessage());
return NOLANECHANGENEEDED;
}
}
/**
* Check that front or back of GTU can (also) change lane.
* @param laneMap Map<Lane, Length>; Map of lanes that the front or back GTU is in
* @param positions Map<Lane, Length>; Map of lanes that the reference point of the GTU is in
* @param gtu LaneBasedGTU; the GTU
* @param direction LateralDirectionality; direction of the intended lane change
* @return boolean; true if the lane change appears possible; false if the lane change is not possible for the front or back
* of the GTU
*/
private boolean canChangeLane(final Map<Lane, Length> laneMap, final Map<Lane, Length> positions, final LaneBasedGTU gtu,
final LateralDirectionality direction)
{
for (Lane lane : laneMap.keySet())
{
Length positionInLane = laneMap.get(lane);
if (positionInLane.si < 0 || positionInLane.gt(lane.getLength()))
{
continue;
}
if (null == gtu.getPerception().bestAccessibleAdjacentLane(lane, direction, positions.get(lane)))
{
// System.out.println("Front or back of " + gtu + " cannot change lane");
return false;
}
}
return true;
}
/**
* Compute deceleration needed to stop at a specified distance.
* @param gtu the GTU for which to calculate the acceleration to come to a full stop at the distance
* @param stopDistance DoubleScalar.Rel<LengthUnit>; the distance
* @return double; the acceleration (deceleration) needed to stop at the specified distance in m/s/s
*/
private double acceleration(final LaneBasedGTU gtu, final Length stopDistance)
{
// What is the deceleration that will bring this GTU to a stop at exactly the suitability distance?
// Answer: a = -v^2 / 2 / suitabilityDistance
double v = gtu.getSpeed().getSI();
double a = -v * v / 2 / stopDistance.getSI();
return a;
}
/**
* Determine the suitability of being at a particular longitudinal position in a particular Lane for following this Route.
* @param lane Lane; the lane to consider
* @param longitudinalPosition DoubleScalar.Rel<LengthUnit>; the longitudinal position in the lane
* @param gtu GTU; the GTU (used to check lane compatibility of lanes, and current lane the GTU is on)
* @param timeHorizon DoubleScalar.Rel<TimeUnit>; the maximum time that a driver may want to look ahead
* @return DoubleScalar.Rel<LengthUnit>; a value that indicates within what distance the GTU should try to vacate this
* lane.
* @throws NetworkException on network inconsistency, or when the continuation Link at a branch cannot be determined
*/
private Length suitability(final Lane lane, final Length longitudinalPosition, final LaneBasedGTU gtu,
final Duration timeHorizon) throws NetworkException
{
double remainingDistance = lane.getLength().getSI() - longitudinalPosition.getSI();
double spareTime = timeHorizon.getSI() - remainingDistance / lane.getSpeedLimit(gtu.getGTUType()).getSI();
// Find the first upcoming Node where there is a branch
Node nextNode = lane.getParentLink().getEndNode();
Link lastLink = lane.getParentLink();
Node nextSplitNode = null;
Lane currentLane = lane;
CrossSectionLink linkBeforeBranch = lane.getParentLink();
while (null != nextNode)
{
if (spareTime <= 0)
{
return NOLANECHANGENEEDED; // It is not yet time to worry; this lane will do as well as any other
}
int laneCount = countCompatibleLanes(linkBeforeBranch, gtu.getGTUType());
if (0 == laneCount)
{
throw new NetworkException("No compatible Lanes on Link " + linkBeforeBranch);
}
if (1 == laneCount)
{
return NOLANECHANGENEEDED; // Only one compatible lane available; we'll get there "automatically";
// i.e. without influence from the Route
}
int branching = nextNode.getLinks().size();
if (branching > 2)
{ // Found a split
nextSplitNode = nextNode;
break;
}
else if (1 == branching)
{
return NOLANECHANGENEEDED; // dead end; no more choices to make
}
else
{ // Look beyond this nextNode
Link nextLink = gtu.getStrategicalPlanner().nextLinkDirection(nextNode, lastLink, gtu.getGTUType()).getLink();
if (nextLink instanceof CrossSectionLink)
{
nextNode = nextLink.getEndNode();
// Oops: wrong code added the length of linkBeforeBranch in stead of length of nextLink
remainingDistance += nextLink.getLength().getSI();
linkBeforeBranch = (CrossSectionLink) nextLink;
// Figure out the new currentLane
if (currentLane.nextLanes(gtu.getGTUType()).size() == 0)
{
// Lane drop; our lane disappears. This is a compulsory lane change; which is not controlled
// by the Route. Perform the forced lane change.
if (currentLane.accessibleAdjacentLanes(LateralDirectionality.RIGHT, gtu.getGTUType()).size() > 0)
{
for (Lane adjacentLane : currentLane.accessibleAdjacentLanes(LateralDirectionality.RIGHT,
gtu.getGTUType()))
{
if (adjacentLane.nextLanes(gtu.getGTUType()).size() > 0)
{
currentLane = adjacentLane;
break;
}
// If there are several adjacent lanes that have non empty nextLanes, we simple take the
// first in the set
}
}
for (Lane adjacentLane : currentLane.accessibleAdjacentLanes(LateralDirectionality.LEFT,
gtu.getGTUType()))
{
if (adjacentLane.nextLanes(gtu.getGTUType()).size() > 0)
{
currentLane = adjacentLane;
break;
}
// If there are several adjacent lanes that have non empty nextLanes, we simple take the
// first in the set
}
if (currentLane.nextLanes(gtu.getGTUType()).size() == 0)
{
throw new NetworkException("Lane ends and there is not a compatible adjacent lane that does "
+ "not end");
}
}
// Any compulsory lane change(s) have been performed and there is guaranteed a compatible next lane.
for (Lane nextLane : currentLane.nextLanes(gtu.getGTUType()).keySet())
{
if (nextLane.getLaneType().isCompatible(gtu.getGTUType()))
{
currentLane = currentLane.nextLanes(gtu.getGTUType()).keySet().iterator().next();
break;
}
}
spareTime -= currentLane.getLength().getSI() / currentLane.getSpeedLimit(gtu.getGTUType()).getSI();
}
else
{
// There is a non-CrossSectionLink on the path to the next branch. A non-CrossSectionLink does not
// have identifiable Lanes, therefore we can't aim for a particular Lane
return NOLANECHANGENEEDED; // Any Lane will do equally well
}
lastLink = nextLink;
}
}
if (null == nextNode)
{
throw new NetworkException("Cannot find the next branch or sink node");
}
// We have now found the first upcoming branching Node
// Which continuing link is the one we need?
Map<Lane, Length> suitabilityOfLanesBeforeBranch = new HashMap<Lane, Length>();
Link linkAfterBranch =
gtu.getStrategicalPlanner().nextLinkDirection(nextSplitNode, lastLink, gtu.getGTUType()).getLink();
for (CrossSectionElement cse : linkBeforeBranch.getCrossSectionElementList())
{
if (cse instanceof Lane)
{
Lane l = (Lane) cse;
if (l.getLaneType().isCompatible(gtu.getGTUType()))
{
for (Lane connectingLane : l.nextLanes(gtu.getGTUType()).keySet())
{
if (connectingLane.getParentLink() == linkAfterBranch
&& connectingLane.getLaneType().isCompatible(gtu.getGTUType()))
{
Length currentValue = suitabilityOfLanesBeforeBranch.get(l);
// Use recursion to find out HOW suitable this continuation lane is, but don't revert back
// to the maximum time horizon (or we could end up in infinite recursion when there are
// loops in the network).
Length value =
suitability(connectingLane, new Length(0, LengthUnit.SI), gtu, new Duration(spareTime,
TimeUnit.SI));
// Use the minimum of the value computed for the first split junction (if there is one)
// and the value computed for the second split junction.
suitabilityOfLanesBeforeBranch.put(l, null == currentValue || value.le(currentValue) ? value
: currentValue);
}
}
}
}
}
if (suitabilityOfLanesBeforeBranch.size() == 0)
{
throw new NetworkException("No lanes available on Link " + linkBeforeBranch);
}
Length currentLaneSuitability = suitabilityOfLanesBeforeBranch.get(currentLane);
if (null != currentLaneSuitability)
{
return currentLaneSuitability; // Following the current lane will keep us on the Route
}
// Performing one or more lane changes (left or right) is required.
int totalLanes = countCompatibleLanes(currentLane.getParentLink(), gtu.getGTUType());
Length leftSuitability =
computeSuitabilityWithLaneChanges(currentLane, remainingDistance, suitabilityOfLanesBeforeBranch, totalLanes,
LateralDirectionality.LEFT, gtu.getGTUType());
Length rightSuitability =
computeSuitabilityWithLaneChanges(currentLane, remainingDistance, suitabilityOfLanesBeforeBranch, totalLanes,
LateralDirectionality.RIGHT, gtu.getGTUType());
if (leftSuitability.ge(rightSuitability))
{
return leftSuitability;
}
else if (rightSuitability.ge(leftSuitability))
{
return rightSuitability;
}
if (leftSuitability.le(GETOFFTHISLANENOW))
{
throw new NetworkException("Changing lanes in any direction does not get the GTU on a suitable lane");
}
return leftSuitability; // left equals right; this is odd but topologically possible
}
/**
* Compute the suitability of a lane from which lane changes are required to get to the next point on the Route.<br>
* This method weighs the suitability of the nearest suitable lane by (m - n) / m where n is the number of lane changes
* required and m is the total number of lanes in the CrossSectionLink.
* @param startLane Lane; the current lane of the GTU
* @param remainingDistance double; distance in m of GTU to first branch
* @param suitabilities Map<Lane, Double>; the set of suitable lanes and their suitability
* @param totalLanes integer; total number of lanes compatible with the GTU type
* @param direction LateralDirectionality; the direction of the lane changes to attempt
* @param gtuType GTUType; the type of the GTU
* @return double; the suitability of the <cite>startLane</cite> for following the Route
*/
protected final Length computeSuitabilityWithLaneChanges(final Lane startLane, final double remainingDistance,
final Map<Lane, Length> suitabilities, final int totalLanes, final LateralDirectionality direction,
final GTUType gtuType)
{
/*-
* The time per required lane change seems more relevant than distance per required lane change.
* Total time required does not grow linearly with the number of required lane changes. Logarithmic, arc tangent
* is more like it.
* Rijkswaterstaat appears to use a fixed time for ANY number of lane changes (about 60s).
* TomTom navigation systems give more time (about 90s).
* In this method the returned suitability decreases linearly with the number of required lane changes. This
* ensures that there is a gradient that coaches the GTU towards the most suitable lane.
*/
int laneChangesUsed = 0;
Lane currentLane = startLane;
Length currentSuitability = null;
while (null == currentSuitability)
{
laneChangesUsed++;
if (currentLane.accessibleAdjacentLanes(direction, gtuType).size() == 0)
{
return GETOFFTHISLANENOW;
}
currentLane = currentLane.accessibleAdjacentLanes(direction, gtuType).iterator().next();
currentSuitability = suitabilities.get(currentLane);
}
double fraction = currentSuitability == NOLANECHANGENEEDED ? 0 : 0.5;
int notSuitableLaneCount = totalLanes - suitabilities.size();
return new Length(remainingDistance * (notSuitableLaneCount - laneChangesUsed + 1 + fraction)
/ (notSuitableLaneCount + fraction), LengthUnit.SI);
}
/**
* Determine how many lanes on a CrossSectionLink are compatible with a particular GTU type.<br>
* TODO: this method should probably be moved into the CrossSectionLink class
* @param link CrossSectionLink; the link
* @param gtuType GTUType; the GTU type
* @return integer; the number of lanes on the link that are compatible with the GTU type
*/
protected final int countCompatibleLanes(final CrossSectionLink link, final GTUType gtuType)
{
int result = 0;
for (CrossSectionElement cse : link.getCrossSectionElementList())
{
if (cse instanceof Lane)
{
Lane l = (Lane) cse;
if (l.getLaneType().isCompatible(gtuType))
{
result++;
}
}
}
return result;
}
/** {@inheritDoc} */
@Override
public final String toString()
{
return "LaneBasedCFLCTacticalPlanner [laneChangeModel=" + this.laneChangeModel + "]";
}
}