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