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