Toledo.java

  1. package org.opentrafficsim.road.gtu.lane.tactical.toledo;

  3. import java.io.Serializable;
  4. import java.util.Iterator;
  5. import java.util.Random;

  7. import org.djunits.unit.AccelerationUnit;
  8. import org.djunits.unit.LengthUnit;
  9. import org.djunits.unit.LinearDensityUnit;
  10. import org.djunits.value.vdouble.scalar.Acceleration;
  11. import org.djunits.value.vdouble.scalar.Length;
  12. import org.djunits.value.vdouble.scalar.LinearDensity;
  13. import org.djunits.value.vdouble.scalar.Speed;
  14. import org.djunits.value.vdouble.scalar.Time;
  15. import org.opentrafficsim.base.parameters.ParameterException;
  16. import org.opentrafficsim.base.parameters.ParameterTypeLength;
  17. import org.opentrafficsim.base.parameters.ParameterTypes;
  18. import org.opentrafficsim.base.parameters.Parameters;
  19. import org.opentrafficsim.core.geometry.OTSGeometryException;
  20. import org.opentrafficsim.core.gtu.GTUException;
  21. import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
  22. import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
  23. import org.opentrafficsim.core.network.LateralDirectionality;
  24. import org.opentrafficsim.core.network.NetworkException;
  25. import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
  26. import org.opentrafficsim.road.gtu.lane.perception.CategoricalLanePerception;
  27. import org.opentrafficsim.road.gtu.lane.perception.InfrastructureLaneChangeInfo;
  28. import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
  29. import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
  30. import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
  31. import org.opentrafficsim.road.gtu.lane.perception.categories.DirectNeighborsPerception;
  32. import org.opentrafficsim.road.gtu.lane.perception.categories.HeadwayGtuType;
  33. import org.opentrafficsim.road.gtu.lane.perception.categories.NeighborsPerception;
  34. import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
  35. import org.opentrafficsim.road.gtu.lane.plan.operational.LaneChange;
  36. import org.opentrafficsim.road.gtu.lane.plan.operational.LaneOperationalPlanBuilder;
  37. import org.opentrafficsim.road.gtu.lane.tactical.AbstractLaneBasedTacticalPlanner;
  38. import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
  39. import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
  40. import org.opentrafficsim.road.network.speed.SpeedLimitProspect;

  42. import nl.tudelft.simulation.language.d3.DirectedPoint;

  44. /**
  45.  * Implementation of the model of Toledo (2003).<br>
  46.  * <br>
  47.  * Tomer Toledo (2003) "Integrated Driving Behavior Modeling", Massachusetts Institute of Technology.
  48.  * <p>
  49.  * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  50.  * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  51.  * <p>
  52.  * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jun 21, 2016 <br>
  53.  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  54.  * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  55.  * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  56.  */

  58. public class Toledo extends AbstractLaneBasedTacticalPlanner
  59. {

  61.     /** */
  62.     private static final long serialVersionUID = 20160711L;

  64.     /** Look ahead parameter type. */
  65.     protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;

  67.     /** Defines light vs heavy vehicles. */
  68.     static final Length MAX_LIGHT_VEHICLE_LENGTH = new Length(9.14, LengthUnit.METER);

  70.     /** Distance defining tail gating. */
  71.     static final Length TAILGATE_LENGTH = new Length(10, LengthUnit.METER);

  73.     /** Density for tail gating (Level Of Service (LOS) A, B or C). */
  74.     static final LinearDensity LOS_DENSITY = new LinearDensity(16, LinearDensityUnit.PER_KILOMETER);

  76.     /** Random number generator. */
  77.     public static final Random RANDOM = new Random();

  79.     /** Lane change status. */
  80.     private final LaneChange laneChange = new LaneChange();

  82.     /**
  83.      * Constructor.
  84.      * @param carFollowingModel Car-following model.
  85.      * @param gtu GTU
  86.      */
  87.     public Toledo(final CarFollowingModel carFollowingModel, final LaneBasedGTU gtu)
  88.     {
  89.         super(carFollowingModel, gtu, new CategoricalLanePerception(gtu));
  90.         getPerception().addPerceptionCategory(new ToledoPerception(getPerception()));
  91.         getPerception().addPerceptionCategory(new DirectNeighborsPerception(getPerception(), HeadwayGtuType.WRAP));
  92.     }

  94.     /** {@inheritDoc} */
  95.     @Override
  96.     public final OperationalPlan generateOperationalPlan(final Time startTime, final DirectedPoint locationAtStartTime)
  97.             throws OperationalPlanException, GTUException, NetworkException, ParameterException
  98.     {

  100.         // obtain objects to get info
  101.         LanePerception perception = getPerception();
  102.         perception.perceive();
  103.         SpeedLimitProspect slp =
  104.                 perception.getPerceptionCategory(ToledoPerception.class).getSpeedLimitProspect(RelativeLane.CURRENT);
  105.         SpeedLimitInfo sli = slp.getSpeedLimitInfo(Length.ZERO);
  106.         Parameters params = getGtu().getParameters();

  108.         Acceleration acceleration = null;

  110.         // if (this.laneChangeDirectionality == null)
  111.         LateralDirectionality initiatedLaneChange;
  112.         NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
  113.         if (!this.laneChange.isChangingLane())
  114.         {
  115.             // not changing lane

  117.             // 3rd layer of model: Target gap model
  118.             // TODO vehicle not ahead and not backwards but completely adjacent
  119.             GapInfo gapFwdL = getGapInfo(params, perception, Gap.FORWARD, RelativeLane.LEFT);
  120.             GapInfo gapAdjL = getGapInfo(params, perception, Gap.ADJACENT, RelativeLane.LEFT);
  121.             GapInfo gapBckL = getGapInfo(params, perception, Gap.BACKWARD, RelativeLane.LEFT);
  122.             GapInfo gapFwdR = getGapInfo(params, perception, Gap.FORWARD, RelativeLane.RIGHT);
  123.             GapInfo gapAdjR = getGapInfo(params, perception, Gap.ADJACENT, RelativeLane.RIGHT);
  124.             GapInfo gapBckR = getGapInfo(params, perception, Gap.BACKWARD, RelativeLane.RIGHT);
  125.             double emuTgL =
  126.                     Math.log(Math.exp(gapFwdL.getUtility()) + Math.exp(gapAdjL.getUtility()) + Math.exp(gapBckL.getUtility()));
  127.             double emuTgR =
  128.                     Math.log(Math.exp(gapFwdR.getUtility()) + Math.exp(gapAdjR.getUtility()) + Math.exp(gapBckR.getUtility()));

  130.             // 2nd layer of model: Gap-acceptance model
  131.             // gap acceptance random terms (variable over time, equal for left and right)
  132.             double eLead = RANDOM.nextGaussian() * Math.pow(params.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD), 2);
  133.             double eLag = RANDOM.nextGaussian() * Math.pow(params.getParameter(ToledoLaneChangeParameters.SIGMA_LAG), 2);
  134.             GapAcceptanceInfo gapAcceptL =
  135.                     getGapAcceptanceInfo(getGtu(), params, perception, emuTgL, eLead, eLag, RelativeLane.LEFT);
  136.             GapAcceptanceInfo gapAcceptR =
  137.                     getGapAcceptanceInfo(getGtu(), params, perception, emuTgR, eLead, eLag, RelativeLane.RIGHT);

  139.             // 1st layer of model: Target lane model
  140.             double vL = laneUtility(getGtu(), params, perception, gapAcceptL.getEmu(), sli, RelativeLane.LEFT);
  141.             double vC = laneUtility(getGtu(), params, perception, 0, sli, RelativeLane.CURRENT);
  142.             double vR = laneUtility(getGtu(), params, perception, gapAcceptR.getEmu(), sli, RelativeLane.RIGHT);

  144.             // change lane?
  145.             if (vL > vR && vL > vC && gapAcceptL.isAcceptable())
  146.             {
  147.                 initiatedLaneChange = LateralDirectionality.LEFT;
  148.             }
  149.             else if (vR > vL && vR > vC && gapAcceptR.isAcceptable())
  150.             {
  151.                 initiatedLaneChange = LateralDirectionality.RIGHT;
  152.             }
  153.             else
  154.             {
  155.                 initiatedLaneChange = LateralDirectionality.NONE;
  156.             }

  158.             // accelerate for gap selection
  159.             if (initiatedLaneChange.isNone())
  160.             {
  161.                 if ((vC > vR && vC > vL)
  162.                         || (!neighbors.getLeaders(RelativeLane.CURRENT).isEmpty() && neighbors.getLeaders(RelativeLane.CURRENT)
  163.                                 .first().getDistance().lt(getCarFollowingModel().desiredHeadway(params, getGtu().getSpeed()))))
  164.                 {
  165.                     acceleration = getCarFollowingModel().followingAcceleration(params, getGtu().getSpeed(), sli,
  166.                             neighbors.getLeaders(RelativeLane.CURRENT));
  167.                 }
  168.                 else
  169.                 {
  170.                     GapInfo gapAdj;
  171.                     GapInfo gapFwd;
  172.                     GapInfo gapBck;
  173.                     if (vL > vR && vL > vC)
  174.                     {
  175.                         gapAdj = gapAdjL;
  176.                         gapFwd = gapFwdL;
  177.                         gapBck = gapBckL;
  178.                     }
  179.                     else
  180.                     {
  181.                         gapAdj = gapAdjR;
  182.                         gapFwd = gapFwdR;
  183.                         gapBck = gapBckR;
  184.                     }
  185.                     if (gapAdj.getUtility() > gapFwd.getUtility() && gapAdj.getUtility() > gapBck.getUtility())
  186.                     {
  187.                         // adjacent gap selected
  188.                         double eadj = Toledo.RANDOM.nextGaussian() * params.getParameter(ToledoLaneChangeParameters.SIGMA_ADJ)
  189.                                 * params.getParameter(ToledoLaneChangeParameters.SIGMA_ADJ);
  190.                         acceleration =
  191.                                 new Acceleration(
  192.                                         params.getParameter(ToledoLaneChangeParameters.C_ADJ_ACC)
  193.                                                 * (params.getParameter(ToledoLaneChangeParameters.BETA_DP)
  194.                                                         * gapAdj.getLength().si - gapAdj.getDistance().si)
  195.                                                 + eadj,
  196.                                         AccelerationUnit.SI);
  197.                     }
  198.                     else if (gapFwd.getUtility() > gapAdj.getUtility() && gapFwd.getUtility() > gapBck.getUtility())
  199.                     {
  200.                         // forward gap selected
  201.                         Length desiredPosition = new Length(
  202.                                 gapFwd.getDistance().si
  203.                                         + params.getParameter(ToledoLaneChangeParameters.BETA_DP) * gapFwd.getLength().si,
  204.                                 LengthUnit.SI);
  205.                         double deltaV = gapFwd.getSpeed().si > getGtu().getSpeed().si
  206.                                 ? params.getParameter(ToledoLaneChangeParameters.LAMBDA_FWD_POS)
  207.                                         * (gapFwd.getSpeed().si - getGtu().getSpeed().si)
  208.                                 : params.getParameter(ToledoLaneChangeParameters.LAMBDA_FWD_NEG)
  209.                                         * (getGtu().getSpeed().si - gapFwd.getSpeed().si);
  210.                         double efwd = Toledo.RANDOM.nextGaussian() * params.getParameter(ToledoLaneChangeParameters.SIGMA_FWD)
  211.                                 * params.getParameter(ToledoLaneChangeParameters.SIGMA_FWD);
  212.                         acceleration = new Acceleration(params.getParameter(ToledoLaneChangeParameters.C_FWD_ACC)
  213.                                 * Math.pow(desiredPosition.si, params.getParameter(ToledoLaneChangeParameters.BETA_FWD))
  214.                                 * Math.exp(deltaV) + efwd, AccelerationUnit.SI);
  215.                     }
  216.                     else
  217.                     {
  218.                         // backward gap selected
  219.                         Length desiredPosition = new Length(
  220.                                 gapBck.getDistance().si
  221.                                         + (1 - params.getParameter(ToledoLaneChangeParameters.BETA_DP)) * gapBck.getLength().si,
  222.                                 LengthUnit.SI);
  223.                         double deltaV = gapBck.getSpeed().si > getGtu().getSpeed().si
  224.                                 ? params.getParameter(ToledoLaneChangeParameters.LAMBDA_BCK_POS)
  225.                                         * (gapBck.getSpeed().si - getGtu().getSpeed().si)
  226.                                 : params.getParameter(ToledoLaneChangeParameters.LAMBDA_BCK_NEG)
  227.                                         * (getGtu().getSpeed().si - gapBck.getSpeed().si);
  228.                         double ebck = Toledo.RANDOM.nextGaussian() * params.getParameter(ToledoLaneChangeParameters.SIGMA_BCK)
  229.                                 * params.getParameter(ToledoLaneChangeParameters.SIGMA_BCK);
  230.                         acceleration = new Acceleration(params.getParameter(ToledoLaneChangeParameters.C_BCK_ACC)
  231.                                 * Math.pow(desiredPosition.si, params.getParameter(ToledoLaneChangeParameters.BETA_BCK))
  232.                                 * Math.exp(deltaV) + ebck, AccelerationUnit.SI);
  233.                     }
  234.                 }
  235.             }
  236.         }
  237.         else
  238.         {
  239.             initiatedLaneChange = LateralDirectionality.NONE;
  240.         }

  242.         if (initiatedLaneChange.isLeft())
  243.         {
  244.             // changing left
  245.             getCarFollowingModel().followingAcceleration(params, getGtu().getSpeed(), sli,
  246.                     neighbors.getLeaders(RelativeLane.LEFT));
  247.         }
  248.         else if (initiatedLaneChange.isRight())
  249.         {
  250.             // changing right
  251.             getCarFollowingModel().followingAcceleration(params, getGtu().getSpeed(), sli,
  252.                     neighbors.getLeaders(RelativeLane.RIGHT));
  253.         }

  255.         if (acceleration == null)
  256.         {
  257.             throw new Error("Acceleration from toledo model is null.");
  258.         }

  260.         // operational plan
  261.         if (initiatedLaneChange.isNone())
  262.         {
  263.             try
  264.             {
  265.                 return LaneOperationalPlanBuilder.buildAccelerationPlan(getGtu(), startTime, getGtu().getSpeed(), acceleration,
  266.                         params.getParameter(ToledoLaneChangeParameters.DT));
  267.             }
  268.             catch (OTSGeometryException exception)
  269.             {
  270.                 throw new OperationalPlanException(exception);
  271.             }
  272.         }

  274.         try
  275.         {
  276.             OperationalPlan plan = LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(getGtu(), initiatedLaneChange,
  277.                     getGtu().getLocation(), startTime, getGtu().getSpeed(), acceleration,
  278.                     params.getParameter(ToledoLaneChangeParameters.DT), this.laneChange);
  279.             return plan;
  280.         }
  281.         catch (OTSGeometryException exception)
  282.         {
  283.             throw new OperationalPlanException(exception);
  284.         }
  285.     }

  287.     /**
  288.      * Returns info regarding a gap.
  289.      * @param params parameters
  290.      * @param perception perception
  291.      * @param gap gap
  292.      * @param lane lane
  293.      * @return utility of gap
  294.      * @throws ParameterException if parameter is not given
  295.      * @throws OperationalPlanException perception exception
  296.      */
  297.     private GapInfo getGapInfo(final Parameters params, final LanePerception perception, final Gap gap, final RelativeLane lane)
  298.             throws ParameterException, OperationalPlanException
  299.     {

  301.         // capture no leaders/follower cases for forward and backward gaps
  302.         NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
  303.         if (gap.equals(Gap.FORWARD) && (neighbors.getLeaders(lane) == null || neighbors.getLeaders(lane).isEmpty()))
  304.         {
  305.             // no leaders
  306.             return new GapInfo(Double.NEGATIVE_INFINITY, null, null, null);
  307.         }
  308.         if (gap.equals(Gap.BACKWARD) && (neighbors.getFollowers(lane) == null || neighbors.getFollowers(lane).isEmpty()))
  309.         {
  310.             // no followers
  311.             return new GapInfo(Double.NEGATIVE_INFINITY, null, null, null);
  312.         }

  314.         double constant; // utility function: constant
  315.         double alpha; // utility function: alpha parameter
  316.         Length distanceToGap; // utility function: distance to gap
  317.         int deltaFrontVehicle; // utility function: dummy whether the gap is limited by vehicle in front
  318.         Length putativeLength; // acceleration model: gap length, not affected by vehicle in front
  319.         Length putativeDistance; // acceleration model: distance to gap, different from 'distanceToGap' in utility function
  320.         Speed putativeSpeed; // acceleration model: speed of putative follower or leader
  321.         Length leaderDist; // leader of (effective) gap
  322.         Speed leaderSpeed;
  323.         Length followerDist; // follower of (effective) gap
  324.         Speed followerSpeed;

  326.         if (gap.equals(Gap.FORWARD))
  327.         {
  328.             constant = params.getParameter(ToledoLaneChangeParameters.C_FWD_TG);
  329.             alpha = 0;
  330.             PerceptionCollectable<HeadwayGTU, LaneBasedGTU> itAble = neighbors.getLeaders(lane);
  331.             HeadwayGTU second = null;
  332.             if (!itAble.isEmpty())
  333.             {
  334.                 Iterator<HeadwayGTU> it = itAble.iterator();
  335.                 it.next();
  336.                 second = it.next();
  337.             }
  338.             if (second != null)
  339.             {
  340.                 // two leaders
  341.                 Iterator<HeadwayGTU> it = neighbors.getLeaders(lane).iterator();
  342.                 it.next();
  343.                 HeadwayGTU leader = it.next();
  344.                 leaderDist = leader.getDistance();
  345.                 leaderSpeed = leader.getSpeed();
  346.                 putativeLength = leaderDist.minus(neighbors.getLeaders(lane).first().getDistance())
  347.                         .minus(neighbors.getLeaders(lane).first().getLength());
  348.             }
  349.             else
  350.             {
  351.                 // TODO infinite -> some limited space, speed also
  352.                 // one leader
  353.                 leaderDist = Length.POSITIVE_INFINITY;
  354.                 leaderSpeed = Speed.POSITIVE_INFINITY;
  355.                 putativeLength = leaderDist;
  356.             }
  357.             // distance to nose, so add length
  358.             followerDist =
  359.                     neighbors.getLeaders(lane).first().getDistance().plus(neighbors.getLeaders(lane).first().getLength());
  360.             followerSpeed = neighbors.getLeaders(lane).first().getSpeed();
  361.             distanceToGap = followerDist; // same as distance to nose of first leader
  362.             putativeDistance = distanceToGap;
  363.             putativeSpeed = followerSpeed;
  364.         }
  365.         else if (gap.equals(Gap.ADJACENT))
  366.         {
  367.             constant = 0;
  368.             alpha = params.getParameter(ToledoLaneChangeParameters.ALPHA_ADJ);
  369.             distanceToGap = Length.ZERO;
  370.             if (neighbors.getLeaders(lane) != null && !neighbors.getLeaders(lane).isEmpty())
  371.             {
  372.                 leaderDist = neighbors.getLeaders(lane).first().getDistance();
  373.                 leaderSpeed = neighbors.getLeaders(lane).first().getSpeed();
  374.             }
  375.             else
  376.             {
  377.                 leaderDist = Length.POS_MAXVALUE;
  378.                 leaderSpeed = Speed.POS_MAXVALUE;
  379.             }
  380.             if (neighbors.getFollowers(lane) != null && !neighbors.getFollowers(lane).isEmpty())
  381.             {
  382.                 // plus own vehicle length for distance from nose of own vehicle (and then whole negative)
  383.                 followerDist = neighbors.getFollowers(lane).first().getDistance().plus(getGtu().getLength());
  384.                 followerSpeed = neighbors.getFollowers(lane).first().getSpeed();
  385.                 putativeDistance = followerDist;
  386.             }
  387.             else
  388.             {
  389.                 followerDist = Length.NEG_MAXVALUE;
  390.                 followerSpeed = Speed.NEG_MAXVALUE;
  391.                 putativeDistance = Length.POS_MAXVALUE;
  392.             }
  393.             putativeSpeed = null;
  394.             putativeLength = leaderDist.plus(followerDist).plus(getGtu().getLength());
  395.         }
  396.         else
  397.         {
  398.             constant = params.getParameter(ToledoLaneChangeParameters.C_BCK_TG);
  399.             alpha = params.getParameter(ToledoLaneChangeParameters.ALPHA_BCK);
  400.             deltaFrontVehicle = 0;
  401.             PerceptionCollectable<HeadwayGTU, LaneBasedGTU> itAble = neighbors.getFollowers(lane);
  402.             HeadwayGTU second = null;
  403.             if (!itAble.isEmpty())
  404.             {
  405.                 Iterator<HeadwayGTU> it = itAble.iterator();
  406.                 it.next();
  407.                 second = it.next();
  408.             }
  409.             if (second != null)
  410.             {
  411.                 // two followers
  412.                 Iterator<HeadwayGTU> it = neighbors.getFollowers(lane).iterator();
  413.                 it.next();
  414.                 HeadwayGTU follower = it.next();
  415.                 followerDist = follower.getDistance();
  416.                 followerSpeed = follower.getSpeed();
  417.                 putativeLength = followerDist.minus(neighbors.getFollowers(lane).first().getDistance())
  418.                         .minus(neighbors.getFollowers(lane).first().getLength());
  419.             }
  420.             else
  421.             {
  422.                 // one follower
  423.                 followerDist = Length.NEGATIVE_INFINITY;
  424.                 followerSpeed = Speed.NEGATIVE_INFINITY;
  425.                 putativeLength = followerDist;
  426.             }
  427.             // add vehicle length to get distance to tail of 1st follower (and then whole negative)
  428.             leaderDist =
  429.                     neighbors.getFollowers(lane).first().getDistance().plus(neighbors.getLeaders(lane).first().getLength());
  430.             leaderSpeed = neighbors.getFollowers(lane).first().getSpeed();
  431.             distanceToGap = neighbors.getFollowers(lane).first().getDistance().plus(getGtu().getLength()); // own nose to nose
  432.             putativeDistance = distanceToGap.plus(neighbors.getFollowers(lane).first().getLength());
  433.             putativeSpeed = leaderSpeed;
  434.         }

  436.         // limit by leader in current lane
  437.         if (!gap.equals(Gap.BACKWARD) && !neighbors.getLeaders(RelativeLane.CURRENT).isEmpty()
  438.                 && neighbors.getLeaders(RelativeLane.CURRENT).first().getDistance().lt(leaderDist))
  439.         {
  440.             leaderDist = neighbors.getLeaders(RelativeLane.CURRENT).first().getDistance();
  441.             leaderSpeed = neighbors.getLeaders(RelativeLane.CURRENT).first().getSpeed();
  442.             deltaFrontVehicle = 1;
  443.         }
  444.         else
  445.         {
  446.             deltaFrontVehicle = 0;
  447.         }

  449.         // calculate relative gap speed and effective gap
  450.         Speed dV = followerSpeed.minus(leaderSpeed);
  451.         Length effectiveGap = leaderDist.minus(followerDist);
  452.         // {@formatter:off}
  453.         // calculate utility 
  454.         double util = constant 
  455.                 + params.getParameter(ToledoLaneChangeParameters.BETA_DTG) * distanceToGap.si
  456.                 + params.getParameter(ToledoLaneChangeParameters.BETA_EG) * effectiveGap.si
  457.                 + params.getParameter(ToledoLaneChangeParameters.BETA_FV) * deltaFrontVehicle
  458.                 + params.getParameter(ToledoLaneChangeParameters.BETA_RGS) * dV.si
  459.                 + alpha * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
  460.         // {@formatter:on}
  461.         return new GapInfo(util, putativeLength, putativeDistance, putativeSpeed);

  463.     }

  465.     /**
  466.      * Returns info regarding gap-acceptance.
  467.      * @param gtu GTU
  468.      * @param params parameters
  469.      * @param perception perception
  470.      * @param emuTg emu from target gap model
  471.      * @param eLead lead error
  472.      * @param eLag lag error
  473.      * @param lane lane to evaluate
  474.      * @return info regarding gap-acceptance
  475.      * @throws ParameterException if parameter is not defined
  476.      * @throws OperationalPlanException perception exception
  477.      */
  478.     private GapAcceptanceInfo getGapAcceptanceInfo(final LaneBasedGTU gtu, final Parameters params,
  479.             final LanePerception perception, final double emuTg, final double eLead, final double eLag, final RelativeLane lane)
  480.             throws ParameterException, OperationalPlanException
  481.     {

  483.         // get lead and lag utility and acceptance
  484.         double vLead;
  485.         double vLag;
  486.         boolean acceptLead;
  487.         boolean acceptLag;

  489.         NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
  490.         if (neighbors.getLeaders(lane) != null && !neighbors.getLeaders(lane).isEmpty())
  491.         {
  492.             Speed dVLead = neighbors.getLeaders(lane).first().getSpeed().minus(gtu.getSpeed());
  493.             Length sLead = neighbors.getLeaders(lane).first().getDistance();
  494.             // critical gap
  495.             Length gLead =
  496.                     new Length(
  497.                             Math.exp(params.getParameter(ToledoLaneChangeParameters.C_LEAD)
  498.                                     + params.getParameter(ToledoLaneChangeParameters.BETA_POS_LEAD)
  499.                                             * Speed.max(dVLead, Speed.ZERO).si
  500.                                     + params.getParameter(ToledoLaneChangeParameters.BETA_NEG_LEAD)
  501.                                             * Speed.min(dVLead, Speed.ZERO).si
  502.                                     + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_LEAD) * emuTg
  503.                                     + params.getParameter(ToledoLaneChangeParameters.ALPHA_TL_LEAD)
  504.                                             * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM)
  505.                                     + eLead),
  506.                             LengthUnit.SI);
  507.             vLead = Math.log(gLead.si) - (params.getParameter(ToledoLaneChangeParameters.C_LEAD)
  508.                     + params.getParameter(ToledoLaneChangeParameters.BETA_POS_LEAD) * Speed.max(dVLead, Speed.ZERO).si
  509.                     + params.getParameter(ToledoLaneChangeParameters.BETA_NEG_LEAD) * Speed.min(dVLead, Speed.ZERO).si
  510.                     + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_LEAD) * emuTg);
  511.             acceptLead = gLead.lt(sLead);
  512.         }
  513.         else
  514.         {
  515.             vLead = 0;
  516.             acceptLead = false;
  517.         }

  519.         if (neighbors.getFollowers(lane) != null && !neighbors.getFollowers(lane).isEmpty())
  520.         {
  521.             Speed dVLag = neighbors.getFollowers(lane).first().getSpeed().minus(gtu.getSpeed());
  522.             Length sLag = neighbors.getFollowers(lane).first().getDistance();
  523.             // critical gap
  524.             Length gLag =
  525.                     new Length(
  526.                             Math.exp(params.getParameter(ToledoLaneChangeParameters.C_LAG)
  527.                                     + params.getParameter(ToledoLaneChangeParameters.BETA_POS_LAG)
  528.                                             * Speed.max(dVLag, Speed.ZERO).si
  529.                                     + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_LAG) * emuTg
  530.                                     + params.getParameter(ToledoLaneChangeParameters.ALPHA_TL_LAG)
  531.                                             * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM)
  532.                                     + eLag),
  533.                             LengthUnit.SI);
  534.             vLag = Math.log(gLag.si) - (params.getParameter(ToledoLaneChangeParameters.C_LAG)
  535.                     + params.getParameter(ToledoLaneChangeParameters.BETA_POS_LAG) * Speed.max(dVLag, Speed.ZERO).si
  536.                     + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_LAG) * emuTg);
  537.             acceptLag = gLag.lt(sLag);
  538.         }
  539.         else
  540.         {
  541.             vLag = 0;
  542.             acceptLag = false;
  543.         }

  545.         // gap acceptance emu
  546.         double vRatLead = vLead / params.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD);
  547.         double vRatLag = vLag / params.getParameter(ToledoLaneChangeParameters.SIGMA_LAG);
  548.         double emuGa = vLead * cumNormDist(vRatLead)
  549.                 + params.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD) * normDist(vRatLead) + vLag * cumNormDist(vRatLag)
  550.                 + params.getParameter(ToledoLaneChangeParameters.SIGMA_LAG) * normDist(vRatLag);

  552.         // return info
  553.         return new GapAcceptanceInfo(emuGa, acceptLead && acceptLag);

  555.     }

  557.     /**
  558.      * Returns the utility of a lane.
  559.      * @param gtu GTU
  560.      * @param params parameters
  561.      * @param perception perception
  562.      * @param emuGa emu from gap acceptance model
  563.      * @param sli speed limit info
  564.      * @param lane lane to evaluate
  565.      * @return utility of lane
  566.      * @throws ParameterException if parameter is not defined
  567.      * @throws OperationalPlanException perception exception
  568.      */
  569.     private double laneUtility(final LaneBasedGTU gtu, final Parameters params, final LanePerception perception,
  570.             final double emuGa, final SpeedLimitInfo sli, final RelativeLane lane)
  571.             throws ParameterException, OperationalPlanException
  572.     {

  574.         ToledoPerception toledo = perception.getPerceptionCategory(ToledoPerception.class);
  575.         if (!perception.getLaneStructure().getExtendedCrossSection().contains(lane))
  576.         {
  577.             return 0.0;
  578.         }

  580.         // get infrastructure info
  581.         boolean takeNextOffRamp = false;
  582.         for (InfrastructureLaneChangeInfoToledo info : toledo.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT))
  583.         {
  584.             if (info.getSplitNumber() == 1)
  585.             {
  586.                 takeNextOffRamp = true;
  587.             }
  588.         }
  589.         int deltaNextExit = takeNextOffRamp ? 1 : 0;

  591.         Length dExit;
  592.         if (!toledo.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT).isEmpty())
  593.         {
  594.             dExit = toledo.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT).first().getRemainingDistance();
  595.         }
  596.         else
  597.         {
  598.             dExit = Length.POSITIVE_INFINITY;
  599.         }

  601.         int[] delta = new int[3];
  602.         int deltaAdd = 0;
  603.         if (!toledo.getInfrastructureLaneChangeInfo(lane).isEmpty())
  604.         {
  605.             InfrastructureLaneChangeInfo ilciLef = toledo.getInfrastructureLaneChangeInfo(lane).first();
  606.             if (ilciLef.getRequiredNumberOfLaneChanges() > 1 && ilciLef.getRequiredNumberOfLaneChanges() < 5)
  607.             {
  608.                 deltaAdd = ilciLef.getRequiredNumberOfLaneChanges() - 2;
  609.                 delta[deltaAdd] = 1;
  610.             }
  611.         }

  613.         // heavy neighbor
  614.         NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
  615.         Length leaderLength = !lane.isCurrent() && !neighbors.getFirstLeaders(lane.getLateralDirectionality()).isEmpty()
  616.                 ? neighbors.getFirstLeaders(lane.getLateralDirectionality()).first().getLength() : Length.ZERO;
  617.         Length followerLength = !lane.isCurrent() && !neighbors.getFirstFollowers(lane.getLateralDirectionality()).isEmpty()
  618.                 ? neighbors.getFirstFollowers(lane.getLateralDirectionality()).first().getDistance() : Length.ZERO;
  619.         int deltaHeavy = leaderLength.gt(MAX_LIGHT_VEHICLE_LENGTH) || followerLength.gt(MAX_LIGHT_VEHICLE_LENGTH) ? 1 : 0;

  621.         // get density
  622.         LinearDensity d = getDensityInLane(gtu, perception, lane);

  624.         // tail gating
  625.         int deltaTailgate = 0;
  626.         if (lane.equals(RelativeLane.CURRENT) && !neighbors.getFollowers(RelativeLane.CURRENT).isEmpty()
  627.                 && neighbors.getFollowers(RelativeLane.CURRENT).first().getDistance().le(TAILGATE_LENGTH))
  628.         {
  629.             LinearDensity dL = getDensityInLane(gtu, perception, RelativeLane.LEFT);
  630.             LinearDensity dR = getDensityInLane(gtu, perception, RelativeLane.RIGHT);
  631.             LinearDensity dRoad = new LinearDensity((dL.si + d.si + dR.si) / 3, LinearDensityUnit.SI);
  632.             if (dRoad.le(LOS_DENSITY))
  633.             {
  634.                 deltaTailgate = 1;
  635.             }
  636.         }

  638.         // right most
  639.         // TODO definition of 'right most lane' does not account for ramps and weaving sections
  640.         int deltaRightMost = 0;
  641.         if (lane.equals(RelativeLane.CURRENT) && !toledo.getCrossSection().contains(RelativeLane.RIGHT))
  642.         {
  643.             deltaRightMost = 1;
  644.         }
  645.         else if (lane.equals(RelativeLane.RIGHT) && toledo.getCrossSection().contains(RelativeLane.RIGHT)
  646.                 && !toledo.getCrossSection().contains(RelativeLane.SECOND_RIGHT))
  647.         {
  648.             deltaRightMost = 1;
  649.         }

  651.         // current lane traffic
  652.         Speed vFront;
  653.         Length sFront;
  654.         if (lane.equals(RelativeLane.CURRENT))
  655.         {
  656.             if (!neighbors.getLeaders(RelativeLane.CURRENT).isEmpty())
  657.             {
  658.                 vFront = neighbors.getLeaders(RelativeLane.CURRENT).first().getSpeed();
  659.                 sFront = neighbors.getLeaders(RelativeLane.CURRENT).first().getDistance();
  660.             }
  661.             else
  662.             {
  663.                 vFront = getCarFollowingModel().desiredSpeed(params, sli);
  664.                 sFront = getCarFollowingModel().desiredHeadway(params, gtu.getSpeed());
  665.             }
  666.         }
  667.         else
  668.         {
  669.             vFront = Speed.ZERO;
  670.             sFront = Length.ZERO;
  671.         }

  673.         // target lane utility
  674.         double constant = 0;
  675.         double error = 0;
  676.         if (lane.equals(RelativeLane.CURRENT))
  677.         {
  678.             constant = params.getParameter(ToledoLaneChangeParameters.C_CL);
  679.             error = params.getParameter(ToledoLaneChangeParameters.ALPHA_CL)
  680.                     * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
  681.         }
  682.         else if (lane.equals(RelativeLane.RIGHT))
  683.         {
  684.             constant = params.getParameter(ToledoLaneChangeParameters.C_RL);
  685.             error = params.getParameter(ToledoLaneChangeParameters.ALPHA_RL)
  686.                     * params.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
  687.         }
  688.         // {@formatter:off}
  689.         return constant
  690.                 + params.getParameter(ToledoLaneChangeParameters.BETA_RIGHT_MOST) * deltaRightMost // 0 for LEFT
  691.                 + params.getParameter(ToledoLaneChangeParameters.BETA_VFRONT) * vFront.si // 0 for LEFT/RIGHT
  692.                 + params.getParameter(ToledoLaneChangeParameters.BETA_SFRONT) * sFront.si // 0 for LEFT/RIGHT
  693.                 + params.getParameter(ToledoLaneChangeParameters.BETA_DENSITY) * d.getInUnit(LinearDensityUnit.PER_KILOMETER)
  694.                 + params.getParameter(ToledoLaneChangeParameters.BETA_HEAVY_NEIGHBOUR) * deltaHeavy
  695.                 + params.getParameter(ToledoLaneChangeParameters.BETA_TAILGATE) * deltaTailgate
  696.                 + Math.pow(dExit.getInUnit(LengthUnit.KILOMETER), params.getParameter(ToledoLaneChangeParameters.THETA_MLC)) 
  697.                 * (params.getParameter(ToledoLaneChangeParameters.BETA1) * delta[0] 
  698.                         + params.getParameter(ToledoLaneChangeParameters.BETA2) * delta[1] 
  699.                         + params.getParameter(ToledoLaneChangeParameters.BETA3) * delta[2])
  700.                 + params.getParameter(ToledoLaneChangeParameters.BETA_NEXT_EXIT) * deltaNextExit
  701.                 + params.getParameter(ToledoLaneChangeParameters.BETA_ADD) * deltaAdd
  702.                 + params.getParameter(ToledoLaneChangeParameters.BETA_EMU_GA) * emuGa // 0 for CURRENT (given correct input)
  703.                 + error; // 0 for LEFT
  704.         // {@formatter:on}
  705.     }

  707.     /**
  708.      * Returns the density in the given lane based on the following and leading vehicles.
  709.      * @param gtu subject GTU
  710.      * @param perception perception
  711.      * @param lane lane to get density of
  712.      * @return density in the given lane based on the following and leading vehicles
  713.      * @throws OperationalPlanException perception exception
  714.      */
  715.     private LinearDensity getDensityInLane(final LaneBasedGTU gtu, final LanePerception perception, final RelativeLane lane)
  716.             throws OperationalPlanException
  717.     {
  718.         int nVehicles = 0;
  719.         NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
  720.         Length up = Length.ZERO;
  721.         Length down = Length.ZERO;
  722.         for (HeadwayGTU neighbor : neighbors.getFollowers(lane))
  723.         {
  724.             nVehicles++;
  725.             down = neighbor.getDistance();
  726.         }
  727.         for (HeadwayGTU neighbor : neighbors.getLeaders(lane))
  728.         {
  729.             nVehicles++;
  730.             up = neighbor.getDistance();
  731.         }
  732.         if (nVehicles > 0)
  733.         {
  734.             return new LinearDensity(nVehicles / up.plus(down).getInUnit(LengthUnit.KILOMETER),
  735.                     LinearDensityUnit.PER_KILOMETER);
  736.         }
  737.         return LinearDensity.ZERO;
  738.     }

  740.     /**
  741.      * Returns the cumulative density function (CDF) at given value of the standard normal distribution.
  742.      * @param x value
  743.      * @return cumulative density function (CDF) at given value of the standard normal distribution
  744.      */
  745.     private static double cumNormDist(final double x)
  746.     {
  747.         return .5 * (1 + erf(x / Math.sqrt(2)));
  748.     }

  750.     /**
  751.      * Error function approximation using Horner's method.
  752.      * @param x value
  753.      * @return error function approximation
  754.      */
  755.     private static double erf(final double x)
  756.     {
  757.         double t = 1.0 / (1.0 + 0.5 * Math.abs(x));
  758.         // use Horner's method
  759.         // {@formatter:off}
  760.         double tau = t * Math.exp(-x * x - 1.26551223 + t * (1.00002368 + t * (0.37409196 
  761.                 + t * (0.09678418 + t * (0.18628806 + t * (0.27886807 + t * (-1.13520398 
  762.                         + t * (1.48851587 + t * (-0.82215223 + t * (0.17087277))))))))));
  763.         // {@formatter:on}
  764.         return x >= 0 ? 1 - tau : tau - 1;
  765.     }

  767.     /**
  768.      * Returns the probability density function (PDF) at given value of the standard normal distribution.
  769.      * @param x value
  770.      * @return probability density function (PDF) at given value of the standard normal distribution
  771.      */
  772.     private static double normDist(final double x)
  773.     {
  774.         return Math.exp(-x * x / 2) / Math.sqrt(2 * Math.PI);
  775.     }

  777.     /**
  778.      * Gap indicator in adjacent lane.
  779.      * <p>
  780.      * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
  781.      * <br>
  782.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  783.      * <p>
  784.      * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jul 11, 2016 <br>
  785.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  786.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  787.      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  788.      */
  789.     private enum Gap
  790.     {
  791.         /** Gap in front of leader in adjacent lane. */
  792.         FORWARD,

  794.         /** Gap between follower and leader in adjacent lane. */
  795.         ADJACENT,

  797.         /** Gap behind follower in adjacent lane. */
  798.         BACKWARD;
  799.     }

  801.     /**
  802.      * Contains info regarding an adjacent gap.
  803.      * <p>
  804.      * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
  805.      * <br>
  806.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  807.      * <p>
  808.      * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jul 11, 2016 <br>
  809.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  810.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  811.      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  812.      */
  813.     private class GapInfo implements Serializable
  814.     {

  816.         /** */
  817.         private static final long serialVersionUID = 20160811L;

  819.         /** Utility of the gap. */
  820.         private final double utility;

  822.         /** Length of the gap. */
  823.         private final Length length;

  825.         /** Distance towards the gap. */
  826.         private final Length distance;

  828.         /** Speed of the vehicle in front or behind the gap, always the closest. */
  829.         private final Speed speed;

  831.         /**
  832.          * @param utility utility of gap
  833.          * @param length length of the gap
  834.          * @param distance distance towards the gap
  835.          * @param speed speed of the vehicle in front or behind the gap, always the closest
  836.          */
  837.         GapInfo(final double utility, final Length length, final Length distance, final Speed speed)
  838.         {
  839.             this.utility = utility;
  840.             this.length = length;
  841.             this.distance = distance;
  842.             this.speed = speed;
  843.         }

  845.         /**
  846.          * @return utility
  847.          */
  848.         public final double getUtility()
  849.         {
  850.             return this.utility;
  851.         }

  853.         /**
  854.          * @return length
  855.          */
  856.         public final Length getLength()
  857.         {
  858.             return this.length;
  859.         }

  861.         /**
  862.          * @return distance
  863.          */
  864.         public final Length getDistance()
  865.         {
  866.             return this.distance;
  867.         }

  869.         /**
  870.          * @return speed
  871.          */
  872.         public final Speed getSpeed()
  873.         {
  874.             return this.speed;
  875.         }

  877.         /** {@inheritDoc} */
  878.         @Override
  879.         public String toString()
  880.         {
  881.             return "GapAcceptanceInfo [utility = " + this.utility + ", length = " + this.length + ", distance = "
  882.                     + this.distance + ", speed = " + this.speed + "]";
  883.         }

  885.     }

  887.     /**
  888.      * Contains info regarding gap-acceptance.
  889.      * <p>
  890.      * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
  891.      * <br>
  892.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  893.      * <p>
  894.      * @version $Revision$, $LastChangedDate$, by $Author$, initial version Jul 11, 2016 <br>
  895.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  896.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  897.      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
  898.      */
  899.     private class GapAcceptanceInfo implements Serializable
  900.     {

  902.         /** */
  903.         private static final long serialVersionUID = 20160811L;

  905.         /** Emu value of gap-acceptance. */
  906.         private final double emu;

  908.         /** Whether gap is acceptable. */
  909.         private final boolean acceptable;

  911.         /**
  912.          * @param emu emu
  913.          * @param acceptable whether gap is acceptable
  914.          */
  915.         GapAcceptanceInfo(final double emu, final boolean acceptable)
  916.         {
  917.             this.emu = emu;
  918.             this.acceptable = acceptable;
  919.         }

  921.         /**
  922.          * @return emu
  923.          */
  924.         public final double getEmu()
  925.         {
  926.             return this.emu;
  927.         }

  929.         /**
  930.          * @return acceptable
  931.          */
  932.         public final boolean isAcceptable()
  933.         {
  934.             return this.acceptable;
  935.         }

  937.         /** {@inheritDoc} */
  938.         @Override
  939.         public String toString()
  940.         {
  941.             return "GapAcceptanceInfo [emu = " + this.emu + ", acceptable = " + this.acceptable + "]";
  942.         }

  944.     }

  946.     /** {@inheritDoc} */
  947.     @Override
  948.     public final String toString()
  949.     {
  950.         return "Toledo tactical planner.";
  951.     }

  953. }
Created with JaCoCo 0.8.2.201808211720