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.DurationUnit;
16 import org.djunits.unit.TimeUnit;
17 import org.djunits.unit.UNITS;
18 import org.djunits.value.vdouble.scalar.Acceleration;
19 import org.djunits.value.vdouble.scalar.Duration;
20 import org.djunits.value.vdouble.scalar.Length;
21 import org.djunits.value.vdouble.scalar.Speed;
22 import org.djunits.value.vdouble.scalar.Time;
23 import org.junit.Test;
24 import org.opentrafficsim.base.parameters.ParameterTypes;
25 import org.opentrafficsim.base.parameters.Parameters;
26 import org.opentrafficsim.core.dsol.AbstractOTSModel;
27 import org.opentrafficsim.core.dsol.OTSSimulator;
28 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
29 import org.opentrafficsim.core.geometry.OTSPoint3D;
30 import org.opentrafficsim.core.gtu.GTUDirectionality;
31 import org.opentrafficsim.core.gtu.GTUException;
32 import org.opentrafficsim.core.gtu.GTUType;
33 import org.opentrafficsim.core.idgenerator.IdGenerator;
34 import org.opentrafficsim.core.network.OTSNetwork;
35 import org.opentrafficsim.core.network.OTSNode;
36 import org.opentrafficsim.road.DefaultTestParameters;
37 import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
38 import org.opentrafficsim.road.gtu.lane.perception.categories.DefaultSimplePerception;
39 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
40 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
41 import org.opentrafficsim.road.gtu.lane.tactical.following.FixedAccelerationModel;
42 import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
43 import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
44 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.FixedLaneChangeModel;
45 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
46 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
47 import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
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
55 import nl.tudelft.simulation.dsol.SimRuntimeException;
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 OTSSimulatorInterface simulator = new OTSSimulator();
98 Model model = new Model(simulator);
99 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
100 GTUType carType = CAR;
101 GTUType truckType = TRUCK;
102 LaneType laneType = LaneType.TWO_WAY_LANE;
103
104 ArrayList<OTSNode> nodes = new ArrayList<OTSNode>();
105 int[] linkBoundaries = { 0, 25, 50, 100, 101, 102, 103, 104, 105, 150, 175, 200 };
106 for (int xPos : linkBoundaries)
107 {
108 nodes.add(new OTSNode(this.network, "Node at " + xPos, new OTSPoint3D(xPos, 20, 0)));
109 }
110
111 ArrayList<CrossSectionLink> links = new ArrayList<CrossSectionLink>();
112 final int laneCount = 5;
113 for (int i = 1; i < nodes.size(); i++)
114 {
115 OTSNode fromNode = nodes.get(i - 1);
116 OTSNode toNode = nodes.get(i);
117 String linkName = fromNode.getId() + "-" + toNode.getId();
118 Lane[] lanes = LaneFactory.makeMultiLane(this.network, linkName, fromNode, toNode, null, laneCount, laneType,
119 new Speed(100, KM_PER_HOUR), simulator);
120 links.add(lanes[0].getParentLink());
121 }
122
123 Length truckPosition = new Length(99.5, METER);
124 Length truckLength = new Length(15, METER);
125
126 Set<DirectedLanePosition> truckPositions =
127 buildPositionsSet(truckPosition, truckLength, links, truckFromLane, truckUpToLane);
128 Speed truckSpeed = new Speed(0, KM_PER_HOUR);
129 Length truckWidth = new Length(2.5, METER);
130 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
131 Speed maximumSpeed = new Speed(120, KM_PER_HOUR);
132 GTUFollowingModelOld gtuFollowingModel = new IDMPlusOld();
133 Parameters parameters = DefaultTestParameters.create();
134
135 LaneBasedIndividualGTU truck = 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 OTSSimulatorInterface simulator = new OTSSimulator();
374 Model model = new Model(simulator);
375 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
376
377 simulator.runUpTo(new Time(60, TimeUnit.BASE_SECOND));
378 while (simulator.isRunning())
379 {
380 try
381 {
382 Thread.sleep(1);
383 }
384 catch (InterruptedException ie)
385 {
386 ie = null;
387 }
388 }
389 GTUType carType = CAR;
390 LaneType laneType = LaneType.TWO_WAY_LANE;
391 OTSNode fromNode = new OTSNode(this.network, "Node A", new OTSPoint3D(0, 0, 0));
392 OTSNode toNode = new OTSNode(this.network, "Node B", new OTSPoint3D(1000, 0, 0));
393 String linkName = "AB";
394 Lane lane = LaneFactory.makeMultiLane(this.network, linkName, fromNode, toNode, null, 1, laneType,
395 new Speed(200, KM_PER_HOUR), simulator)[0];
396 Length carPosition = new Length(100, METER);
397 Set<DirectedLanePosition> carPositions = new LinkedHashSet<>(1);
398 carPositions.add(new DirectedLanePosition(lane, carPosition, GTUDirectionality.DIR_PLUS));
399 Speed carSpeed = new Speed(10, METER_PER_SECOND);
400 Acceleration acceleration = new Acceleration(a, METER_PER_SECOND_2);
401 FixedAccelerationModel fam = new FixedAccelerationModel(acceleration, new Duration(10, SECOND));
402 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
403 Speed maximumSpeed = new Speed(200, KM_PER_HOUR);
404 Parameters parameters = DefaultTestParameters.create();
405
406 LaneBasedIndividualGTU car = new LaneBasedIndividualGTU("Car" + this.idGenerator.nextId(), carType,
407 new Length(4, METER), new Length(1.8, METER), maximumSpeed, Length.createSI(2.0), simulator, this.network);
408 LaneBasedStrategicalPlanner strategicalPlanner =
409 new LaneBasedStrategicalRoutePlanner(new LaneBasedCFLCTacticalPlanner(fam, laneChangeModel, car), car);
410 car.setParameters(parameters);
411 car.init(strategicalPlanner, carPositions, carSpeed);
412
413 simulator.runUpTo(new Time(61, TimeUnit.BASE_SECOND));
414 while (simulator.isRunning())
415 {
416 try
417 {
418 Thread.sleep(1);
419 }
420 catch (InterruptedException ie)
421 {
422 ie = null;
423 }
424 }
425
426
427
428 for (int timeStep = 1; timeStep < 100; timeStep++)
429 {
430 double deltaTime = 0.1 * timeStep;
431 double distanceAtTime = carSpeed.getSI() * deltaTime + 0.5 * acceleration.getSI() * deltaTime * deltaTime;
432
433
434
435
436
437
438
439
440
441 }
442 }
443 }
444
445
446
447
448 public final void autoPauseSimulator()
449 {
450
451 }
452
453
454
455
456
457
458
459
460
461
462 private Set<DirectedLanePosition> buildPositionsSet(final Length totalLongitudinalPosition, final Length gtuLength,
463 final ArrayList<CrossSectionLink> links, final int fromLaneRank, final int uptoLaneRank)
464 {
465 Set<DirectedLanePosition> result = new LinkedHashSet<>(1);
466 double cumulativeLength = 0;
467 for (CrossSectionLink link : links)
468 {
469 double linkLength = link.getLength().getSI();
470 double frontPositionInLink = totalLongitudinalPosition.getSI() - cumulativeLength + gtuLength.getSI();
471 double rearPositionInLink = frontPositionInLink - gtuLength.getSI();
472 double linkEnd = cumulativeLength + linkLength;
473
474
475 if (rearPositionInLink < linkLength && frontPositionInLink >= 0)
476 {
477
478 for (int laneRank = fromLaneRank; laneRank <= uptoLaneRank; laneRank++)
479 {
480 Lane lane = getNthLane(link, laneRank);
481 if (null == lane)
482 {
483 fail("Error in test; canot find lane with rank " + laneRank);
484 }
485 try
486 {
487 result.add(new DirectedLanePosition(lane, new Length(rearPositionInLink, METER),
488 GTUDirectionality.DIR_PLUS));
489 }
490 catch (GTUException exception)
491 {
492 fail("Error in test; DirectedLanePosition for lane " + lane);
493 }
494 }
495 }
496 cumulativeLength += linkLength;
497 }
498 return result;
499 }
500
501
502
503
504
505
506
507 private Lane getNthLane(final CrossSectionLink link, int rank)
508 {
509 for (CrossSectionElement cse : link.getCrossSectionElementList())
510 {
511 if (cse instanceof Lane)
512 {
513 if (0 == rank--)
514 {
515 return (Lane) cse;
516 }
517 }
518 }
519 return null;
520 }
521
522
523 public static class Model extends AbstractOTSModel
524 {
525
526
527
528 public Model(final OTSSimulatorInterface simulator)
529 {
530 super(simulator);
531 }
532
533
534 private static final long serialVersionUID = 20141027L;
535
536
537 @Override
538 public final void constructModel() throws SimRuntimeException
539 {
540
541 }
542
543
544 @Override
545 public final OTSNetwork getNetwork()
546 {
547 return null;
548 }
549 }
550 }