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