IncentiveSpeedWithCourtesy.java

package org.opentrafficsim.road.gtu.lane.tactical.lmrs;

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.Speed;
import org.djutils.exceptions.Try;
import org.opentrafficsim.base.parameters.ParameterException;
import org.opentrafficsim.base.parameters.ParameterTypeAcceleration;
import org.opentrafficsim.base.parameters.ParameterTypeSpeed;
import org.opentrafficsim.base.parameters.ParameterTypes;
import org.opentrafficsim.base.parameters.Parameters;
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.InfrastructurePerception;
import org.opentrafficsim.road.gtu.lane.perception.categories.TrafficPerception;
import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
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;

/**
 * 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-2019 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
{

    /** Acceleration parameter type. */
    protected static final ParameterTypeAcceleration A = ParameterTypes.A;

    /** Anticipation speed difference parameter type. */
    protected static final ParameterTypeSpeed VGAIN = LmrsParameters.VGAIN;

    /** Speed threshold below which traffic is considered congested. */
    protected static final ParameterTypeSpeed VCONG = ParameterTypes.VCONG;

    /** {@inheritDoc} */
    @Override
    public final Desire determineDesire(final Parameters parameters, final LanePerception perception,
            final CarFollowingModel carFollowingModel, final Desire mandatoryDesire, final Desire voluntaryDesire)
            throws ParameterException, OperationalPlanException
    {

        // zero if no lane change is possible
        InfrastructurePerception infra = perception.getPerceptionCategory(InfrastructurePerception.class);
        TrafficPerception traffic = perception.getPerceptionCategory(TrafficPerception.class);
        EgoPerception ego = perception.getPerceptionCategory(EgoPerception.class);
        double leftDist = infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, LateralDirectionality.LEFT).si;
        double rightDist = infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, LateralDirectionality.RIGHT).si;

        // gather some info
        Speed vCur = traffic.getSpeed(RelativeLane.CURRENT);
        Speed vGain = parameters.getParameter(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 reasons here. E.g. traffic lights, conflicts, etc.)
         */
        Acceleration aCur = ego.getAcceleration();

        /*
         * The idea to let aCur simply be the current acceleration is wrong; aCur -should- only describe the car-following
         * relation, as this describes a sense of sticking to a lane as the leader is getting away.
         */
        aCur = Try.assign(() -> perception.getGtu().getCarFollowingAcceleration(), "Could not obtain the GTU.");
        if (aCur.si > 0)
        {
            Acceleration a = parameters.getParameter(A);
            aGain = a.minus(aCur).divideBy(a);
        }
        else
        {
            aGain = new Dimensionless(1, DimensionlessUnit.SI);
        }

        // left desire
        double dLeft;
        if (leftDist > 0.0 && infra.getCrossSection().contains(RelativeLane.LEFT))
        {
            Speed vLeft = traffic.getSpeed(RelativeLane.LEFT);
            dLeft = aGain.si * (vLeft.si - vCur.si) / vGain.si;
        }
        else
        {
            dLeft = 0.0;
        }

        // right desire
        double dRight;
        if (rightDist > 0.0 && infra.getCrossSection().contains(RelativeLane.RIGHT))
        {
            Speed vRight = traffic.getSpeed(RelativeLane.RIGHT);
            if (vCur.si >= parameters.getParameter(VCONG).si)
            {
                dRight = aGain.si * Math.min(vRight.si - vCur.si, 0) / vGain.si;
            }
            else
            {
                dRight = aGain.si * (vRight.si - vCur.si) / vGain.si;
            }
        }
        else
        {
            dRight = 0.0;
        }

        // return desire
        return new Desire(dLeft, dRight);
    }

    /** {@inheritDoc} */
    @Override
    public final String toString()
    {
        return "IncentiveSpeedWithCourtesy";
    }

}