1 package org.opentrafficsim.road.gtu;
2
3 import static org.junit.Assert.assertEquals;
4 import static org.junit.Assert.assertTrue;
5 import static org.junit.Assert.fail;
6 import static org.opentrafficsim.core.gtu.GTUType.CAR;
7
8 import java.util.ArrayList;
9 import java.util.LinkedHashSet;
10 import java.util.List;
11 import java.util.Map;
12 import java.util.Set;
13
14 import org.djunits.unit.LengthUnit;
15 import org.djunits.unit.TimeUnit;
16 import org.djunits.unit.UNITS;
17 import org.djunits.value.vdouble.scalar.Acceleration;
18 import org.djunits.value.vdouble.scalar.Duration;
19 import org.djunits.value.vdouble.scalar.Length;
20 import org.djunits.value.vdouble.scalar.Speed;
21 import org.djunits.value.vdouble.scalar.Time;
22 import org.junit.Test;
23 import org.opentrafficsim.base.parameters.Parameters;
24 import org.opentrafficsim.core.dsol.OTSModelInterface;
25 import org.opentrafficsim.core.geometry.OTSPoint3D;
26 import org.opentrafficsim.core.gtu.GTUDirectionality;
27 import org.opentrafficsim.core.gtu.GTUException;
28 import org.opentrafficsim.core.gtu.GTUType;
29 import org.opentrafficsim.core.gtu.RelativePosition;
30 import org.opentrafficsim.core.network.Node;
31 import org.opentrafficsim.core.network.OTSNetwork;
32 import org.opentrafficsim.core.network.OTSNode;
33 import org.opentrafficsim.core.network.route.CompleteRoute;
34 import org.opentrafficsim.road.DefaultTestParameters;
35 import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
36 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
37 import org.opentrafficsim.road.gtu.lane.tactical.following.FixedAccelerationModel;
38 import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
39 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.FixedLaneChangeModel;
40 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
41 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
42 import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
43 import org.opentrafficsim.road.network.factory.LaneFactory;
44 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
45 import org.opentrafficsim.road.network.lane.Lane;
46 import org.opentrafficsim.road.network.lane.LaneType;
47 import org.opentrafficsim.simulationengine.SimpleSimulator;
48 import org.opentrafficsim.simulationengine.SimpleSimulatorInterface;
49
50 import nl.tudelft.simulation.dsol.SimRuntimeException;
51 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
52 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
53
54
55
56
57
58
59
60
61
62
63
64
65
66 public class AbstractLaneBasedGTUTest implements UNITS
67 {
68
69 private OTSNetwork network = new OTSNetwork("lane base gtu test network");
70
71
72
73
74
75 @Test
76 public final void abstractLaneBasedGTUTest() throws Exception
77 {
78
79
80
81 OTSNode nodeAFrom = new OTSNode(this.network, "AFrom", new OTSPoint3D(0, 0, 0));
82 OTSNode nodeATo = new OTSNode(this.network, "ATo", new OTSPoint3D(1000, 0, 0));
83 GTUType gtuType = CAR;
84 LaneType laneType = LaneType.TWO_WAY_LANE;
85
86 OTSModelInterface model = new DummyModel();
87 final SimpleSimulatorInterface simulator =
88 new SimpleSimulator(Time.ZERO, Duration.ZERO, new Duration(3600.0, SECOND), model);
89
90 Lane[] lanesGroupA = LaneFactory.makeMultiLane(this.network, "A", nodeAFrom, nodeATo, null, 3, laneType,
91 new Speed(100, KM_PER_HOUR), simulator);
92
93 OTSNode nodeBFrom = new OTSNode(this.network, "BFrom", new OTSPoint3D(10, 0, 0));
94 OTSNode nodeBTo = new OTSNode(this.network, "BTo", new OTSPoint3D(1000, 0, 0));
95 Lane[] lanesGroupB = LaneFactory.makeMultiLane(this.network, "B", nodeBFrom, nodeBTo, null, 3, laneType,
96 new Speed(100, KM_PER_HOUR), simulator);
97 Set<DirectedLanePosition> initialLongitudinalPositions = new LinkedHashSet<>(2);
98
99 Length positionA = new Length(100, METER);
100 initialLongitudinalPositions.add(new DirectedLanePosition(lanesGroupA[1], positionA, GTUDirectionality.DIR_PLUS));
101 Length positionB = new Length(90, METER);
102 initialLongitudinalPositions.add(new DirectedLanePosition(lanesGroupB[1], positionB, GTUDirectionality.DIR_PLUS));
103
104 Acceleration acceleration = new Acceleration(2, METER_PER_SECOND_2);
105 Duration validFor = new Duration(10, SECOND);
106 GTUFollowingModelOld gfm = new FixedAccelerationModel(acceleration, validFor);
107
108
109 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
110
111 Speed initialSpeed = new Speed(50, KM_PER_HOUR);
112
113 Length carLength = new Length(4, METER);
114
115 Length carWidth = new Length(1.8, METER);
116
117 Speed maximumSpeed = new Speed(200, KM_PER_HOUR);
118
119 String carID = "theCar";
120
121 List<Node> nodeList = new ArrayList<Node>();
122 nodeList.add(nodeAFrom);
123 nodeList.add(nodeATo);
124
125 CompleteRoute route = new CompleteRoute("Route", gtuType, nodeList);
126
127 Parameters parameters = DefaultTestParameters.create();
128
129
130
131 LaneBasedIndividualGTU car = new LaneBasedIndividualGTU(carID, gtuType, carLength, carWidth, maximumSpeed,
132 carLength.multiplyBy(0.5), simulator, this.network);
133 LaneBasedStrategicalPlanner strategicalPlanner =
134 new LaneBasedStrategicalRoutePlanner(new LaneBasedCFLCTacticalPlanner(gfm, laneChangeModel, car), car);
135 car.setParameters(parameters);
136 car.init(strategicalPlanner, initialLongitudinalPositions, initialSpeed);
137
138 assertEquals("ID of the car should be identical to the provided one", carID, car.getId());
139
140
141
142 assertEquals("Width should be identical to the provided width", carWidth, car.getWidth());
143 assertEquals("Length should be identical to the provided length", carLength, car.getLength());
144 assertEquals("GTU type should be identical to the provided one", gtuType, car.getGTUType());
145 assertEquals("front in lanesGroupA[1] is positionA", positionA.getSI(),
146 car.position(lanesGroupA[1], car.getReference()).getSI(), 0.0001);
147 assertEquals("front in lanesGroupB[1] is positionB", positionB.getSI(),
148 car.position(lanesGroupB[1], car.getReference()).getSI(), 0.0001);
149
150
151 assertEquals("acceleration is 2", 2.0, car.getAcceleration().getSI(), 0.00001);
152 assertEquals("longitudinal speed is " + initialSpeed, initialSpeed.getSI(), car.getSpeed().getSI(), 0.00001);
153 assertEquals("lastEvaluation time is 0", 0, car.getOperationalPlan().getStartTime().getSI(), 0.00001);
154
155
156
157
158
159
160
161
162
163
164
165 for (Lane[] laneGroup : new Lane[][] { lanesGroupA, lanesGroupB })
166 {
167 for (int laneIndex = 0; laneIndex < laneGroup.length; laneIndex++)
168 {
169 Lane lane = laneGroup[laneIndex];
170 boolean expectException = 1 != laneIndex;
171 for (RelativePosition relativePosition : new RelativePosition[] { car.getFront(), car.getReference(),
172 car.getRear() })
173 {
174
175
176 try
177 {
178 Length position = car.position(lane, relativePosition);
179 if (expectException)
180 {
181
182 fail("Calling position on lane that the car is NOT on should have thrown a NetworkException");
183 }
184 else
185 {
186 Length expectedPosition = laneGroup == lanesGroupA ? positionA : positionB;
187 expectedPosition = expectedPosition.plus(relativePosition.getDx());
188
189
190 assertEquals("Position should match initial position", expectedPosition.getSI(), position.getSI(),
191 0.0001);
192 }
193 }
194 catch (GTUException ne)
195 {
196 if (!expectException)
197 {
198 System.out.println(ne);
199 fail("Calling position on lane that the car is on should NOT have thrown a NetworkException");
200 }
201 }
202 }
203 }
204 }
205
206
207 assertEquals("lastEvaluation time is 0", 0, car.getOperationalPlan().getStartTime().getSI(), 0.00001);
208
209
210 assertEquals("nextEvaluation time is 10", 10.0, car.getOperationalPlan().getEndTime().getSI(), 0.00001);
211
212 double step = 0.01d;
213 for (int i = 0;; i++)
214 {
215 Time stepTime = new Time(i * step, TimeUnit.BASE_SECOND);
216 if (stepTime.getSI() > validFor.getSI())
217 {
218 break;
219 }
220 if (stepTime.getSI() > 0.5)
221 {
222 step = 0.1;
223 }
224
225 simulator.runUpTo(stepTime);
226 while (simulator.isRunning())
227 {
228 try
229 {
230 Thread.sleep(1);
231 }
232 catch (InterruptedException ie)
233 {
234 ie = null;
235 }
236 }
237
238 if (stepTime.getSI() > 0)
239 {
240 assertEquals("nextEvaluation time is " + validFor, validFor.getSI(),
241 car.getOperationalPlan().getEndTime().getSI(), 0.0001);
242 assertEquals("acceleration is " + acceleration, acceleration.getSI(), car.getAcceleration().getSI(), 0.00001);
243 }
244 Speed longitudinalSpeed = car.getSpeed();
245 double expectedLongitudinalSpeed = initialSpeed.getSI() + stepTime.getSI() * acceleration.getSI();
246 assertEquals("longitudinal speed is " + expectedLongitudinalSpeed, expectedLongitudinalSpeed,
247 longitudinalSpeed.getSI(), 0.00001);
248 for (RelativePosition relativePosition : new RelativePosition[] { car.getFront(), car.getRear() })
249 {
250 Map<Lane, Double> positions = car.fractionalPositions(relativePosition);
251 assertEquals("Car should be in two lanes", 2, positions.size());
252 Double pos = positions.get(lanesGroupA[1]);
253
254 assertTrue("Car should be in lane 1 of lane group A", null != pos);
255 assertEquals("fractional position should be equal to result of fractionalPosition(lane, ...)", pos,
256 car.fractionalPosition(lanesGroupA[1], relativePosition), 0.0000001);
257 pos = positions.get(lanesGroupB[1]);
258 assertTrue("Car should be in lane 1 of lane group B", null != pos);
259 assertEquals("fractional position should be equal to result of fractionalPosition(lane, ...)", pos,
260 car.fractionalPosition(lanesGroupB[1], relativePosition), 0.0000001);
261 }
262 for (Lane[] laneGroup : new Lane[][] { lanesGroupA, lanesGroupB })
263 {
264 for (int laneIndex = 0; laneIndex < laneGroup.length; laneIndex++)
265 {
266 Lane lane = laneGroup[laneIndex];
267 boolean expectException = 1 != laneIndex;
268 for (RelativePosition relativePosition : new RelativePosition[] { car.getFront(), car.getReference(),
269 car.getRear() })
270 {
271
272
273 try
274 {
275 Length position = car.position(lane, relativePosition);
276 if (expectException)
277 {
278
279 fail("Calling position on lane that the car is NOT on should have thrown a "
280 + "NetworkException");
281 }
282 else
283 {
284 Length expectedPosition = laneGroup == lanesGroupA ? positionA : positionB;
285 expectedPosition = expectedPosition
286 .plus(new Length(stepTime.getSI() * initialSpeed.getSI(), LengthUnit.SI));
287 expectedPosition = expectedPosition.plus(new Length(
288 0.5 * acceleration.getSI() * stepTime.getSI() * stepTime.getSI(), LengthUnit.SI));
289 expectedPosition = expectedPosition.plus(relativePosition.getDx());
290
291
292 assertEquals("Position should match initial position", expectedPosition.getSI(),
293 position.getSI(), 0.0001);
294 }
295 }
296 catch (GTUException ne)
297 {
298 if (!expectException)
299 {
300 System.out.println(ne);
301 fail("Calling position on lane that the car is on should NOT have thrown a NetworkException");
302 }
303 }
304 try
305 {
306 double fractionalPosition = car.fractionalPosition(lane, relativePosition);
307 if (expectException)
308 {
309
310 fail("Calling position on lane that the car is NOT on should have thrown a NetworkException");
311 }
312 else
313 {
314 Length expectedPosition = laneGroup == lanesGroupA ? positionA : positionB;
315 expectedPosition = expectedPosition
316 .plus(new Length(stepTime.getSI() * initialSpeed.getSI(), LengthUnit.SI));
317 expectedPosition = expectedPosition.plus(new Length(
318 0.5 * acceleration.getSI() * stepTime.getSI() * stepTime.getSI(), LengthUnit.SI));
319 expectedPosition = expectedPosition.plus(relativePosition.getDx());
320
321
322 double expectedFractionalPosition = expectedPosition.getSI() / lane.getLength().getSI();
323 assertEquals("Position should match initial position", expectedFractionalPosition,
324 fractionalPosition, 0.000001);
325 }
326 }
327 catch (GTUException ne)
328 {
329 if (!expectException)
330 {
331 System.out.println(ne);
332 fail("Calling fractionalPosition on lane that the car is on should NOT have thrown a "
333 + "NetworkException");
334 }
335 }
336 }
337 }
338 }
339 }
340
341 OTSNode nodeCFrom = new OTSNode(this.network, "CFrom", new OTSPoint3D(10, 100, 0));
342 OTSNode nodeCTo = new OTSNode(this.network, "CTo", new OTSPoint3D(1000, 0, 0));
343 Lane[] lanesGroupC = LaneFactory.makeMultiLane(this.network, "C", nodeCFrom, nodeCTo, null, 3, laneType,
344 new Speed(100, KM_PER_HOUR), simulator);
345 car.enterLane(lanesGroupC[0], new Length(0.0, LengthUnit.SI), GTUDirectionality.DIR_PLUS);
346 for (RelativePosition relativePosition : new RelativePosition[] { car.getFront(), car.getRear() })
347 {
348 Map<Lane, Double> positions = car.fractionalPositions(relativePosition);
349 assertEquals("Car should be in three lanes", 3, positions.size());
350 Double pos = positions.get(lanesGroupA[1]);
351 assertTrue("Car should be in lane 1 of lane group A", null != pos);
352 assertEquals("fractional position should be equal to result of fractionalPosition(lane, ...)", pos,
353 car.fractionalPosition(lanesGroupA[1], relativePosition), 0.0000001);
354 pos = positions.get(lanesGroupB[1]);
355 assertTrue("Car should be in lane 1 of lane group B", null != pos);
356 assertEquals("fractional position should be equal to result of fractionalPosition(lane, ...)", pos,
357 car.fractionalPosition(lanesGroupB[1], relativePosition), 0.0000001);
358 pos = positions.get(lanesGroupC[0]);
359 assertTrue("Car should be in lane 0 of lane group C", null != pos);
360
361
362
363 assertEquals("fractional position should be equal to result of fractionalPosition(lane, ...)", pos,
364 car.fractionalPosition(lanesGroupC[0], relativePosition), 0.0000001);
365 }
366 car.leaveLane(lanesGroupA[1]);
367 for (RelativePosition relativePosition : new RelativePosition[] { car.getFront(), car.getRear() })
368 {
369 Map<Lane, Double> positions = car.fractionalPositions(relativePosition);
370 assertEquals("Car should be in two lanes", 2, positions.size());
371 Double pos = positions.get(lanesGroupB[1]);
372 assertTrue("Car should be in lane 1 of lane group B", null != pos);
373 assertEquals("fractional position should be equal to result of fractionalPosition(lane, ...)", pos,
374 car.fractionalPosition(lanesGroupB[1], relativePosition), 0.0000001);
375 pos = positions.get(lanesGroupC[0]);
376 assertTrue("Car should be in lane 0 of lane group C", null != pos);
377
378
379
380 assertEquals("fractional position should be equal to result of fractionalPosition(lane, ...)", pos,
381 car.fractionalPosition(lanesGroupC[0], relativePosition), 0.0000001);
382 }
383
384
385 }
386 }
387
388
389
390
391
392
393
394
395
396
397
398 class DummyModel implements OTSModelInterface
399 {
400
401 private static final long serialVersionUID = 20150114L;
402
403
404 private SimulatorInterface<Time, Duration, SimTimeDoubleUnit> simulator;
405
406
407
408
409
410 public void setSimulator(final SimulatorInterface<Time, Duration, SimTimeDoubleUnit> simulator)
411 {
412 this.simulator = simulator;
413 }
414
415
416 @Override
417 public OTSNetwork getNetwork()
418 {
419 return null;
420 }
421
422
423 @Override
424 public void constructModel(final SimulatorInterface<Time, Duration, SimTimeDoubleUnit> arg0) throws SimRuntimeException
425 {
426
427 }
428
429
430 @Override
431 public SimulatorInterface<Time, Duration, SimTimeDoubleUnit> getSimulator()
432
433 {
434 if (null == this.simulator)
435 {
436 throw new Error("getSimulator called, but simulator field is null");
437 }
438 return this.simulator;
439 }
440
441 }