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.core.perception.HistoryManagerDevs;
36 import org.opentrafficsim.road.DefaultTestParameters;
37 import org.opentrafficsim.road.definitions.DefaultsRoadNl;
38 import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
39 import org.opentrafficsim.road.gtu.lane.perception.categories.DefaultSimplePerception;
40 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
41 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCfLcTacticalPlanner;
42 import org.opentrafficsim.road.gtu.lane.tactical.following.FixedAccelerationModel;
43 import org.opentrafficsim.road.gtu.lane.tactical.following.GtuFollowingModelOld;
44 import org.opentrafficsim.road.gtu.lane.tactical.following.IdmPlusOld;
45 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.FixedLaneChangeModel;
46 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
47 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
48 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalRoutePlanner;
49 import org.opentrafficsim.road.network.RoadNetwork;
50 import org.opentrafficsim.road.network.factory.LaneFactory;
51 import org.opentrafficsim.road.network.lane.CrossSectionElement;
52 import org.opentrafficsim.road.network.lane.CrossSectionLink;
53 import org.opentrafficsim.road.network.lane.Lane;
54 import org.opentrafficsim.road.network.lane.LanePosition;
55 import org.opentrafficsim.road.network.lane.LaneType;
56
57 import nl.tudelft.simulation.dsol.SimRuntimeException;
58
59
60
61
62
63
64
65
66
67
68 public class LaneBasedGtuTest implements UNITS
69 {
70
71 private IdGenerator idGenerator = new IdGenerator("id");
72
73
74
75
76
77
78
79
80
81
82 private void leaderFollowerParallel(final int truckFromLane, final int truckUpToLane, final int carLanesCovered)
83 throws Exception
84 {
85
86 if (carLanesCovered < 1)
87 {
88 fail("carLanesCovered must be >= 1 (got " + carLanesCovered + ")");
89 }
90 if (truckUpToLane < truckFromLane)
91 {
92 fail("truckUpToLane must be >= truckFromLane");
93 }
94 OtsSimulatorInterface simulator = new OtsSimulator("leaderFollowerParallel");
95 RoadNetwork network = new RoadNetwork("leader follower parallel gtu test network", simulator);
96
97 Model model = new Model(simulator);
98 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model,
99 HistoryManagerDevs.noHistory(simulator));
100 GtuType carType = DefaultsNl.CAR;
101 GtuType truckType = DefaultsNl.TRUCK;
102 LaneType laneType = DefaultsRoadNl.TWO_WAY_LANE;
103
104 List<Node> nodes = new ArrayList<>();
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 Node(network, "Node at " + xPos, new Point2d(xPos, 20), Direction.ZERO));
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 Node fromNode = nodes.get(i - 1);
116 Node toNode = nodes.get(i);
117 String linkName = fromNode.getId() + "-" + toNode.getId();
118 Lane[] lanes = LaneFactory.makeMultiLane(network, linkName, fromNode, toNode, null, laneCount, laneType,
119 new Speed(100, KM_PER_HOUR), simulator, DefaultsNl.VEHICLE);
120 links.add(lanes[0].getLink());
121 }
122
123 Length truckPosition = new Length(99.5, METER);
124 Length truckLength = new Length(15, METER);
125
126 Set<LanePosition> truckPositions = buildPositionsSet(truckPosition, truckLength, links, truckFromLane, truckUpToLane);
127 Speed truckSpeed = new Speed(0, KM_PER_HOUR);
128 Length truckWidth = new Length(2.5, METER);
129 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
130 Speed maximumSpeed = new Speed(120, KM_PER_HOUR);
131 GtuFollowingModelOld gtuFollowingModel = new IdmPlusOld();
132 Parameters parameters = DefaultTestParameters.create();
133
134 LaneBasedGtu truck =
135 new LaneBasedGtu("Truck", truckType, truckLength, truckWidth, maximumSpeed, truckLength.times(0.5), network);
136 LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(
137 new LaneBasedCfLcTacticalPlanner(gtuFollowingModel, laneChangeModel, truck), truck);
138 truck.setParameters(parameters);
139 truck.init(strategicalPlanner, getReferencePosition(truckPositions), truckSpeed);
140
141 int lanesChecked = 0;
142 int found = 0;
143 for (CrossSectionLink link : links)
144 {
145 for (CrossSectionElement cse : link.getCrossSectionElementList())
146 {
147 if (cse instanceof Lane)
148 {
149 Lane lane = (Lane) cse;
150 boolean truckPositionsOnLane = false;
151 for (LanePosition pos : truckPositions)
152 {
153 if (pos.lane().equals(lane))
154 {
155 truckPositionsOnLane = true;
156 }
157 }
158 if (truckPositionsOnLane)
159 {
160 assertTrue(lane.getGtuList().contains(truck), "Truck should be registered on Lane " + lane);
161 found++;
162 }
163 else
164 {
165 assertFalse(lane.getGtuList().contains(truck), "Truck should NOT be registered on Lane " + lane);
166 }
167 lanesChecked++;
168 }
169 }
170 }
171
172 assertEquals(laneCount * links.size(), lanesChecked,
173 "lanesChecked should equals the number of Links times the number of lanes on each Link");
174 assertEquals(truckPositions.size(), found, "Truck should be registered in " + truckPositions.size() + " lanes");
175 Length forwardMaxDistance = truck.getParameters().getParameter(ParameterTypes.LOOKAHEAD);
176
177 truck.getTacticalPlanner().getPerception().perceive();
178 Headway leader = truck.getTacticalPlanner().getPerception().getPerceptionCategory(DefaultSimplePerception.class)
179 .getForwardHeadwayGtu();
180 assertTrue(forwardMaxDistance.getSI() >= leader.getDistance().si && leader.getDistance().si > 0,
181 "With one vehicle in the network forward headway should return a value larger than zero, and smaller than maxDistance");
182 assertEquals(null, leader.getId(), "With one vehicle in the network forward headwayGTU should return null");
183
184 Length reverseMaxDistance = truck.getParameters().getParameter(ParameterTypes.LOOKBACKOLD);
185 Headway follower = truck.getTacticalPlanner().getPerception().getPerceptionCategory(DefaultSimplePerception.class)
186 .getBackwardHeadway();
187 assertTrue(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, 0.1,
245 "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, 0.001,
275 "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, actualReverseHeadway, 0.001,
293 "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 HistoryManagerDevs.noHistory(simulator));
374
375 simulator.runUpTo(new Time(60, TimeUnit.BASE_SECOND));
376 while (simulator.isStartingOrRunning())
377 {
378 try
379 {
380 Thread.sleep(1);
381 }
382 catch (InterruptedException ie)
383 {
384 ie = null;
385 }
386 }
387 GtuType carType = DefaultsNl.CAR;
388 LaneType laneType = DefaultsRoadNl.TWO_WAY_LANE;
389 Node fromNode = new Node(network, "Node A", new Point2d(0, 0), Direction.ZERO);
390 Node toNode = new Node(network, "Node B", new Point2d(1000, 0), Direction.ZERO);
391 String linkName = "AB";
392 Lane lane = LaneFactory.makeMultiLane(network, linkName, fromNode, toNode, null, 1, laneType,
393 new Speed(200, KM_PER_HOUR), simulator, DefaultsNl.VEHICLE)[0];
394 Length carPosition = new Length(100, METER);
395 Set<LanePosition> carPositions = new LinkedHashSet<>(1);
396 carPositions.add(new LanePosition(lane, carPosition));
397 Speed carSpeed = new Speed(10, METER_PER_SECOND);
398 Acceleration acceleration = new Acceleration(a, METER_PER_SECOND_2);
399 FixedAccelerationModel fam = new FixedAccelerationModel(acceleration, new Duration(10, SECOND));
400 LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null);
401 Speed maximumSpeed = new Speed(200, KM_PER_HOUR);
402 Parameters parameters = DefaultTestParameters.create();
403
404 LaneBasedGtu car = new LaneBasedGtu("Car" + this.idGenerator.get(), carType, new Length(4, METER),
405 new Length(1.8, METER), maximumSpeed, Length.instantiateSI(2.0), network);
406 LaneBasedStrategicalPlanner strategicalPlanner =
407 new LaneBasedStrategicalRoutePlanner(new LaneBasedCfLcTacticalPlanner(fam, laneChangeModel, car), car);
408 car.setParameters(parameters);
409 car.init(strategicalPlanner, getReferencePosition(carPositions), carSpeed);
410
411 simulator.runUpTo(new Time(61, TimeUnit.BASE_SECOND));
412 while (simulator.isStartingOrRunning())
413 {
414 try
415 {
416 Thread.sleep(1);
417 }
418 catch (InterruptedException ie)
419 {
420 ie = null;
421 }
422 }
423
424
425
426 for (int timeStep = 1; timeStep < 100; timeStep++)
427 {
428 double deltaTime = 0.1 * timeStep;
429 double distanceAtTime = carSpeed.getSI() * deltaTime + 0.5 * acceleration.getSI() * deltaTime * deltaTime;
430
431
432
433
434
435
436
437
438
439 }
440 }
441 }
442
443
444
445
446 public final void autoPauseSimulator()
447 {
448
449 }
450
451
452
453
454
455
456
457
458
459
460 private Set<LanePosition> buildPositionsSet(final Length totalLongitudinalPosition, final Length gtuLength,
461 final ArrayList<CrossSectionLink> links, final int fromLaneRank, final int uptoLaneRank)
462 {
463 Set<LanePosition> result = new LinkedHashSet<>(1);
464 double cumulativeLength = 0;
465 for (CrossSectionLink link : links)
466 {
467 double linkLength = link.getLength().getSI();
468 double frontPositionInLink = totalLongitudinalPosition.getSI() - cumulativeLength + gtuLength.getSI();
469 double rearPositionInLink = frontPositionInLink - gtuLength.getSI();
470 double midPositionInLink = frontPositionInLink - gtuLength.getSI() / 2.0;
471
472
473
474 if (rearPositionInLink < linkLength && frontPositionInLink >= 0)
475 {
476
477 for (int laneRank = fromLaneRank; laneRank <= uptoLaneRank; laneRank++)
478 {
479 Lane lane = getNthLane(link, laneRank);
480 if (null == lane)
481 {
482 fail("Error in test; canot find lane with rank " + laneRank);
483 }
484 result.add(new LanePosition(lane, new Length(midPositionInLink, METER)));
485 }
486 }
487 cumulativeLength += linkLength;
488 }
489 return result;
490 }
491
492
493
494
495
496
497 private LanePosition getReferencePosition(final Set<LanePosition> positions)
498 {
499 for (LanePosition lanePosition : positions)
500 {
501 if (lanePosition.position().gt0() && lanePosition.position().le(lanePosition.lane().getLength()))
502 {
503 return lanePosition;
504 }
505 }
506 throw new NoSuchElementException("Reference point is not on any of the given lanes.");
507 }
508
509
510
511
512
513
514
515 private Lane getNthLane(final CrossSectionLink link, int rank)
516 {
517 for (CrossSectionElement cse : link.getCrossSectionElementList())
518 {
519 if (cse instanceof Lane)
520 {
521 if (0 == rank--)
522 {
523 return (Lane) cse;
524 }
525 }
526 }
527 return null;
528 }
529
530
531 public static class Model extends AbstractOtsModel
532 {
533
534
535
536 public Model(final OtsSimulatorInterface simulator)
537 {
538 super(simulator);
539 }
540
541
542 private static final long serialVersionUID = 20141027L;
543
544 @Override
545 public final void constructModel() throws SimRuntimeException
546 {
547
548 }
549
550 @Override
551 public final RoadNetwork getNetwork()
552 {
553 return null;
554 }
555 }
556 }