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