View Javadoc
1   package org.opentrafficsim.road.gtu.lane.changing;
2   
3   import static org.junit.Assert.assertEquals;
4   
5   import java.io.Serializable;
6   import java.util.Collection;
7   import java.util.LinkedHashMap;
8   import java.util.LinkedHashSet;
9   import java.util.Map;
10  import java.util.Set;
11  
12  import javax.naming.NamingException;
13  
14  import org.djunits.unit.DurationUnit;
15  import org.djunits.unit.LengthUnit;
16  import org.djunits.unit.util.UNITS;
17  import org.djunits.value.vdouble.scalar.Acceleration;
18  import org.djunits.value.vdouble.scalar.Direction;
19  import org.djunits.value.vdouble.scalar.Duration;
20  import org.djunits.value.vdouble.scalar.Length;
21  import org.djunits.value.vdouble.scalar.Speed;
22  import org.djunits.value.vdouble.scalar.Time;
23  import org.junit.Test;
24  import org.opentrafficsim.base.parameters.ParameterSet;
25  import org.opentrafficsim.base.parameters.ParameterTypes;
26  import org.opentrafficsim.core.dsol.AbstractOTSModel;
27  import org.opentrafficsim.core.dsol.OTSSimulator;
28  import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
29  import org.opentrafficsim.core.geometry.OTSGeometryException;
30  import org.opentrafficsim.core.geometry.OTSLine3D;
31  import org.opentrafficsim.core.geometry.OTSPoint3D;
32  import org.opentrafficsim.core.gtu.GTUDirectionality;
33  import org.opentrafficsim.core.gtu.GTUType;
34  import org.opentrafficsim.core.gtu.RelativePosition;
35  import org.opentrafficsim.core.network.LateralDirectionality;
36  import org.opentrafficsim.core.network.LinkType;
37  import org.opentrafficsim.core.network.NetworkException;
38  import org.opentrafficsim.road.DefaultTestParameters;
39  import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
40  import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
41  import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTUSimple;
42  import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
43  import org.opentrafficsim.road.gtu.lane.tactical.following.AbstractIDM;
44  import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
45  import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.AbstractLaneChangeModel;
46  import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Altruistic;
47  import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Egoistic;
48  import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneMovementStep;
49  import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
50  import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
51  import org.opentrafficsim.road.network.OTSRoadNetwork;
52  import org.opentrafficsim.road.network.lane.CrossSectionLink;
53  import org.opentrafficsim.road.network.lane.DirectedLanePosition;
54  import org.opentrafficsim.road.network.lane.Lane;
55  import org.opentrafficsim.road.network.lane.LaneType;
56  import org.opentrafficsim.road.network.lane.OTSRoadNode;
57  import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
58  
59  import nl.tudelft.simulation.dsol.SimRuntimeException;
60  
61  /**
62   * Test some very basic properties of lane change models.
63   * <p>
64   * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
65   * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
66   * <p>
67   * $LastChangedDate: 2015-09-16 19:20:07 +0200 (Wed, 16 Sep 2015) $, @version $Revision: 1405 $, by $Author: averbraeck $,
68   * initial version 14 nov. 2014 <br>
69   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
70   */
71  public class LaneChangeModelTest extends AbstractOTSModel implements UNITS
72  {
73      /** */
74      private static final long serialVersionUID = 20150313;
75  
76      /** The network. */
77      private OTSRoadNetwork network;
78  
79      /**
80       */
81      public LaneChangeModelTest()
82      {
83          super(new OTSSimulator("LaneChangeModelTest"));
84          this.network  = new OTSRoadNetwork("lane change model test network", true, getSimulator());
85      }
86  
87      /**
88       * Create a Link.
89       * @param network OTSRoadNetwork; the network
90       * @param name String; name of the new Link
91       * @param from OTSRoadNode; start node of the new Link
92       * @param to OTSRoadNode; end node of the new Link
93       * @param width Length; the width of the new Link
94       * @param simulator OTSSimulatorInterface; the simulator
95       * @return Link
96       * @throws OTSGeometryException when coordinates cannot be calculated
97       * @throws NetworkException if link already exists in the network, if name of the link is not unique, or if the start node
98       *             or the end node of the link are not registered in the network
99       */
100     private static CrossSectionLink makeLink(final OTSRoadNetwork network, final String name, final OTSRoadNode from,
101             final OTSRoadNode to, final Length width, final OTSSimulatorInterface simulator)
102             throws OTSGeometryException, NetworkException
103     {
104         // TODO create a LinkAnimation if the simulator is compatible with that.
105         // FIXME The current LinkAnimation is too bad to use...
106         OTSPoint3D[] coordinates = new OTSPoint3D[] { new OTSPoint3D(from.getPoint().x, from.getPoint().y, 0),
107                 new OTSPoint3D(to.getPoint().x, to.getPoint().y, 0) };
108         OTSLine3D line = new OTSLine3D(coordinates);
109         CrossSectionLink link = new CrossSectionLink(network, name, from, to, network.getLinkType(LinkType.DEFAULTS.ROAD), line,
110                 LaneKeepingPolicy.KEEPRIGHT);
111         return link;
112     }
113 
114     /**
115      * Create one Lane.
116      * @param link Link; the link that owns the new Lane
117      * @param id String; the id of the lane, has to be unique within the link
118      * @param laneType LaneType&lt;String&gt;; the type of the new Lane
119      * @param latPos Length; the lateral position of the new Lane with respect to the design line of the link
120      * @param width Length; the width of the new Lane
121      * @return Lane
122      * @throws NamingException on ???
123      * @throws NetworkException on ??
124      * @throws OTSGeometryException when center line or contour of a link or lane cannot be generated
125      */
126     private static Lane makeLane(final CrossSectionLink link, final String id, final LaneType laneType, final Length latPos,
127             final Length width) throws NamingException, NetworkException, OTSGeometryException
128     {
129         Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
130         speedMap.put(link.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(100, KM_PER_HOUR));
131         // XXX Decide what type of overtaking conditions we want in this test
132         Lane result = new Lane(link, id, latPos, latPos, width, width, laneType, speedMap);
133         return result;
134     }
135 
136     /**
137      * Create a simple straight road with the specified number of Lanes.
138      * @param network OTSRoadNetwork; the network
139      * @param name String; name of the Link
140      * @param from OTSRoadNode; starting node of the new Lane
141      * @param to OTSRoadNode; ending node of the new Lane
142      * @param laneType LaneType&lt;String&gt;; the type of GTU that can use the lanes
143      * @param laneCount int; number of lanes in the road
144      * @param simulator OTSSimulatorInterface; the simulator
145      * @return Lane&lt;String, String&gt;[]; array containing the new Lanes
146      * @throws Exception when something goes wrong (should not happen)
147      */
148     public static Lane[] makeMultiLane(final OTSRoadNetwork network, final String name, final OTSRoadNode from,
149             final OTSRoadNode to, final LaneType laneType, final int laneCount, final OTSSimulatorInterface simulator)
150             throws Exception
151     {
152         Length width = new Length(laneCount * 4.0, METER);
153         final CrossSectionLink link = makeLink(network, name, from, to, width, simulator);
154         Lane[] result = new Lane[laneCount];
155         width = new Length(4.0, METER);
156         for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
157         {
158             // successive lanes have a more negative offset => more to the RIGHT
159             Length latPos = new Length((-0.5 - laneIndex) * width.getSI(), METER);
160             result[laneIndex] = makeLane(link, "lane." + laneIndex, laneType, latPos, width);
161         }
162         return result;
163     }
164 
165     /**
166      * Test that a vehicle in the left lane changes to the right lane if that is empty, or there is enough room.
167      * @throws Exception when something goes wrong (should not happen)
168      */
169     @Test
170     public final void changeRight() throws Exception
171     {
172         GTUType gtuType = this.network.getGtuType(GTUType.DEFAULTS.CAR);
173         LaneType laneType = this.network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
174         int laneCount = 2;
175         this.simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), this);
176         Lane[] lanes = makeMultiLane(this.network, "Road with two lanes",
177                 new OTSRoadNode(this.network, "From", new OTSPoint3D(0, 0, 0), Direction.ZERO),
178                 new OTSRoadNode(this.network, "To", new OTSPoint3D(200, 0, 0), Direction.ZERO), laneType, laneCount,
179                 this.simulator);
180 
181         // Let's see if adjacent lanes are accessible
182         // lanes: | 0 : 1 : 2 | in case of three lanes
183         assertEquals("Leftmost lane should not have accessible adjacent lanes on the LEFT side", 0,
184                 lanes[0].accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtuType, GTUDirectionality.DIR_PLUS).size());
185         assertEquals("Leftmost lane should have one accessible adjacent lane on the RIGHT side", 1,
186                 lanes[0].accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtuType, GTUDirectionality.DIR_PLUS).size());
187         assertEquals("Rightmost lane should have one accessible adjacent lane on the LEFT side", 1,
188                 lanes[1].accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtuType, GTUDirectionality.DIR_PLUS).size());
189         assertEquals("Rightmost lane should not have accessible adjacent lanes on the RIGHT side", 0,
190                 lanes[1].accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtuType, GTUDirectionality.DIR_PLUS).size());
191 
192         Set<DirectedLanePosition> initialLongitudinalPositions = new LinkedHashSet<>(1);
193         initialLongitudinalPositions
194                 .add(new DirectedLanePosition(lanes[1], new Length(100, METER), GTUDirectionality.DIR_PLUS));
195         AbstractLaneChangeModel laneChangeModel = new Egoistic();
196         ParameterSet parameters = DefaultTestParameters.create();
197         // LaneBasedBehavioralCharacteristics drivingCharacteristics =
198         // new LaneBasedBehavioralCharacteristics(new IDMPlusOld(new Acceleration(1, METER_PER_SECOND_2), new Acceleration(
199         // 1.5, METER_PER_SECOND_2), new Length(2, METER), new Duration(1, SECOND), 1d), laneChangeModel);
200         LaneBasedIndividualGTU car = new LaneBasedIndividualGTU("ReferenceCar", gtuType, new Length(4, METER),
201                 new Length(2, METER), new Speed(150, KM_PER_HOUR), Length.instantiateSI(2.0), this.simulator, this.network);
202         LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
203                 new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), laneChangeModel, car), car);
204         car.setParameters(parameters);
205         car.init(strategicalPlanner, initialLongitudinalPositions, new Speed(100, KM_PER_HOUR));
206         car.getTacticalPlanner().getPerception().perceive();
207         Collection<Headway> sameLaneGTUs = new LinkedHashSet<>();
208         sameLaneGTUs.add(new HeadwayGTUSimple(car.getId(), car.getGTUType(), Length.ZERO, Length.ZERO, car.getLength(),
209                 car.getSpeed(), car.getAcceleration(), null));
210         Collection<Headway> preferredLaneGTUs = new LinkedHashSet<>();
211         Collection<Headway> nonPreferredLaneGTUs = new LinkedHashSet<>();
212         LaneMovementStep laneChangeModelResult = laneChangeModel.computeLaneChangeAndAcceleration(car, sameLaneGTUs,
213                 preferredLaneGTUs, nonPreferredLaneGTUs, new Speed(100, KM_PER_HOUR), new Acceleration(0.3, METER_PER_SECOND_2),
214                 new Acceleration(0.1, METER_PER_SECOND_2), new Acceleration(-0.3, METER_PER_SECOND_2));
215         // System.out.println(laneChangeModelResult.toString());
216         assertEquals("Vehicle want to change to the right lane", LateralDirectionality.RIGHT,
217                 laneChangeModelResult.getLaneChangeDirection());
218         Length rear = car.position(lanes[0], car.getRear());
219         Length front = car.position(lanes[0], car.getFront());
220         Length reference = car.position(lanes[0], RelativePosition.REFERENCE_POSITION);
221         // System.out.println("rear: " + rear);
222         // System.out.println("front: " + front);
223         // System.out.println("reference: " + reference);
224         Length vehicleLength = front.minus(rear);
225         Length collisionStart = reference.minus(vehicleLength);
226         Length collisionEnd = reference.plus(vehicleLength);
227         for (double pos = collisionStart.getSI() + 0.01; pos < collisionEnd.getSI() - 0.01; pos += 0.1)
228         {
229             Set<DirectedLanePosition> otherLongitudinalPositions = new LinkedHashSet<>(1);
230             otherLongitudinalPositions
231                     .add(new DirectedLanePosition(lanes[1], new Length(pos, METER), GTUDirectionality.DIR_PLUS));
232 
233             parameters = DefaultTestParameters.create();
234             // parameters = new BehavioralCharacteristics();
235             // parameters.setParameter(ParameterTypes.A, new Acceleration(1, METER_PER_SECOND_2));
236             // parameters.setParameter(ParameterTypes.B, new Acceleration(1.5, METER_PER_SECOND_2));
237             // parameters.setParameter(ParameterTypes.S0, new Length(2, METER));
238             // parameters.setParameter(ParameterTypes.T, new Duration(1, SECOND));
239             // parameters.setParameter(ParameterTypes.A, new Acceleration(1, METER_PER_SECOND_2));
240             // parameters.setParameter(AbstractIDM.DELTA, 1d);
241             // drivingCharacteristics =
242             // new LaneBasedBehavioralCharacteristics(new IDMPlusOld(new Acceleration(1, METER_PER_SECOND_2),
243             // new Acceleration(1.5, METER_PER_SECOND_2), new Length(2, METER), new Duration(1, SECOND), 1d),
244             // laneChangeModel);
245             LaneBasedIndividualGTU collisionCar =
246                     new LaneBasedIndividualGTU("LaneChangeBlockingCarAt" + pos, gtuType, vehicleLength, new Length(2, METER),
247                             new Speed(150, KM_PER_HOUR), vehicleLength.times(0.5), this.simulator, this.network);
248             strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
249                     new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), laneChangeModel, collisionCar), collisionCar);
250             collisionCar.setParameters(parameters);
251             collisionCar.init(strategicalPlanner, otherLongitudinalPositions, new Speed(100, KM_PER_HOUR));
252             preferredLaneGTUs.clear();
253             HeadwayGTUSimple collisionHWGTU = new HeadwayGTUSimple(collisionCar.getId(), collisionCar.getGTUType(),
254                     new Length(pos - reference.getSI(), LengthUnit.SI), collisionCar.getLength(), collisionCar.getWidth(),
255                     collisionCar.getSpeed(), collisionCar.getAcceleration(), null);
256             preferredLaneGTUs.add(collisionHWGTU);
257             laneChangeModelResult = new Egoistic().computeLaneChangeAndAcceleration(car, sameLaneGTUs, preferredLaneGTUs,
258                     nonPreferredLaneGTUs, new Speed(100, KM_PER_HOUR), new Acceleration(0.3, METER_PER_SECOND_2),
259                     new Acceleration(0.1, METER_PER_SECOND_2), new Acceleration(-0.3, METER_PER_SECOND_2));
260             // System.out.println(laneChangeModelResult.toString());
261             assertEquals("Vehicle cannot to change to the right lane because that would result in an immediate collision", null,
262                     laneChangeModelResult.getLaneChangeDirection());
263         }
264         for (double pos = 0; pos < 180; pos += 5) // beyond 180m, a GTU gets a plan beyond the 200m long network
265         {
266             Set<DirectedLanePosition> otherLongitudinalPositions = new LinkedHashSet<>(1);
267             otherLongitudinalPositions
268                     .add(new DirectedLanePosition(lanes[1], new Length(pos, METER), GTUDirectionality.DIR_PLUS));
269 
270             parameters = new ParameterSet();
271             parameters.setParameter(ParameterTypes.A, new Acceleration(1, METER_PER_SECOND_2));
272             parameters.setParameter(ParameterTypes.B, new Acceleration(1.5, METER_PER_SECOND_2));
273             parameters.setParameter(ParameterTypes.S0, new Length(2, METER));
274             parameters.setParameter(ParameterTypes.T, new Duration(1, SECOND));
275             parameters.setParameter(ParameterTypes.A, new Acceleration(1, METER_PER_SECOND_2));
276             parameters.setDefaultParameter(ParameterTypes.LOOKAHEAD);
277             parameters.setDefaultParameter(ParameterTypes.LOOKBACKOLD);
278             parameters.setParameter(AbstractIDM.DELTA, 1d);
279             // drivingCharacteristics =
280             // new LaneBasedBehavioralCharacteristics(new IDMPlusOld(new Acceleration(1, METER_PER_SECOND_2),
281             // new Acceleration(1.5, METER_PER_SECOND_2), new Length(2, METER), new Duration(1, SECOND), 1d),
282             // laneChangeModel);
283             LaneBasedIndividualGTU otherCar =
284                     new LaneBasedIndividualGTU("OtherCarAt" + pos, gtuType, vehicleLength, new Length(2, METER),
285                             new Speed(150, KM_PER_HOUR), vehicleLength.times(0.5), this.simulator, this.network);
286             strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
287                     new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), laneChangeModel, otherCar), otherCar);
288             otherCar.setParameters(parameters);
289             otherCar.init(strategicalPlanner, otherLongitudinalPositions, new Speed(100, KM_PER_HOUR));
290             preferredLaneGTUs.clear();
291             HeadwayGTUSimple collisionHWGTU = new HeadwayGTUSimple(otherCar.getId(), otherCar.getGTUType(),
292                     new Length(pos - car.position(lanes[0], car.getReference()).getSI(), LengthUnit.SI), otherCar.getLength(),
293                     otherCar.getWidth(), otherCar.getSpeed(), otherCar.getAcceleration(), null);
294             preferredLaneGTUs.add(collisionHWGTU);
295             laneChangeModelResult = new Egoistic().computeLaneChangeAndAcceleration(car, sameLaneGTUs, preferredLaneGTUs,
296                     nonPreferredLaneGTUs, new Speed(100, KM_PER_HOUR), new Acceleration(0.3, METER_PER_SECOND_2),
297                     new Acceleration(0.1, METER_PER_SECOND_2), new Acceleration(-0.3, METER_PER_SECOND_2));
298             // System.out.println(String.format("pos=%5fm Egoistic: %s", pos, laneChangeModelResult.toString()));
299             laneChangeModelResult = new Altruistic().computeLaneChangeAndAcceleration(car, sameLaneGTUs, preferredLaneGTUs,
300                     nonPreferredLaneGTUs, new Speed(100, KM_PER_HOUR), new Acceleration(0.3, METER_PER_SECOND_2),
301                     new Acceleration(0.1, METER_PER_SECOND_2), new Acceleration(-0.3, METER_PER_SECOND_2));
302             // System.out.println(String.format("pos=%5fm Altruistic: %s", pos, laneChangeModelResult.toString()));
303             // assertEquals(
304             // "Vehicle cannot to change to the right lane because that would result in an immediate collision",
305             // null, laneChangeModelResult.getLaneChange());
306         }
307     }
308 
309     // TODO test/prove the expected differences between Egoistic and Altruistic
310     // TODO prove that the most restrictive car in the other lane determines what happens
311     // TODO test merge into overtaking lane
312 
313     /** {@inheritDoc} */
314     @Override
315     public void constructModel() throws SimRuntimeException
316     {
317         // DO NOTHING
318     }
319 
320     /** {@inheritDoc} */
321     @Override
322     public final OTSRoadNetwork getNetwork()
323     {
324         return this.network;
325     }
326 
327     /** {@inheritDoc} */
328     @Override
329     public Serializable getSourceId()
330     {
331         return "LaneChangeModelTest.Model";
332     }
333 
334 }