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