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