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