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