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