IncentiveSpeedWithCourtesy.java
package org.opentrafficsim.road.gtu.lane.tactical.lmrs;
import java.util.SortedSet;
import org.djunits.unit.DimensionlessUnit;
import org.djunits.value.vdouble.scalar.Acceleration;
import org.djunits.value.vdouble.scalar.Dimensionless;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Speed;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypes;
import org.opentrafficsim.core.gtu.perception.EgoPerception;
import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
import org.opentrafficsim.core.network.LateralDirectionality;
import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
import org.opentrafficsim.road.gtu.lane.perception.categories.DirectIntersectionPerception;
import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
import org.opentrafficsim.road.gtu.lane.perception.categories.IntersectionPerception;
import org.opentrafficsim.road.gtu.lane.perception.categories.NeighborsPerception;
import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTUSimple;
import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
import org.opentrafficsim.road.gtu.lane.tactical.util.ConflictUtil;
import org.opentrafficsim.road.gtu.lane.tactical.util.ConflictUtil.ConflictPlans;
import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Desire;
import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.LmrsParameters;
import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.VoluntaryIncentive;
import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
/**
* Determines lane change desire for speed. The anticipation speed in the current and adjacent lanes are compared. The larger
* the difference, the larger the lane change desire. For negative differences, negative desire results. Anticipation speed
* involves the the most critical vehicle considered to be in a lane. Vehicles are more critical if their speed is lower, and if
* they are closer. The set of vehicles considered to be on a lane includes drivers on adjacent lanes of the considered lane,
* with a lane change desire towards the considered lane above a certain certain threshold. If such vehicles have low speeds
* (i.e. vehicle accelerating to merge), this may result in a courtesy lane change, or in not changing lane out of courtesy from
* the 2nd lane of the mainline. Vehicle on the current lane of the driver, are not considered on adjacent lanes. This would
* maintain a large speed difference between the lanes where all drivers do not change lane as they consider leading vehicles to
* be on the adjacent lane, lowering the anticipation speed on the adjacent lane. The desire for speed is reduced as
* acceleration is larger, preventing over-assertive lane changes as acceleration out of congestion in the adjacent lane has
* progressed more.<br>
* <br>
* <b>Note:</b> This incentive includes speed, and a form of courtesy. It should therefore not be combined with incentives
* solely for speed, or solely for courtesy.
* <p>
* Copyright (c) 2013-2017 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/current/license.html">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version Apr 13, 2016 <br>
* @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
*/
public class IncentiveSpeedWithCourtesy implements VoluntaryIncentive
{
/** {@inheritDoc} */
@Override
public final Desire determineDesire(final BehavioralCharacteristics behavioralCharacteristics,
final LanePerception perception, final CarFollowingModel carFollowingModel, final Desire mandatoryDesire,
final Desire voluntaryDesire) throws ParameterException, OperationalPlanException
{
// zero if no lane change is possible
double leftDist = perception.getPerceptionCategory(InfrastructurePerception.class)
.getLegalLaneChangePossibility(RelativeLane.CURRENT, LateralDirectionality.LEFT).si;
double rightDist = perception.getPerceptionCategory(InfrastructurePerception.class)
.getLegalLaneChangePossibility(RelativeLane.CURRENT, LateralDirectionality.RIGHT).si;
// gather some info
Speed vCur = anticipationSpeed(RelativeLane.CURRENT, behavioralCharacteristics, perception, carFollowingModel);
Speed vGain = behavioralCharacteristics.getParameter(LmrsParameters.VGAIN);
// calculate aGain (default 1; lower as acceleration is higher than 0)
Dimensionless aGain;
/*
* Instead of instantaneous car-following acceleration, use current acceleration; only then is the acceleration factor
* consistent with possible lane change incentives pertaining to speed (which used to be only vehicles in the original
* LMRS, but can be any number of reason here. E.g. traffic lights, conflicts, etc.)
*/
Acceleration aCur = perception.getPerceptionCategory(EgoPerception.class).getAcceleration();
if (aCur.si > 0)
{
Acceleration a = behavioralCharacteristics.getParameter(ParameterTypes.A);
aGain = a.minus(aCur).divideBy(a);
}
else
{
aGain = new Dimensionless(1, DimensionlessUnit.SI);
}
// left desire
Dimensionless dLeft;
if (leftDist > 0.0 && perception.getPerceptionCategory(InfrastructurePerception.class).getCrossSection()
.contains(RelativeLane.LEFT))
{
Speed vLeft = anticipationSpeed(RelativeLane.LEFT, behavioralCharacteristics, perception, carFollowingModel);
dLeft = aGain.multiplyBy(vLeft.minus(vCur)).divideBy(vGain);
}
else
{
dLeft = Dimensionless.ZERO;
}
// right desire
Dimensionless dRight;
if (rightDist > 0.0 && perception.getPerceptionCategory(InfrastructurePerception.class).getCrossSection()
.contains(RelativeLane.RIGHT))
{
Speed vRight = anticipationSpeed(RelativeLane.RIGHT, behavioralCharacteristics, perception, carFollowingModel);
dRight = aGain.multiplyBy(vRight.minus(vCur)).divideBy(vGain);
}
else
{
dRight = Dimensionless.ZERO;
}
// return desire
return new Desire(dLeft, dRight);
}
/**
* Determine the anticipation speed on the given lane. This depends on leading vehicles, leading vehicles in adjacent lanes
* with their indicator to this lane, and conflicts.
* @param lane lane to anticipate the speed on
* @param bc behavioral characteristics
* @param perception perception
* @param cfm car-following model, used for the desired speed
* @return anticipation speed on lane
* @throws ParameterException if a parameter is not defined
* @throws OperationalPlanException perception exception
*/
private Speed anticipationSpeed(final RelativeLane lane, final BehavioralCharacteristics bc,
final LanePerception perception, final CarFollowingModel cfm) throws ParameterException, OperationalPlanException
{
SpeedLimitInfo sli = perception.getPerceptionCategory(InfrastructurePerception.class).getSpeedLimitProspect(lane)
.getSpeedLimitInfo(Length.ZERO);
Speed anticipationSpeed = cfm.desiredSpeed(bc, sli);
Speed desiredSpeed = new Speed(anticipationSpeed);
Length x0 = bc.getParameter(ParameterTypes.LOOKAHEAD);
// leaders with right indicators on left lane of considered lane
if (perception.getPerceptionCategory(InfrastructurePerception.class).getCrossSection().contains(lane.getLeft()))
{
for (HeadwayGTU headwayGTU : perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane.getLeft()))
{
// leaders on the current lane with indicator to an adjacent lane are not considered
if (headwayGTU.isRightTurnIndicatorOn() && !lane.getLeft().equals(RelativeLane.CURRENT))
{
anticipationSpeed = anticipateSingle(anticipationSpeed, desiredSpeed, x0, headwayGTU);
}
}
}
// leaders with left indicators on right lane of considered lane
if (perception.getPerceptionCategory(InfrastructurePerception.class).getCrossSection().contains(lane.getRight()))
{
for (HeadwayGTU headwayGTU : perception.getPerceptionCategory(NeighborsPerception.class)
.getLeaders(lane.getRight()))
{
// leaders on the current lane with indicator to an adjacent lane are not considered
if (headwayGTU.isLeftTurnIndicatorOn() && !lane.getRight().equals(RelativeLane.CURRENT))
{
anticipationSpeed = anticipateSingle(anticipationSpeed, desiredSpeed, x0, headwayGTU);
}
}
}
// leaders in the considered lane
for (HeadwayGTU headwayGTU : perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane))
{
anticipationSpeed = anticipateSingle(anticipationSpeed, desiredSpeed, x0, headwayGTU);
}
// conflicts in the considered lane
if (perception.contains(DirectIntersectionPerception.class))
{
for (HeadwayConflict headwayConflict : perception.getPerceptionCategory(IntersectionPerception.class)
.getConflicts(lane))
{
Length vehicleLength = perception.getPerceptionCategory(EgoPerception.class).getLength();
Speed speed = perception.getPerceptionCategory(EgoPerception.class).getSpeed();
SortedSet<HeadwayGTU> leaders = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane);
if (!headwayConflict.getConflictType().isCrossing())
{
// consider first downstream vehicle on split or merge (ignore others)
SortedSet<HeadwayGTU> conflictVehicles = headwayConflict.getDownstreamConflictingGTUs();
if (!conflictVehicles.isEmpty() && conflictVehicles.first().isParallel())
{
HeadwayGTU conflictingGtu = conflictVehicles.first();
Length distance = headwayConflict.getDistance().plus(headwayConflict.getLength())
.plus(conflictingGtu.getOverlapRear());
HeadwayGTU leadingGtu;
try
{
leadingGtu = new HeadwayGTUSimple(conflictingGtu.getId(), conflictingGtu.getGtuType(), distance,
conflictingGtu.getLength());
}
catch (GTUException exception)
{
throw new OperationalPlanException("Could not create HeadwayGTUSimple for GTU on split.",
exception);
}
anticipationSpeed = anticipateSingle(anticipationSpeed, desiredSpeed, x0, leadingGtu);
}
}
switch (headwayConflict.getConflictPriority())
{
case SPLIT:
{
// nothing
break;
}
case PRIORITY:
{
if (ConflictUtil.stopForPriorityConflict(headwayConflict, leaders, speed, vehicleLength, bc,
new ConflictPlans()))
{
anticipationSpeed = anticipateSingle(anticipationSpeed, desiredSpeed, x0, headwayConflict);
}
break;
}
case GIVE_WAY:
{
Acceleration acceleration = perception.getPerceptionCategory(EgoPerception.class).getAcceleration();
if (ConflictUtil.stopForGiveWayConflict(headwayConflict, leaders, speed, acceleration, vehicleLength,
bc, sli, cfm))
{
anticipationSpeed = anticipateSingle(anticipationSpeed, desiredSpeed, x0, headwayConflict);
}
break;
}
case STOP:
{
Acceleration acceleration = perception.getPerceptionCategory(EgoPerception.class).getAcceleration();
if (ConflictUtil.stopForStopConflict(headwayConflict, leaders, speed, acceleration, vehicleLength, bc,
sli, cfm))
{
anticipationSpeed = anticipateSingle(anticipationSpeed, desiredSpeed, x0, headwayConflict);
}
break;
}
case ALL_STOP:
{
if (ConflictUtil.stopForAllStopConflict(headwayConflict, new ConflictPlans()))
{
anticipationSpeed = anticipateSingle(anticipationSpeed, desiredSpeed, x0, headwayConflict);
}
break;
}
default:
throw new RuntimeException(
"Conflict priority " + headwayConflict.getConflictPriority() + " is unknown.");
}
}
}
return anticipationSpeed;
}
/**
* Anticipate a single leader by possibly lowering the anticipation speed.
* @param anticipationSpeed anticipation speed
* @param desiredSpeed desired speed on anticipated lane
* @param x0 look-ahead distance
* @param headway leader/object to anticipate
* @return possibly lowered anticipation speed
*/
private Speed anticipateSingle(final Speed anticipationSpeed, final Speed desiredSpeed, final Length x0,
final Headway headway)
{
Speed speed = headway.getSpeed() == null ? Speed.ZERO : headway.getSpeed();
if (speed.gt(anticipationSpeed) || headway.getDistance().gt(x0))
{
return anticipationSpeed;
}
Speed vSingle = Speed.interpolate(speed, desiredSpeed, headway.getDistance().si / x0.si);
return anticipationSpeed.lt(vSingle) ? anticipationSpeed : vSingle;
}
/** {@inheritDoc} */
@Override
public final String toString()
{
return "IncentiveSpeedWithCourtesy";
}
}