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
63
64
65
66
67
68
69
70
71 public class LaneChangeModelTest extends AbstractOTSModel implements UNITS
72 {
73
74 private static final long serialVersionUID = 20150313;
75
76
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
89
90
91
92
93
94
95
96
97
98
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
105
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
116
117
118
119
120
121
122
123
124
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
132 Lane result = new Lane(link, id, latPos, latPos, width, width, laneType, speedMap);
133 return result;
134 }
135
136
137
138
139
140
141
142
143
144
145
146
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
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
167
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
182
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
198
199
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
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
222
223
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
235
236
237
238
239
240
241
242
243
244
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
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)
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
280
281
282
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
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
303
304
305
306 }
307 }
308
309
310
311
312
313
314 @Override
315 public void constructModel() throws SimRuntimeException
316 {
317
318 }
319
320
321 @Override
322 public final OTSRoadNetwork getNetwork()
323 {
324 return this.network;
325 }
326
327
328 @Override
329 public Serializable getSourceId()
330 {
331 return "LaneChangeModelTest.Model";
332 }
333
334 }