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