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.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.core.dsol.OTSModelInterface;
25 import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
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.gtu.behavioralcharacteristics.BehavioralCharacteristics;
31 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypes;
32 import org.opentrafficsim.core.idgenerator.IdGenerator;
33 import org.opentrafficsim.core.network.LongitudinalDirectionality;
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 import org.opentrafficsim.simulationengine.SimpleSimulator;
55
56 import nl.tudelft.simulation.dsol.SimRuntimeException;
57 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
58
59
60
61
62
63
64
65
66
67
68
69
70 public class LaneBasedGTUTest implements UNITS
71 {
72
73 private OTSNetwork network = new OTSNetwork("leader follower parallel gtu test network");
74
75
76 private IdGenerator idGenerator = new IdGenerator("id");
77
78
79
80
81
82
83
84
85
86
87 private void leaderFollowerParallel(int truckFromLane, int truckUpToLane, int carLanesCovered) throws Exception
88 {
89
90 if (carLanesCovered < 1)
91 {
92 fail("carLanesCovered must be >= 1 (got " + carLanesCovered + ")");
93 }
94 if (truckUpToLane < truckFromLane)
95 {
96 fail("truckUpToLane must be >= truckFromLane");
97 }
98 OTSModelInterface model = new Model();
99 SimpleSimulator simulator = new SimpleSimulator(Time.ZERO, Duration.ZERO, 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 = new SimpleSimulator(Time.ZERO, Duration.ZERO, new Duration(3600.0, 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 Set<GTUType> compatibility = new HashSet<GTUType>();
391 compatibility.add(carType);
392 LaneType laneType = new LaneType("CarLane", compatibility);
393 OTSNode fromNode = new OTSNode(this.network, "Node A", new OTSPoint3D(0, 0, 0));
394 OTSNode toNode = new OTSNode(this.network, "Node B", new OTSPoint3D(1000, 0, 0));
395 String linkName = "AB";
396 Lane lane = LaneFactory.makeMultiLane(this.network, linkName, fromNode, toNode, null, 1, laneType,
397 new Speed(200, KM_PER_HOUR), simulator, LongitudinalDirectionality.DIR_PLUS)[0];
398 Length carPosition = new Length(100, METER);
399 Set<DirectedLanePosition> carPositions = new LinkedHashSet<>(1);
400 carPositions.add(new DirectedLanePosition(lane, carPosition, GTUDirectionality.DIR_PLUS));
401 Speed carSpeed = new Speed(10, METER_PER_SECOND);
402 Acceleration acceleration = new Acceleration(a, METER_PER_SECOND_2);
403 FixedAccelerationModel fam = new FixedAccelerationModel(acceleration, new Duration(10, SECOND));
404 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
405 Speed maximumSpeed = new Speed(200, KM_PER_HOUR);
406 BehavioralCharacteristics behavioralCharacteristics = DefaultTestParameters.create();
407
408 LaneBasedIndividualGTU car = new LaneBasedIndividualGTU("Car" + this.idGenerator.nextId(), carType,
409 new Length(4, METER), new Length(1.8, METER), maximumSpeed, simulator, this.network);
410 LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(behavioralCharacteristics,
411 new LaneBasedCFLCTacticalPlanner(fam, laneChangeModel, car), car);
412 car.init(strategicalPlanner, carPositions, carSpeed);
413
414 simulator.runUpTo(new Time(61, TimeUnit.BASE_SECOND));
415 while (simulator.isRunning())
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(Length totalLongitudinalPosition, Length gtuLength,
464 ArrayList<CrossSectionLink> links, int fromLaneRank, 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
525 class Model implements OTSModelInterface
526 {
527
528
529 private static final long serialVersionUID = 20150127L;
530
531
532 @Override
533 public void constructModel(SimulatorInterface<Time, Duration, OTSSimTimeDouble> simulator) throws SimRuntimeException
534 {
535
536 }
537
538
539 @Override
540 public SimulatorInterface<Time, Duration, OTSSimTimeDouble> getSimulator()
541
542 {
543 return null;
544 }
545
546
547 @Override
548 public OTSNetwork getNetwork()
549 {
550 return null;
551 }
552
553 }