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