1 package org.opentrafficsim.road.gtu;
2
3 import static org.junit.Assert.assertEquals;
4 import static org.junit.Assert.assertFalse;
5 import static org.junit.Assert.assertTrue;
6 import static org.junit.Assert.fail;
7
8 import java.util.ArrayList;
9 import java.util.LinkedHashSet;
10 import java.util.Set;
11
12 import nl.tudelft.simulation.dsol.SimRuntimeException;
13 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
14
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.DoubleScalar;
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.core.dsol.OTSModelInterface;
24 import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
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.idgenerator.IdGenerator;
30 import org.opentrafficsim.core.network.LongitudinalDirectionality;
31 import org.opentrafficsim.core.network.OTSNetwork;
32 import org.opentrafficsim.core.network.OTSNode;
33 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
34 import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
35 import org.opentrafficsim.road.gtu.lane.driver.LaneBasedBehavioralCharacteristics;
36 import org.opentrafficsim.road.gtu.lane.perception.LanePerceptionFull;
37 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
38 import org.opentrafficsim.road.gtu.lane.tactical.following.FixedAccelerationModel;
39 import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
40 import org.opentrafficsim.road.gtu.lane.tactical.following.HeadwayGTU;
41 import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
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.CrossSectionElement;
48 import org.opentrafficsim.road.network.lane.CrossSectionLink;
49 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
50 import org.opentrafficsim.road.network.lane.Lane;
51 import org.opentrafficsim.road.network.lane.LaneType;
52 import org.opentrafficsim.simulationengine.SimpleSimulator;
53
54
55
56
57
58
59
60
61
62
63
64
65 public class LaneBasedGTUTest implements UNITS
66 {
67
68 private OTSNetwork network = new OTSNetwork("network");
69
70
71 private IdGenerator idGenerator = new IdGenerator("id");
72
73
74
75
76
77
78
79
80
81
82 private void leaderFollowerParallel(int truckFromLane, int truckUpToLane, int carLanesCovered) throws Exception
83 {
84
85 if (carLanesCovered < 1)
86 {
87 fail("carLanesCovered must be >= 1 (got " + carLanesCovered + ")");
88 }
89 if (truckUpToLane < truckFromLane)
90 {
91 fail("truckUpToLane must be >= truckFromLane");
92 }
93 OTSModelInterface model = new Model();
94 SimpleSimulator simulator =
95 new SimpleSimulator(new Time.Abs(0.0, SECOND), new Time.Rel(0.0, SECOND), new Time.Rel(3600.0, SECOND), model);
96 GTUType carType = GTUType.makeGTUType("car");
97 GTUType truckType = GTUType.makeGTUType("truck");
98 LaneType laneType = new LaneType("CarLane");
99 laneType.addCompatibility(carType);
100 laneType.addCompatibility(truckType);
101
102 ArrayList<OTSNode> nodes = new ArrayList<OTSNode>();
103 int[] linkBoundaries = { 0, 25, 50, 100, 101, 102, 103, 104, 105, 150, 175, 200 };
104 for (int xPos : linkBoundaries)
105 {
106 nodes.add(new OTSNode("Node at " + xPos, new OTSPoint3D(xPos, 20, 0)));
107 }
108
109 ArrayList<CrossSectionLink> links = new ArrayList<CrossSectionLink>();
110 final int laneCount = 5;
111 for (int i = 1; i < nodes.size(); i++)
112 {
113 OTSNode fromNode = nodes.get(i - 1);
114 OTSNode toNode = nodes.get(i);
115 String linkName = fromNode.getId() + "-" + toNode.getId();
116 Lane[] lanes =
117 LaneFactory.makeMultiLane(linkName, fromNode, toNode, null, laneCount, laneType,
118 new Speed(100, KM_PER_HOUR), simulator, LongitudinalDirectionality.DIR_PLUS);
119 links.add(lanes[0].getParentLink());
120 }
121
122 Length.Rel truckPosition = new Length.Rel(99.5, METER);
123 Length.Rel truckLength = new Length.Rel(15, METER);
124
125 Set<DirectedLanePosition> truckPositions =
126 buildPositionsSet(truckPosition, truckLength, links, truckFromLane, truckUpToLane);
127 Speed truckSpeed = new Speed(0, KM_PER_HOUR);
128 Length.Rel truckWidth = new Length.Rel(2.5, METER);
129 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
130 Speed maximumVelocity = new Speed(120, KM_PER_HOUR);
131 GTUFollowingModelOld gtuFollowingModel = new IDMPlusOld();
132 LaneBasedBehavioralCharacteristics drivingCharacteristics =
133 new LaneBasedBehavioralCharacteristics(gtuFollowingModel, laneChangeModel);
134 LaneBasedStrategicalPlanner strategicalPlanner =
135 new LaneBasedStrategicalRoutePlanner(drivingCharacteristics, new LaneBasedCFLCTacticalPlanner());
136 LaneBasedIndividualGTU truck =
137 new LaneBasedIndividualGTU("Truck", truckType, truckPositions, truckSpeed, truckLength, truckWidth,
138 maximumVelocity, simulator, strategicalPlanner, new LanePerceptionFull(), this.network);
139
140 int lanesChecked = 0;
141 int found = 0;
142 for (CrossSectionLink link : links)
143 {
144 for (CrossSectionElement cse : link.getCrossSectionElementList())
145 {
146 if (cse instanceof Lane)
147 {
148 Lane lane = (Lane) cse;
149 boolean truckPositionsOnLane = false;
150 for (DirectedLanePosition pos : truckPositions)
151 {
152 if (pos.getLane().equals(lane))
153 {
154 truckPositionsOnLane = true;
155 }
156 }
157 if (truckPositionsOnLane)
158 {
159 assertTrue("Truck should be registered on Lane " + lane, lane.getGtuList().contains(truck));
160 found++;
161 }
162 else
163 {
164 assertFalse("Truck should NOT be registered on Lane " + lane, lane.getGtuList().contains(truck));
165 }
166 lanesChecked++;
167 }
168 }
169 }
170
171 assertEquals("lanesChecked should equals the number of Links times the number of lanes on each Link",
172 laneCount * links.size(), lanesChecked);
173 assertEquals("Truck should be registered in " + truckPositions.size() + " lanes", truckPositions.size(), found);
174 Length.Rel forwardMaxDistance = truck.getBehavioralCharacteristics().getForwardHeadwayDistance();
175
176 truck.getPerception().perceive();
177 HeadwayGTU leader = truck.getPerception().getForwardHeadwayGTU();
178 assertTrue(
179 "With one vehicle in the network forward headway should return a value larger than zero, and smaller than maxDistance",
180 forwardMaxDistance.getSI() >= leader.getDistance().si && leader.getDistance().si > 0);
181 assertEquals("With one vehicle in the network forward headwayGTU should return null", null, leader.getGtuId());
182
183 Length.Rel reverseMaxDistance = truck.getBehavioralCharacteristics().getBackwardHeadwayDistance();
184 HeadwayGTU follower = truck.getPerception().getBackwardHeadwayGTU();
185 assertTrue(
186 "With one vehicle in the network reverse headway should return a value less than zero, and smaller than |maxDistance|",
187 Math.abs(reverseMaxDistance.getSI()) >= Math.abs(follower.getDistance().si) && follower.getDistance().si < 0);
188 assertEquals("With one vehicle in the network reverse headwayGTU should return null", null, follower.getGtuId());
189 Length.Rel carLength = new Length.Rel(4, METER);
190 Length.Rel carWidth = new Length.Rel(1.8, METER);
191 Speed carSpeed = new Speed(0, KM_PER_HOUR);
192 int maxStep = linkBoundaries[linkBoundaries.length - 1];
193 for (int laneRank = 0; laneRank < laneCount + 1 - carLanesCovered; laneRank++)
194 {
195 for (int step = 0; step < maxStep; step += 5)
196 {
197 if (laneRank >= truckFromLane && laneRank <= truckUpToLane
198 && step >= truckPosition.getSI() - truckLength.getSI()
199 && step - carLength.getSI() <= truckPosition.getSI())
200 {
201 continue;
202 }
203 Length.Rel carPosition = new Length.Rel(step, METER);
204 Set<DirectedLanePosition> carPositions =
205 buildPositionsSet(carPosition, carLength, links, laneRank, laneRank + carLanesCovered - 1);
206 drivingCharacteristics = new LaneBasedBehavioralCharacteristics(gtuFollowingModel, laneChangeModel);
207 strategicalPlanner =
208 new LaneBasedStrategicalRoutePlanner(drivingCharacteristics, new LaneBasedCFLCTacticalPlanner());
209 LaneBasedIndividualGTU car =
210 new LaneBasedIndividualGTU("Car", carType, carPositions, carSpeed, carLength, carWidth,
211 maximumVelocity, simulator, strategicalPlanner, new LanePerceptionFull(), this.network);
212
213
214 leader = truck.getPerception().getForwardHeadwayGTU();
215 double actualHeadway = leader.getDistance().si;
216 double expectedHeadway =
217 laneRank + carLanesCovered - 1 < truckFromLane || laneRank > truckUpToLane
218 || step - truckPosition.getSI() - truckLength.getSI() <= 0 ? Double.MAX_VALUE : step
219 - truckLength.getSI() - truckPosition.getSI();
220
221
222
223
224 assertEquals("Forward headway should return " + expectedHeadway, expectedHeadway, actualHeadway, 0.1);
225 String leaderGtuId = leader.getGtuId();
226 if (expectedHeadway == Double.MAX_VALUE)
227 {
228 assertEquals("Leader id should be null", null, leaderGtuId);
229 }
230 else
231 {
232 assertEquals("Leader id should be the car id", car, leaderGtuId);
233 }
234
235 follower = truck.getPerception().getBackwardHeadwayGTU();
236 double actualReverseHeadway = follower.getDistance().si;
237 double expectedReverseHeadway =
238 laneRank + carLanesCovered - 1 < truckFromLane || laneRank > truckUpToLane
239 || step + carLength.getSI() >= truckPosition.getSI() ? Double.MAX_VALUE : truckPosition.getSI()
240 - carLength.getSI() - step;
241 assertEquals("Reverse headway should return " + expectedReverseHeadway, expectedReverseHeadway,
242 actualReverseHeadway, 0.1);
243 String followerGtuId = follower.getGtuId();
244 if (expectedReverseHeadway == Double.MAX_VALUE)
245 {
246 assertEquals("Follower id should be null", null, followerGtuId);
247 }
248 else
249 {
250 assertEquals("Follower id should be the car id", car.getId(), followerGtuId);
251 }
252 for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
253 {
254 Lane l = null;
255 double cumulativeDistance = 0;
256 for (CrossSectionLink csl : links)
257 {
258 cumulativeDistance += csl.getLength().getSI();
259 if (cumulativeDistance >= truckPosition.getSI())
260 {
261 l = getNthLane(csl, laneIndex);
262 break;
263 }
264 }
265 leader = truck.getPerception().getForwardHeadwayGTU();
266 actualHeadway = leader.getDistance().si;
267 expectedHeadway =
268 laneIndex < laneRank || laneIndex > laneRank + carLanesCovered - 1
269 || step - truckLength.getSI() - truckPosition.getSI() <= 0 ? Double.MAX_VALUE : step
270 - truckLength.getSI() - truckPosition.getSI();
271 assertEquals("Headway on lane " + laneIndex + " should be " + expectedHeadway, expectedHeadway,
272 actualHeadway, 0.001);
273 leaderGtuId = leader.getGtuId();
274 if (laneIndex >= laneRank && laneIndex <= laneRank + carLanesCovered - 1
275 && step - truckLength.getSI() - truckPosition.getSI() > 0)
276 {
277 assertEquals("Leader id should be the car id", car.getId(), leaderGtuId);
278 }
279 else
280 {
281 assertEquals("Leader id should be null", null, leaderGtuId);
282 }
283 follower = truck.getPerception().getBackwardHeadwayGTU();
284 actualReverseHeadway = follower.getDistance().si;
285 expectedReverseHeadway =
286 laneIndex < laneRank || laneIndex > laneRank + carLanesCovered - 1
287 || step + carLength.getSI() >= truckPosition.getSI() ? Double.MAX_VALUE : truckPosition
288 .getSI() - carLength.getSI() - step;
289 assertEquals("Headway on lane " + laneIndex + " should be " + expectedReverseHeadway,
290 expectedReverseHeadway, actualReverseHeadway, 0.001);
291 followerGtuId = follower.getGtuId();
292 if (laneIndex >= laneRank && laneIndex <= laneRank + carLanesCovered - 1
293 && step + carLength.getSI() < truckPosition.getSI())
294 {
295 assertEquals("Follower id should be the car id", car, followerGtuId);
296 }
297 else
298 {
299 assertEquals("Follower id should be null", null, followerGtuId);
300 }
301 }
302 Set<LaneBasedGTU> leftParallel = truck.getPerception().getParallelGTUsLeft();
303 int expectedLeftSize =
304 laneRank + carLanesCovered - 1 < truckFromLane - 1 || laneRank >= truckUpToLane
305 || step + carLength.getSI() <= truckPosition.getSI()
306 || step > truckPosition.getSI() + truckLength.getSI() ? 0 : 1;
307
308 assertEquals("Left parallel set size should be " + expectedLeftSize, expectedLeftSize, leftParallel.size());
309 if (leftParallel.size() > 0)
310 {
311 assertTrue("Parallel GTU should be the car", leftParallel.contains(car));
312 }
313 Set<LaneBasedGTU> rightParallel = truck.getPerception().getParallelGTUsRight();
314 int expectedRightSize =
315 laneRank + carLanesCovered - 1 <= truckFromLane || laneRank > truckUpToLane + 1
316 || step + carLength.getSI() < truckPosition.getSI()
317 || step > truckPosition.getSI() + truckLength.getSI() ? 0 : 1;
318 assertEquals("Right parallel set size should be " + expectedRightSize, expectedRightSize, rightParallel.size());
319 if (rightParallel.size() > 0)
320 {
321 assertTrue("Parallel GTU should be the car", rightParallel.contains(car));
322 }
323 for (DirectedLanePosition pos : carPositions)
324 {
325 pos.getLane().removeGTU(car);
326 }
327 }
328 }
329 }
330
331
332
333
334
335 @Test
336 public void leaderFollowerAndParallelTest() throws Exception
337 {
338
339
340
341
342 }
343
344
345
346
347
348 @Test
349 public void timeAtDistanceTest() throws Exception
350 {
351 for (int a = 1; a >= -1; a--)
352 {
353
354 OTSModelInterface model = new Model();
355 SimpleSimulator simulator =
356 new SimpleSimulator(new Time.Abs(0.0, SECOND), new Time.Rel(0.0, SECOND), new Time.Rel(3600.0, SECOND),
357 model);
358
359 simulator.runUpTo(new Time.Abs(60, SECOND));
360 while (simulator.isRunning())
361 {
362 try
363 {
364 Thread.sleep(1);
365 }
366 catch (InterruptedException ie)
367 {
368 ie = null;
369 }
370 }
371 GTUType carType = GTUType.makeGTUType("car");
372 LaneType laneType = new LaneType("CarLane");
373 laneType.addCompatibility(carType);
374 OTSNode fromNode = new OTSNode("Node A", new OTSPoint3D(0, 0, 0));
375 OTSNode toNode = new OTSNode("Node B", new OTSPoint3D(1000, 0, 0));
376 String linkName = "AB";
377 Lane lane =
378 LaneFactory.makeMultiLane(linkName, fromNode, toNode, null, 1, laneType, new Speed(200, KM_PER_HOUR),
379 simulator, LongitudinalDirectionality.DIR_PLUS)[0];
380 Length.Rel carPosition = new Length.Rel(100, METER);
381 Set<DirectedLanePosition> carPositions = new LinkedHashSet<>(1);
382 carPositions.add(new DirectedLanePosition(lane, carPosition, GTUDirectionality.DIR_PLUS));
383 Speed carSpeed = new Speed(10, METER_PER_SECOND);
384 Acceleration acceleration = new Acceleration(a, METER_PER_SECOND_2);
385 FixedAccelerationModel fam = new FixedAccelerationModel(acceleration, new Time.Rel(10, SECOND));
386 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
387 Speed maximumVelocity = new Speed(200, KM_PER_HOUR);
388 LaneBasedBehavioralCharacteristics drivingCharacteristics =
389 new LaneBasedBehavioralCharacteristics(fam, laneChangeModel);
390 LaneBasedStrategicalPlanner strategicalPlanner =
391 new LaneBasedStrategicalRoutePlanner(drivingCharacteristics, new LaneBasedCFLCTacticalPlanner());
392 LaneBasedIndividualGTU car =
393 new LaneBasedIndividualGTU("Car" + this.idGenerator.nextId(), carType, carPositions, carSpeed,
394 new Length.Rel(4, METER), new Length.Rel(1.8, METER), maximumVelocity, simulator,
395 strategicalPlanner, new LanePerceptionFull(), this.network);
396
397 simulator.runUpTo(new Time.Abs(61, SECOND));
398 while (simulator.isRunning())
399 {
400 try
401 {
402 Thread.sleep(1);
403 }
404 catch (InterruptedException ie)
405 {
406 ie = null;
407 }
408 }
409
410
411
412 for (int timeStep = 1; timeStep < 100; timeStep++)
413 {
414 double deltaTime = 0.1 * timeStep;
415 double distanceAtTime = carSpeed.getSI() * deltaTime + 0.5 * acceleration.getSI() * deltaTime * deltaTime;
416
417
418
419
420
421
422
423
424
425 }
426 }
427 }
428
429
430
431
432 public final void autoPauseSimulator()
433 {
434
435 }
436
437
438
439
440
441
442
443
444
445
446 private Set<DirectedLanePosition> buildPositionsSet(Length.Rel totalLongitudinalPosition, Length.Rel gtuLength,
447 ArrayList<CrossSectionLink> links, int fromLaneRank, int uptoLaneRank)
448 {
449 Set<DirectedLanePosition> result = new LinkedHashSet<>(1);
450 double cumulativeLength = 0;
451 for (CrossSectionLink link : links)
452 {
453 double linkLength = link.getLength().getSI();
454 double frontPositionInLink = totalLongitudinalPosition.getSI() - cumulativeLength + gtuLength.getSI();
455 double rearPositionInLink = frontPositionInLink - gtuLength.getSI();
456 double linkEnd = cumulativeLength + linkLength;
457
458
459 if (rearPositionInLink < linkLength && frontPositionInLink >= 0)
460 {
461
462 for (int laneRank = fromLaneRank; laneRank <= uptoLaneRank; laneRank++)
463 {
464 Lane lane = getNthLane(link, laneRank);
465 if (null == lane)
466 {
467 fail("Error in test; canot find lane with rank " + laneRank);
468 }
469 try
470 {
471 result.add(new DirectedLanePosition(lane, new Length.Rel(rearPositionInLink, METER),
472 GTUDirectionality.DIR_PLUS));
473 }
474 catch (GTUException exception)
475 {
476 fail("Error in test; DirectedLanePosition for lane " + lane);
477 }
478 }
479 }
480 cumulativeLength += linkLength;
481 }
482 return result;
483 }
484
485
486
487
488
489
490
491 private Lane getNthLane(final CrossSectionLink link, int rank)
492 {
493 for (CrossSectionElement cse : link.getCrossSectionElementList())
494 {
495 if (cse instanceof Lane)
496 {
497 if (0 == rank--)
498 {
499 return (Lane) cse;
500 }
501 }
502 }
503 return null;
504 }
505 }
506
507
508 class Model implements OTSModelInterface
509 {
510
511
512 private static final long serialVersionUID = 20150127L;
513
514
515 @Override
516 public void constructModel(
517 SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble> simulator)
518 throws SimRuntimeException
519 {
520
521 }
522
523
524 @Override
525 public SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble> getSimulator()
526
527 {
528 return null;
529 }
530
531 }