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.io.Serializable;
9 import java.util.ArrayList;
10 import java.util.Collection;
11 import java.util.LinkedHashSet;
12 import java.util.List;
13 import java.util.Set;
14
15 import org.djunits.unit.DurationUnit;
16 import org.djunits.unit.TimeUnit;
17 import org.djunits.unit.util.UNITS;
18 import org.djunits.value.vdouble.scalar.Acceleration;
19 import org.djunits.value.vdouble.scalar.Direction;
20 import org.djunits.value.vdouble.scalar.Duration;
21 import org.djunits.value.vdouble.scalar.Length;
22 import org.djunits.value.vdouble.scalar.Speed;
23 import org.djunits.value.vdouble.scalar.Time;
24 import org.junit.Test;
25 import org.opentrafficsim.base.parameters.ParameterTypes;
26 import org.opentrafficsim.base.parameters.Parameters;
27 import org.opentrafficsim.core.dsol.AbstractOTSModel;
28 import org.opentrafficsim.core.dsol.OTSSimulator;
29 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
30 import org.opentrafficsim.core.geometry.OTSPoint3D;
31 import org.opentrafficsim.core.gtu.GTUDirectionality;
32 import org.opentrafficsim.core.gtu.GTUException;
33 import org.opentrafficsim.core.gtu.GTUType;
34 import org.opentrafficsim.core.idgenerator.IdGenerator;
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.OTSRoadNetwork;
48 import org.opentrafficsim.road.network.factory.LaneFactory;
49 import org.opentrafficsim.road.network.lane.CrossSectionElement;
50 import org.opentrafficsim.road.network.lane.CrossSectionLink;
51 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
52 import org.opentrafficsim.road.network.lane.Lane;
53 import org.opentrafficsim.road.network.lane.LaneType;
54 import org.opentrafficsim.road.network.lane.OTSRoadNode;
55
56 import nl.tudelft.simulation.dsol.SimRuntimeException;
57 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
58
59
60
61
62
63
64
65
66
67
68
69
70 public class LaneBasedGTUTest implements UNITS
71 {
72
73 private IdGenerator idGenerator = new IdGenerator("id");
74
75
76
77
78
79
80
81
82
83
84 private void leaderFollowerParallel(final int truckFromLane, final int truckUpToLane, final int carLanesCovered)
85 throws Exception
86 {
87
88 if (carLanesCovered < 1)
89 {
90 fail("carLanesCovered must be >= 1 (got " + carLanesCovered + ")");
91 }
92 if (truckUpToLane < truckFromLane)
93 {
94 fail("truckUpToLane must be >= truckFromLane");
95 }
96 OTSSimulatorInterface simulator = new OTSSimulator("leaderFollowerParallel");
97 OTSRoadNetwork network = new OTSRoadNetwork("leader follower parallel gtu test network", true, simulator);
98
99 Model model = new Model(simulator);
100 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
101 GTUType carType = network.getGtuType(GTUType.DEFAULTS.CAR);
102 GTUType truckType = network.getGtuType(GTUType.DEFAULTS.TRUCK);
103 LaneType laneType = network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
104
105 List<OTSRoadNode> nodes = new ArrayList<>();
106 int[] linkBoundaries = {0, 25, 50, 100, 101, 102, 103, 104, 105, 150, 175, 200};
107 for (int xPos : linkBoundaries)
108 {
109 nodes.add(new OTSRoadNode(network, "Node at " + xPos, new OTSPoint3D(xPos, 20, 0), Direction.ZERO));
110 }
111
112 ArrayList<CrossSectionLink> links = new ArrayList<CrossSectionLink>();
113 final int laneCount = 5;
114 for (int i = 1; i < nodes.size(); i++)
115 {
116 OTSRoadNode fromNode = nodes.get(i - 1);
117 OTSRoadNode toNode = nodes.get(i);
118 String linkName = fromNode.getId() + "-" + toNode.getId();
119 Lane[] lanes = LaneFactory.makeMultiLane(network, linkName, fromNode, toNode, null, laneCount, laneType,
120 new Speed(100, KM_PER_HOUR), simulator);
121 links.add(lanes[0].getParentLink());
122 }
123
124 Length truckPosition = new Length(99.5, METER);
125 Length truckLength = new Length(15, METER);
126
127 Set<DirectedLanePosition> truckPositions =
128 buildPositionsSet(truckPosition, truckLength, links, truckFromLane, truckUpToLane);
129 Speed truckSpeed = new Speed(0, KM_PER_HOUR);
130 Length truckWidth = new Length(2.5, METER);
131 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
132 Speed maximumSpeed = new Speed(120, KM_PER_HOUR);
133 GTUFollowingModelOld gtuFollowingModel = new IDMPlusOld();
134 Parameters parameters = DefaultTestParameters.create();
135
136 LaneBasedIndividualGTU truck = new LaneBasedIndividualGTU("Truck", truckType, truckLength, truckWidth, maximumSpeed,
137 truckLength.times(0.5), simulator, network);
138 LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
139 new LaneBasedCFLCTacticalPlanner(gtuFollowingModel, laneChangeModel, truck), truck);
140 truck.setParameters(parameters);
141 truck.init(strategicalPlanner, truckPositions, truckSpeed);
142
143 int lanesChecked = 0;
144 int found = 0;
145 for (CrossSectionLink link : links)
146 {
147 for (CrossSectionElement cse : link.getCrossSectionElementList())
148 {
149 if (cse instanceof Lane)
150 {
151 Lane lane = (Lane) cse;
152 boolean truckPositionsOnLane = false;
153 for (DirectedLanePosition pos : truckPositions)
154 {
155 if (pos.getLane().equals(lane))
156 {
157 truckPositionsOnLane = true;
158 }
159 }
160 if (truckPositionsOnLane)
161 {
162 assertTrue("Truck should be registered on Lane " + lane, lane.getGtuList().contains(truck));
163 found++;
164 }
165 else
166 {
167 assertFalse("Truck should NOT be registered on Lane " + lane, lane.getGtuList().contains(truck));
168 }
169 lanesChecked++;
170 }
171 }
172 }
173
174 assertEquals("lanesChecked should equals the number of Links times the number of lanes on each Link",
175 laneCount * links.size(), lanesChecked);
176 assertEquals("Truck should be registered in " + truckPositions.size() + " lanes", truckPositions.size(), found);
177 Length forwardMaxDistance = truck.getParameters().getParameter(ParameterTypes.LOOKAHEAD);
178
179 truck.getTacticalPlanner().getPerception().perceive();
180 Headway leader = truck.getTacticalPlanner().getPerception().getPerceptionCategory(DefaultSimplePerception.class)
181 .getForwardHeadwayGTU();
182 assertTrue(
183 "With one vehicle in the network forward headway should return a value larger than zero, and smaller than maxDistance",
184 forwardMaxDistance.getSI() >= leader.getDistance().si && leader.getDistance().si > 0);
185 assertEquals("With one vehicle in the network forward headwayGTU should return null", null, leader.getId());
186
187 Length reverseMaxDistance = truck.getParameters().getParameter(ParameterTypes.LOOKBACKOLD);
188 Headway follower = truck.getTacticalPlanner().getPerception().getPerceptionCategory(DefaultSimplePerception.class)
189 .getBackwardHeadway();
190 assertTrue(
191 "With one vehicle in the network reverse headway should return a value less than zero, and smaller than |maxDistance|",
192 Math.abs(reverseMaxDistance.getSI()) >= Math.abs(follower.getDistance().si) && follower.getDistance().si < 0);
193 assertEquals("With one vehicle in the network reverse headwayGTU should return null", null, follower.getId());
194 Length carLength = new Length(4, METER);
195 Length carWidth = new Length(1.8, METER);
196 Speed carSpeed = new Speed(0, KM_PER_HOUR);
197 int maxStep = linkBoundaries[linkBoundaries.length - 1];
198 for (int laneRank = 0; laneRank < laneCount + 1 - carLanesCovered; laneRank++)
199 {
200 for (int step = 0; step < maxStep; step += 5)
201 {
202 if (laneRank >= truckFromLane && laneRank <= truckUpToLane
203 && step >= truckPosition.getSI() - truckLength.getSI()
204 && step - carLength.getSI() <= truckPosition.getSI())
205 {
206 continue;
207 }
208 Length carPosition = new Length(step, METER);
209 Set<DirectedLanePosition> carPositions =
210 buildPositionsSet(carPosition, carLength, links, laneRank, laneRank + carLanesCovered - 1);
211 parameters = DefaultTestParameters.create();
212
213 LaneBasedIndividualGTU car = new LaneBasedIndividualGTU("Car", carType, carLength, carWidth, maximumSpeed,
214 carLength.times(0.5), simulator, network);
215 strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
216 new LaneBasedCFLCTacticalPlanner(gtuFollowingModel, laneChangeModel, car), car);
217 car.setParameters(parameters);
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 final void timeAtDistanceTest() throws Exception
369 {
370 for (int a = 1; a >= -1; a--)
371 {
372 OTSSimulatorInterface simulator = new OTSSimulator("timeAtDistanceTest");
373 OTSRoadNetwork network = new OTSRoadNetwork("test", true, simulator);
374
375 Model model = new Model(simulator);
376 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
377
378 simulator.runUpTo(new SimTimeDoubleUnit(new Time(60, TimeUnit.BASE_SECOND)));
379 while (simulator.isStartingOrRunning())
380 {
381 try
382 {
383 Thread.sleep(1);
384 }
385 catch (InterruptedException ie)
386 {
387 ie = null;
388 }
389 }
390 GTUType carType = network.getGtuType(GTUType.DEFAULTS.CAR);
391 LaneType laneType = network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
392 OTSRoadNode fromNode = new OTSRoadNode(network, "Node A", new OTSPoint3D(0, 0, 0), Direction.ZERO);
393 OTSRoadNode toNode = new OTSRoadNode(network, "Node B", new OTSPoint3D(1000, 0, 0), Direction.ZERO);
394 String linkName = "AB";
395 Lane lane = LaneFactory.makeMultiLane(network, linkName, fromNode, toNode, null, 1, laneType,
396 new Speed(200, KM_PER_HOUR), simulator)[0];
397 Length carPosition = new Length(100, METER);
398 Set<DirectedLanePosition> carPositions = new LinkedHashSet<>(1);
399 carPositions.add(new DirectedLanePosition(lane, carPosition, GTUDirectionality.DIR_PLUS));
400 Speed carSpeed = new Speed(10, METER_PER_SECOND);
401 Acceleration acceleration = new Acceleration(a, METER_PER_SECOND_2);
402 FixedAccelerationModel fam = new FixedAccelerationModel(acceleration, new Duration(10, SECOND));
403 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
404 Speed maximumSpeed = new Speed(200, KM_PER_HOUR);
405 Parameters parameters = DefaultTestParameters.create();
406
407 LaneBasedIndividualGTU car = new LaneBasedIndividualGTU("Car" + this.idGenerator.nextId(), carType,
408 new Length(4, METER), new Length(1.8, METER), maximumSpeed, Length.instantiateSI(2.0), simulator, network);
409 LaneBasedStrategicalPlanner strategicalPlanner =
410 new LaneBasedStrategicalRoutePlanner(new LaneBasedCFLCTacticalPlanner(fam, laneChangeModel, car), car);
411 car.setParameters(parameters);
412 car.init(strategicalPlanner, carPositions, carSpeed);
413
414 simulator.runUpTo(new SimTimeDoubleUnit(new Time(61, TimeUnit.BASE_SECOND)));
415 while (simulator.isStartingOrRunning())
416 {
417 try
418 {
419 Thread.sleep(1);
420 }
421 catch (InterruptedException ie)
422 {
423 ie = null;
424 }
425 }
426
427
428
429 for (int timeStep = 1; timeStep < 100; timeStep++)
430 {
431 double deltaTime = 0.1 * timeStep;
432 double distanceAtTime = carSpeed.getSI() * deltaTime + 0.5 * acceleration.getSI() * deltaTime * deltaTime;
433
434
435
436
437
438
439
440
441
442 }
443 }
444 }
445
446
447
448
449 public final void autoPauseSimulator()
450 {
451
452 }
453
454
455
456
457
458
459
460
461
462
463 private Set<DirectedLanePosition> buildPositionsSet(final Length totalLongitudinalPosition, final Length gtuLength,
464 final ArrayList<CrossSectionLink> links, final int fromLaneRank, final int uptoLaneRank)
465 {
466 Set<DirectedLanePosition> result = new LinkedHashSet<>(1);
467 double cumulativeLength = 0;
468 for (CrossSectionLink link : links)
469 {
470 double linkLength = link.getLength().getSI();
471 double frontPositionInLink = totalLongitudinalPosition.getSI() - cumulativeLength + gtuLength.getSI();
472 double rearPositionInLink = frontPositionInLink - gtuLength.getSI();
473 double linkEnd = cumulativeLength + linkLength;
474
475
476 if (rearPositionInLink < linkLength && frontPositionInLink >= 0)
477 {
478
479 for (int laneRank = fromLaneRank; laneRank <= uptoLaneRank; laneRank++)
480 {
481 Lane lane = getNthLane(link, laneRank);
482 if (null == lane)
483 {
484 fail("Error in test; canot find lane with rank " + laneRank);
485 }
486 try
487 {
488 result.add(new DirectedLanePosition(lane, new Length(rearPositionInLink, METER),
489 GTUDirectionality.DIR_PLUS));
490 }
491 catch (GTUException exception)
492 {
493 fail("Error in test; DirectedLanePosition for lane " + lane);
494 }
495 }
496 }
497 cumulativeLength += linkLength;
498 }
499 return result;
500 }
501
502
503
504
505
506
507
508 private Lane getNthLane(final CrossSectionLink link, int rank)
509 {
510 for (CrossSectionElement cse : link.getCrossSectionElementList())
511 {
512 if (cse instanceof Lane)
513 {
514 if (0 == rank--)
515 {
516 return (Lane) cse;
517 }
518 }
519 }
520 return null;
521 }
522
523
524 public static class Model extends AbstractOTSModel
525 {
526
527
528
529 public Model(final OTSSimulatorInterface simulator)
530 {
531 super(simulator);
532 }
533
534
535 private static final long serialVersionUID = 20141027L;
536
537
538 @Override
539 public final void constructModel() throws SimRuntimeException
540 {
541
542 }
543
544
545 @Override
546 public final OTSRoadNetwork getNetwork()
547 {
548 return null;
549 }
550
551
552 @Override
553 public Serializable getSourceId()
554 {
555 return "LaneBasedGTUTest.Model";
556 }
557 }
558 }