package org.opentrafficsim.road.gtu; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertFalse; import static org.junit.Assert.assertTrue; import static org.junit.Assert.fail; import java.util.ArrayList; import java.util.LinkedHashSet; import java.util.Set; import nl.tudelft.simulation.dsol.SimRuntimeException; import nl.tudelft.simulation.dsol.simulators.SimulatorInterface; import org.djunits.unit.TimeUnit; import org.djunits.unit.UNITS; import org.djunits.value.vdouble.scalar.Acceleration; import org.djunits.value.vdouble.scalar.DoubleScalar; import org.djunits.value.vdouble.scalar.Length; import org.djunits.value.vdouble.scalar.Speed; import org.djunits.value.vdouble.scalar.Time; import org.junit.Test; import org.opentrafficsim.core.dsol.OTSModelInterface; import org.opentrafficsim.core.dsol.OTSSimTimeDouble; import org.opentrafficsim.core.geometry.OTSPoint3D; import org.opentrafficsim.core.gtu.GTUDirectionality; import org.opentrafficsim.core.gtu.GTUType; import org.opentrafficsim.core.network.LongitudinalDirectionality; import org.opentrafficsim.core.network.OTSNetwork; import org.opentrafficsim.core.network.OTSNode; import org.opentrafficsim.road.car.LaneBasedIndividualCar; import org.opentrafficsim.road.gtu.lane.LaneBasedGTU; import org.opentrafficsim.road.gtu.lane.driver.LaneBasedDrivingCharacteristics; import org.opentrafficsim.road.gtu.lane.perception.LanePerceptionFull; import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner; import org.opentrafficsim.road.gtu.lane.tactical.following.FixedAccelerationModel; import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModel; import org.opentrafficsim.road.gtu.lane.tactical.following.HeadwayGTU; import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlus; import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.FixedLaneChangeModel; import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel; import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner; import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner; import org.opentrafficsim.road.network.factory.LaneFactory; import org.opentrafficsim.road.network.lane.CrossSectionElement; import org.opentrafficsim.road.network.lane.CrossSectionLink; import org.opentrafficsim.road.network.lane.DirectedLanePosition; import org.opentrafficsim.road.network.lane.Lane; import org.opentrafficsim.road.network.lane.LaneType; import org.opentrafficsim.simulationengine.SimpleSimulator; /** * Test the LaneBasedGTU class. *

* Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* BSD-style license. See OpenTrafficSim License. *

* $LastChangedDate: 2015-09-14 01:33:02 +0200 (Mon, 14 Sep 2015) $, @version $Revision: 1401 $, by $Author: averbraeck $, * initial version 27 jan. 2015
* @author Alexander Verbraeck * @author Peter Knoppers */ public class LaneBasedGTUTest implements UNITS { /** network. */ private OTSNetwork network = new OTSNetwork("network"); /** * Test if a Truck covering a specified range of lanes can see a Car covering a specified range of lanes.
* The network is a linear array of Nodes connected by 5-Lane Links. In the middle, the Nodes are very closely spaced. A * truck is positioned over those center Nodes ensuring it covers several of the short Lanes in succession. * @param truckFromLane int; lowest rank of lane range of the truck * @param truckUpToLane int; highest rank of lane range of the truck * @param carLanesCovered int; number of lanes that the car covers * @throws Exception when something goes wrong (should not happen) */ private void leaderFollowerParallel(int truckFromLane, int truckUpToLane, int carLanesCovered) throws Exception { // Perform a few sanity checks if (carLanesCovered < 1) { fail("carLanesCovered must be >= 1 (got " + carLanesCovered + ")"); } if (truckUpToLane < truckFromLane) { fail("truckUpToLane must be >= truckFromLane"); } OTSModelInterface model = new Model(); SimpleSimulator simulator = new SimpleSimulator(new Time.Abs(0.0, SECOND), new Time.Rel(0.0, SECOND), new Time.Rel(3600.0, SECOND), model); GTUType carType = GTUType.makeGTUType("car"); GTUType truckType = GTUType.makeGTUType("truck"); LaneType laneType = new LaneType("CarLane"); laneType.addCompatibility(carType); laneType.addCompatibility(truckType); // Create a series of Nodes (some closely bunched together) ArrayList nodes = new ArrayList(); int[] linkBoundaries = {0, 25, 50, 100, 101, 102, 103, 104, 105, 150, 175, 200}; for (int xPos : linkBoundaries) { nodes.add(new OTSNode("Node at " + xPos, new OTSPoint3D(xPos, 20, 0))); } // Now we can build a series of Links with Lanes on them ArrayList links = new ArrayList(); final int laneCount = 5; for (int i = 1; i < nodes.size(); i++) { OTSNode fromNode = nodes.get(i - 1); OTSNode toNode = nodes.get(i); String linkName = fromNode.getId() + "-" + toNode.getId(); Lane[] lanes = LaneFactory.makeMultiLane(linkName, fromNode, toNode, null, laneCount, laneType, new Speed(100, KM_PER_HOUR), simulator, LongitudinalDirectionality.DIR_PLUS); links.add(lanes[0].getParentLink()); } // Create a long truck with its front (reference) one meter in the last link on the 3rd lane Length.Rel truckPosition = new Length.Rel(99.5, METER); Length.Rel truckLength = new Length.Rel(15, METER); Set truckPositions = buildPositionsSet(truckPosition, truckLength, links, truckFromLane, truckUpToLane); Speed truckSpeed = new Speed(0, KM_PER_HOUR); Length.Rel truckWidth = new Length.Rel(2.5, METER); LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null); Speed maximumVelocity = new Speed(120, KM_PER_HOUR); GTUFollowingModel gtuFollowingModel = new IDMPlus(); LaneBasedDrivingCharacteristics drivingCharacteristics = new LaneBasedDrivingCharacteristics(gtuFollowingModel, laneChangeModel); LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(drivingCharacteristics, new LaneBasedCFLCTacticalPlanner()); LaneBasedIndividualCar truck = new LaneBasedIndividualCar("Truck", truckType, truckPositions, truckSpeed, truckLength, truckWidth, maximumVelocity, simulator, strategicalPlanner, new LanePerceptionFull(), this.network); // Verify that the truck is registered on the correct Lanes int lanesChecked = 0; int found = 0; for (CrossSectionLink link : links) { for (CrossSectionElement cse : link.getCrossSectionElementList()) { if (cse instanceof Lane) { Lane lane = (Lane) cse; boolean truckPositionsOnLane = false; for (DirectedLanePosition pos : truckPositions) { if (pos.getLane().equals(lane)) { truckPositionsOnLane = true; } } if (truckPositionsOnLane) { assertTrue("Truck should be registered on Lane " + lane, lane.getGtuList().contains(truck)); found++; } else { assertFalse("Truck should NOT be registered on Lane " + lane, lane.getGtuList().contains(truck)); } lanesChecked++; } } } // Make sure we tested them all assertEquals("lanesChecked should equals the number of Links times the number of lanes on each Link", laneCount * links.size(), lanesChecked); assertEquals("Truck should be registered in " + truckPositions.size() + " lanes", truckPositions.size(), found); Length.Rel forwardMaxDistance = truck.getDrivingCharacteristics().getForwardHeadwayDistance(); // TODO see how we can ask the vehicle to look this far ahead truck.getPerception().perceive(); HeadwayGTU leader = truck.getPerception().getForwardHeadwayGTU(); assertTrue( "With one vehicle in the network forward headway should return a value larger than zero, and smaller than maxDistance", forwardMaxDistance.getSI() >= leader.getDistance().si && leader.getDistance().si > 0); assertEquals("With one vehicle in the network forward headwayGTU should return null", null, leader.getGtuId()); // TODO see how we can ask the vehicle to look this far behind Length.Rel reverseMaxDistance = truck.getDrivingCharacteristics().getBackwardHeadwayDistance(); HeadwayGTU follower = truck.getPerception().getBackwardHeadwayGTU(); assertTrue( "With one vehicle in the network reverse headway should return a value less than zero, and smaller than |maxDistance|", Math.abs(reverseMaxDistance.getSI()) >= Math.abs(follower.getDistance().si) && follower.getDistance().si < 0); assertEquals("With one vehicle in the network reverse headwayGTU should return null", null, follower.getGtuId()); Length.Rel carLength = new Length.Rel(4, METER); Length.Rel carWidth = new Length.Rel(1.8, METER); Speed carSpeed = new Speed(0, KM_PER_HOUR); int maxStep = linkBoundaries[linkBoundaries.length - 1]; for (int laneRank = 0; laneRank < laneCount + 1 - carLanesCovered; laneRank++) { for (int step = 0; step < maxStep; step += 5) { if (laneRank >= truckFromLane && laneRank <= truckUpToLane && step >= truckPosition.getSI() - truckLength.getSI() && step - carLength.getSI() <= truckPosition.getSI()) { continue; // Truck and car would overlap; the result of that placement is not defined :-) } Length.Rel carPosition = new Length.Rel(step, METER); Set carPositions = buildPositionsSet(carPosition, carLength, links, laneRank, laneRank + carLanesCovered - 1); drivingCharacteristics = new LaneBasedDrivingCharacteristics(gtuFollowingModel, laneChangeModel); strategicalPlanner = new LaneBasedStrategicalRoutePlanner(drivingCharacteristics, new LaneBasedCFLCTacticalPlanner()); LaneBasedIndividualCar car = new LaneBasedIndividualCar("Car", carType, carPositions, carSpeed, carLength, carWidth, maximumVelocity, simulator, strategicalPlanner, new LanePerceptionFull(), this.network); // leader = truck.headway(forwardMaxDistance); // TODO see how we can ask the vehicle to look 'forwardMaxDistance' ahead leader = truck.getPerception().getForwardHeadwayGTU(); double actualHeadway = leader.getDistance().si; double expectedHeadway = laneRank + carLanesCovered - 1 < truckFromLane || laneRank > truckUpToLane || step - truckPosition.getSI() - truckLength.getSI() <= 0 ? Double.MAX_VALUE : step - truckLength.getSI() - truckPosition.getSI(); // System.out.println("carLanesCovered " + laneRank + ".." + (laneRank + carLanesCovered - 1) // + " truckLanesCovered " + truckFromLane + ".." + truckUpToLane + " car pos " + step // + " laneRank " + laneRank + " expected headway " + expectedHeadway); // The next assert found a subtle bug (">" instead of ">=") assertEquals("Forward headway should return " + expectedHeadway, expectedHeadway, actualHeadway, 0.1); String leaderGtuId = leader.getGtuId(); if (expectedHeadway == Double.MAX_VALUE) { assertEquals("Leader id should be null", null, leaderGtuId); } else { assertEquals("Leader id should be the car id", car, leaderGtuId); } // TODO follower = truck.headway(reverseMaxDistance); follower = truck.getPerception().getBackwardHeadwayGTU(); double actualReverseHeadway = follower.getDistance().si; double expectedReverseHeadway = laneRank + carLanesCovered - 1 < truckFromLane || laneRank > truckUpToLane || step + carLength.getSI() >= truckPosition.getSI() ? Double.MAX_VALUE : truckPosition.getSI() - carLength.getSI() - step; assertEquals("Reverse headway should return " + expectedReverseHeadway, expectedReverseHeadway, actualReverseHeadway, 0.1); String followerGtuId = follower.getGtuId(); if (expectedReverseHeadway == Double.MAX_VALUE) { assertEquals("Follower id should be null", null, followerGtuId); } else { assertEquals("Follower id should be the car id", car.getId(), followerGtuId); } for (int laneIndex = 0; laneIndex < laneCount; laneIndex++) { Lane l = null; double cumulativeDistance = 0; for (CrossSectionLink csl : links) { cumulativeDistance += csl.getLength().getSI(); if (cumulativeDistance >= truckPosition.getSI()) { l = getNthLane(csl, laneIndex); break; } } leader = truck.getPerception().getForwardHeadwayGTU(); actualHeadway = leader.getDistance().si; expectedHeadway = laneIndex < laneRank || laneIndex > laneRank + carLanesCovered - 1 || step - truckLength.getSI() - truckPosition.getSI() <= 0 ? Double.MAX_VALUE : step - truckLength.getSI() - truckPosition.getSI(); assertEquals("Headway on lane " + laneIndex + " should be " + expectedHeadway, expectedHeadway, actualHeadway, 0.001); leaderGtuId = leader.getGtuId(); if (laneIndex >= laneRank && laneIndex <= laneRank + carLanesCovered - 1 && step - truckLength.getSI() - truckPosition.getSI() > 0) { assertEquals("Leader id should be the car id", car.getId(), leaderGtuId); } else { assertEquals("Leader id should be null", null, leaderGtuId); } follower = truck.getPerception().getBackwardHeadwayGTU(); actualReverseHeadway = follower.getDistance().si; expectedReverseHeadway = laneIndex < laneRank || laneIndex > laneRank + carLanesCovered - 1 || step + carLength.getSI() >= truckPosition.getSI() ? Double.MAX_VALUE : truckPosition .getSI() - carLength.getSI() - step; assertEquals("Headway on lane " + laneIndex + " should be " + expectedReverseHeadway, expectedReverseHeadway, actualReverseHeadway, 0.001); followerGtuId = follower.getGtuId(); if (laneIndex >= laneRank && laneIndex <= laneRank + carLanesCovered - 1 && step + carLength.getSI() < truckPosition.getSI()) { assertEquals("Follower id should be the car id", car, followerGtuId); } else { assertEquals("Follower id should be null", null, followerGtuId); } } Set leftParallel = truck.getPerception().getParallelGTUsLeft(); int expectedLeftSize = laneRank + carLanesCovered - 1 < truckFromLane - 1 || laneRank >= truckUpToLane || step + carLength.getSI() <= truckPosition.getSI() || step > truckPosition.getSI() + truckLength.getSI() ? 0 : 1; // This one caught a complex bug assertEquals("Left parallel set size should be " + expectedLeftSize, expectedLeftSize, leftParallel.size()); if (leftParallel.size() > 0) { assertTrue("Parallel GTU should be the car", leftParallel.contains(car)); } Set rightParallel = truck.getPerception().getParallelGTUsRight(); int expectedRightSize = laneRank + carLanesCovered - 1 <= truckFromLane || laneRank > truckUpToLane + 1 || step + carLength.getSI() < truckPosition.getSI() || step > truckPosition.getSI() + truckLength.getSI() ? 0 : 1; assertEquals("Right parallel set size should be " + expectedRightSize, expectedRightSize, rightParallel.size()); if (rightParallel.size() > 0) { assertTrue("Parallel GTU should be the car", rightParallel.contains(car)); } for (DirectedLanePosition pos : carPositions) { pos.getLane().removeGTU(car); } } } } /** * Test the leader, follower and parallel methods. * @throws Exception when something goes wrong (should not happen) */ @Test public void leaderFollowerAndParallelTest() throws Exception { // TODO leaderFollowerParallel(2, 2, 1); // TODO leaderFollowerParallel(2, 3, 1); // TODO leaderFollowerParallel(2, 2, 2); // TODO leaderFollowerParallel(2, 3, 2); } /** * Test the deltaTimeForDistance and timeAtDistance methods. * @throws Exception when something goes wrong (should not happen) */ @Test public void timeAtDistanceTest() throws Exception { for (int a = 1; a >= -1; a--) { // Create a car with constant acceleration OTSModelInterface model = new Model(); SimpleSimulator simulator = new SimpleSimulator(new Time.Abs(0.0, SECOND), new Time.Rel(0.0, SECOND), new Time.Rel(3600.0, SECOND), model); // Run the simulator clock to some non-zero value simulator.runUpTo(new Time.Abs(60, SECOND)); while (simulator.isRunning()) { try { Thread.sleep(1); } catch (InterruptedException ie) { ie = null; // ignore } } GTUType carType = GTUType.makeGTUType("car"); LaneType laneType = new LaneType("CarLane"); laneType.addCompatibility(carType); OTSNode fromNode = new OTSNode("Node A", new OTSPoint3D(0, 0, 0)); OTSNode toNode = new OTSNode("Node B", new OTSPoint3D(1000, 0, 0)); String linkName = "AB"; Lane lane = LaneFactory.makeMultiLane(linkName, fromNode, toNode, null, 1, laneType, new Speed(200, KM_PER_HOUR), simulator, LongitudinalDirectionality.DIR_PLUS)[0]; Length.Rel carPosition = new Length.Rel(100, METER); Set carPositions = new LinkedHashSet<>(1); carPositions.add(new DirectedLanePosition(lane, carPosition, GTUDirectionality.DIR_PLUS)); Speed carSpeed = new Speed(10, METER_PER_SECOND); Acceleration acceleration = new Acceleration(a, METER_PER_SECOND_2); FixedAccelerationModel fam = new FixedAccelerationModel(acceleration, new Time.Rel(10, SECOND)); LaneChangeModel laneChangeModel = new FixedLaneChangeModel(null); Speed maximumVelocity = new Speed(200, KM_PER_HOUR); LaneBasedDrivingCharacteristics drivingCharacteristics = new LaneBasedDrivingCharacteristics(fam, laneChangeModel); LaneBasedStrategicalPlanner strategicalPlanner = new LaneBasedStrategicalRoutePlanner(drivingCharacteristics, new LaneBasedCFLCTacticalPlanner()); LaneBasedIndividualCar car = new LaneBasedIndividualCar("Car", carType, carPositions, carSpeed, new Length.Rel(4, METER), new Length.Rel(1.8, METER), maximumVelocity, simulator, strategicalPlanner, new LanePerceptionFull(), this.network); // Let the simulator execute the move method of the car simulator.runUpTo(new Time.Abs(61, SECOND)); while (simulator.isRunning()) { try { Thread.sleep(1); } catch (InterruptedException ie) { ie = null; // ignore } } // System.out.println("acceleration is " + acceleration); // Check the results for (int timeStep = 1; timeStep < 100; timeStep++) { double deltaTime = 0.1 * timeStep; double distanceAtTime = carSpeed.getSI() * deltaTime + 0.5 * acceleration.getSI() * deltaTime * deltaTime; // System.out.println(String.format("time %.1fs, distance %.3fm", 60 + deltaTime, carPosition.getSI() // + distanceAtTime)); // System.out.println("Expected differential distance " + distanceAtTime); /*- * TODO assertEquals("It should take " + deltaTime + " seconds to cover distance " + distanceAtTime, deltaTime, * car.deltaTimeForDistance(new Length.Rel(distanceAtTime, METER)).getSI(), 0.0001); * TODO assertEquals("Car should reach distance " + distanceAtTime + " at " + (deltaTime + 60), deltaTime + 60, * car.timeAtDistance(new Length.Rel(distanceAtTime, METER)).getSI(), 0.0001); */ } } } /** * Executed as scheduled event. */ public final void autoPauseSimulator() { // do nothing } /** * Create the Map that records in which lane a GTU is registered. * @param totalLongitudinalPosition Length.Rel; the front position of the GTU from the start of the chain of Links * @param gtuLength Length.Rel; the length of the GTU * @param links ArrayList<CrossSectionLink<?,?>>; the list of Links * @param fromLaneRank int; lowest rank of lanes that the GTU must be registered on (0-based) * @param uptoLaneRank int; highest rank of lanes that the GTU must be registered on (0-based) * @return the Set of the LanePositions that the GTU is registered on */ private Set buildPositionsSet(Length.Rel totalLongitudinalPosition, Length.Rel gtuLength, ArrayList links, int fromLaneRank, int uptoLaneRank) { Set result = new LinkedHashSet<>(1); double cumulativeLength = 0; for (CrossSectionLink link : links) { double linkLength = link.getLength().getSI(); double frontPositionInLink = totalLongitudinalPosition.getSI() - cumulativeLength + gtuLength.getSI(); double rearPositionInLink = frontPositionInLink - gtuLength.getSI(); // double linkEnd = cumulativeLength + linkLength; // System.out.println("cumulativeLength: " + cumulativeLength + ", linkEnd: " + linkEnd + ", frontpos: " // + frontPositionInLink + ", rearpos: " + rearPositionInLink); if (rearPositionInLink < linkLength && frontPositionInLink >= 0) { // Some part of the GTU is in this Link for (int laneRank = fromLaneRank; laneRank <= uptoLaneRank; laneRank++) { Lane lane = getNthLane(link, laneRank); if (null == lane) { fail("Error in test; canot find lane with rank " + laneRank); } result.add(new DirectedLanePosition(lane, new Length.Rel(rearPositionInLink, METER), GTUDirectionality.DIR_PLUS)); } } cumulativeLength += linkLength; } return result; } /** * Find the Nth Lane on a Link. * @param link Link; the Link * @param rank int; the zero-based rank of the Lane to return * @return Lane */ private Lane getNthLane(final CrossSectionLink link, int rank) { for (CrossSectionElement cse : link.getCrossSectionElementList()) { if (cse instanceof Lane) { if (0 == rank--) { return (Lane) cse; } } } return null; } } /** */ class Model implements OTSModelInterface { /** */ private static final long serialVersionUID = 20150127L; /** {@inheritDoc} */ @Override public void constructModel( SimulatorInterface, DoubleScalar.Rel, OTSSimTimeDouble> simulator) throws SimRuntimeException { // Dummy } /** {@inheritDoc} */ @Override public SimulatorInterface, DoubleScalar.Rel, OTSSimTimeDouble> getSimulator() { return null; } }