From f55cd9ef758216aa8eaf7fe7d705c0a5a76329bf Mon Sep 17 00:00:00 2001 From: Duc Pham Date: Fri, 6 Aug 2021 20:16:00 +0700 Subject: [PATCH] Change how leaders/followers are found in `advanced_driving` skill (#3160) --- msi.gama.ext/META-INF/MANIFEST.MF | 2 + msi.gama.ext/pom2.xml | 2 +- simtools.gaml.extensions.traffic/.classpath | 9 +- .../META-INF/MANIFEST.MF | 1 + .../Driving Skill/models/Drive Random.gaml | 2 +- .../models/Driving Skill/models/Traffic.gaml | 4 +- .../extensions/traffic/DrivingOperators.java | 44 +-- .../gaml/extensions/traffic/DrivingSkill.java | 111 ++++--- .../extensions/traffic/RoadNodeSkill.java | 12 +- .../gaml/extensions/traffic/RoadSkill.java | 206 +++++------- .../gaml/extensions/traffic/Utils.java | 305 ------------------ .../carfollowing/CustomDualTreeBidiMap.java | 40 +++ .../traffic/carfollowing/Utils.java | 270 ++++++++++++++++ .../extensions/traffic/lanechange/MOBIL.java | 71 ++-- 14 files changed, 551 insertions(+), 528 deletions(-) delete mode 100644 simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/Utils.java create mode 100644 simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/CustomDualTreeBidiMap.java create mode 100644 simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/Utils.java diff --git a/msi.gama.ext/META-INF/MANIFEST.MF b/msi.gama.ext/META-INF/MANIFEST.MF index fa7e394eaa..2ab769ccbd 100644 --- a/msi.gama.ext/META-INF/MANIFEST.MF +++ b/msi.gama.ext/META-INF/MANIFEST.MF @@ -171,6 +171,8 @@ Export-Package: com.conversantmedia.util.collection, net.sourceforge.jtds.jdbcx, net.sourceforge.jtds.jdbcx.proxy, one.util.streamex, + org.apache.commons.collections4, + org.apache.commons.collections4.bidimap, org.apache.commons.dbcp; uses:="javax.naming.spi, org.apache.commons.pool.impl, diff --git a/msi.gama.ext/pom2.xml b/msi.gama.ext/pom2.xml index 699e723489..b124bf83c1 100644 --- a/msi.gama.ext/pom2.xml +++ b/msi.gama.ext/pom2.xml @@ -44,7 +44,7 @@ commons-collections commons-collections - 3.1 + 4.4 commons-dbcp diff --git a/simtools.gaml.extensions.traffic/.classpath b/simtools.gaml.extensions.traffic/.classpath index ddf901b473..7f70ca801b 100644 --- a/simtools.gaml.extensions.traffic/.classpath +++ b/simtools.gaml.extensions.traffic/.classpath @@ -1,12 +1,17 @@ - - + + + + + + + diff --git a/simtools.gaml.extensions.traffic/META-INF/MANIFEST.MF b/simtools.gaml.extensions.traffic/META-INF/MANIFEST.MF index 5ba48313bc..d6bc44336a 100644 --- a/simtools.gaml.extensions.traffic/META-INF/MANIFEST.MF +++ b/simtools.gaml.extensions.traffic/META-INF/MANIFEST.MF @@ -11,3 +11,4 @@ Bundle-ClassPath: . Bundle-ActivationPolicy: lazy Bundle-RequiredExecutionEnvironment: JavaSE-11 Automatic-Module-Name: simtools.gaml.extensions.traffic +Class-Path: ../msi.gama.ext/geotools/commons-collections4-4.4.jar diff --git a/simtools.gaml.extensions.traffic/models/Driving Skill/models/Drive Random.gaml b/simtools.gaml.extensions.traffic/models/Driving Skill/models/Drive Random.gaml index 7831fc1ecb..028464593b 100644 --- a/simtools.gaml.extensions.traffic/models/Driving Skill/models/Drive Random.gaml +++ b/simtools.gaml.extensions.traffic/models/Driving Skill/models/Drive Random.gaml @@ -11,7 +11,7 @@ import "Traffic.gaml" global { float traffic_light_interval parameter: 'Traffic light interval' init: 60#s; - float step <- 0.1#s; + float step <- 0.2#s; graph full_road_graph; string map_name; diff --git a/simtools.gaml.extensions.traffic/models/Driving Skill/models/Traffic.gaml b/simtools.gaml.extensions.traffic/models/Driving Skill/models/Traffic.gaml index 2e248a1fd3..f2dfea160f 100644 --- a/simtools.gaml.extensions.traffic/models/Driving Skill/models/Traffic.gaml +++ b/simtools.gaml.extensions.traffic/models/Driving Skill/models/Traffic.gaml @@ -18,7 +18,7 @@ species road skills: [skill_road] { string oneway; aspect base { - draw shape color: color end_arrow: 2; + draw shape color: color end_arrow: 1; } } @@ -107,7 +107,7 @@ species base_vehicle skills: [advanced_driving] { // Shifts the position of the vehicle perpendicularly to the road, // in order to visualize different lanes if (current_road != nil) { - float dist <- (road(current_road).lanes - current_lane - + float dist <- (road(current_road).num_lanes - current_lane - mean(range(num_lanes_occupied - 1)) - 0.5) * lane_width; if violating_oneway { dist <- -dist; diff --git a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingOperators.java b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingOperators.java index 7beed69ea6..387a67f24e 100644 --- a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingOperators.java +++ b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingOperators.java @@ -13,11 +13,13 @@ **********************************************************************************************/ package simtools.gaml.extensions.traffic; +import java.util.Collections; +import java.util.Comparator; +import java.util.LinkedList; import java.util.List; -import org.locationtech.jts.geom.Coordinate; +import org.apache.commons.collections4.OrderedBidiMap; -import msi.gama.common.geometry.GeometryUtils; import msi.gama.metamodel.agent.IAgent; import msi.gama.metamodel.shape.IShape; import msi.gama.metamodel.topology.graph.GamaSpatialGraph; @@ -28,11 +30,9 @@ import msi.gama.precompiler.IConcept; import msi.gama.precompiler.ITypeProvider; import msi.gama.runtime.IScope; -import msi.gama.util.GamaListFactory; import msi.gama.util.IContainer; -import msi.gama.util.IList; import msi.gama.util.graph.IGraph; -import msi.gaml.types.Types; +import simtools.gaml.extensions.traffic.carfollowing.CustomDualTreeBidiMap; @SuppressWarnings({ "unchecked", "rawtypes" }) public class DrivingOperators { @@ -47,23 +47,27 @@ public static IGraph spatialDrivingFromEdges(final IScope scope, final IContaine final IGraph graph = new GamaSpatialGraph(edges, nodes, scope); for (final Object edge : edges.iterable(scope)) { if (edge instanceof IShape) { - final IAgent ag = ((IShape) edge).getAgent(); - if (ag.hasAttribute(RoadSkill.NUM_LANES) && ag.hasAttribute(RoadSkill.AGENTS_ON)) { - final int numLanes = RoadSkill.getNumLanes(ag); - if (numLanes > 0) { - final IList agentsOn = (IList) ag.getAttribute(RoadSkill.AGENTS_ON); - for (int i = 0; i < numLanes; i++) { - final int nbSeg = ag.getInnerGeometry().getNumPoints() - 1; - final IList lisSg = GamaListFactory.create(Types.NO_TYPE); - for (int j = 0; j < nbSeg; j++) { - lisSg.add(GamaListFactory.create(Types.NO_TYPE)); + IAgent agent = ((IShape) edge).getAgent(); + int numLanes = RoadSkill.getNumLanes(agent); + List> res = new LinkedList<>(); + //TODO: this should be tied to the setter of numLanes + for (int i = 0; i < numLanes; i += 1) { + res.add( + new CustomDualTreeBidiMap(new Comparator() { + @Override + public int compare(IAgent a, IAgent b) { + int r = a.getSpeciesName().compareTo(b.getSpeciesName()); + if (r != 0) { + return r; + } else { + return Integer.compare(a.getIndex(), b.getIndex()); + } } - agentsOn.add(lisSg); - } - } - ag.setAttribute(RoadSkill.ALL_AGENTS, GamaListFactory.create(Types.NO_TYPE)); + }, + Collections.reverseOrder()) + ); } - + RoadSkill.setVehicleOrdering(agent, res); } } return graph; diff --git a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingSkill.java b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingSkill.java index 79ed2fdeda..a2c2cf4eb0 100644 --- a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingSkill.java +++ b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingSkill.java @@ -19,8 +19,6 @@ import java.util.stream.Collectors; import java.util.stream.IntStream; -import com.google.common.collect.Sets; - import org.apache.commons.lang3.tuple.Pair; import org.locationtech.jts.geom.Coordinate; @@ -28,7 +26,6 @@ import msi.gama.metamodel.agent.AbstractAgent; import msi.gama.metamodel.agent.IAgent; import msi.gama.metamodel.shape.GamaPoint; - import msi.gama.metamodel.shape.IShape; import msi.gama.metamodel.topology.graph.GamaSpatialGraph; import msi.gama.precompiler.GamlAnnotations.action; @@ -371,7 +368,13 @@ name = DrivingSkill.DISTANCE_TO_GOAL, type = IType.FLOAT, init = "0.0", - doc = @doc("euclidean distance to the next point of the current segment") + doc = @doc("euclidean distance to the endpoint of the current segment") + ), + @variable( + name = DrivingSkill.DISTANCE_TO_CURRENT_TARGET, + type = IType.FLOAT, + init = "0.0", + doc = @doc("euclidean distance to the current target node") ), @variable( name = DrivingSkill.SEGMENT_INDEX, @@ -398,6 +401,12 @@ type = IType.FLOAT, init = "nil", doc = @doc("the speed of the leading vehicle") + ), + @variable( + name = DrivingSkill.FOLLOWER, + type = IType.AGENT, + init = "nil", + doc = @doc("the vehicle following this vehicle") ) }) @skill( @@ -422,6 +431,7 @@ public class DrivingSkill extends MovingSkill { @Deprecated public static final String CURRENT_LANE = "current_lane"; public static final String LOWEST_LANE = "lowest_lane"; public static final String DISTANCE_TO_GOAL = "distance_to_goal"; + public static final String DISTANCE_TO_CURRENT_TARGET = "distance_to_current_target"; public static final String VEHICLE_LENGTH = "vehicle_length"; @Deprecated public static final String PROBA_LANE_CHANGE_UP = "proba_lane_change_up"; @Deprecated public static final String PROBA_LANE_CHANGE_DOWN = "proba_lane_change_down"; @@ -460,6 +470,7 @@ public class DrivingSkill extends MovingSkill { public static final String LEADING_VEHICLE = "leading_vehicle"; public static final String LEADING_DISTANCE = "leading_distance"; public static final String LEADING_SPEED = "leading_speed"; + public static final String FOLLOWER = "follower"; // NOTE: Due to approximations in IDM, vehicles will never have the exact same location as its target. // Therefore we consider the vehicle has reached its goal when distToGoal is smaller than this threshold. @@ -868,6 +879,16 @@ public static double getDistanceToGoal(final IAgent vehicle) { return (Double) vehicle.getAttribute(DISTANCE_TO_GOAL); } + @getter(DISTANCE_TO_CURRENT_TARGET) + public static double getDistanceToCurrentTarget(final IAgent vehicle) { + return (Double) vehicle.getAttribute(DISTANCE_TO_CURRENT_TARGET); + } + + @setter(DISTANCE_TO_CURRENT_TARGET) + public static void setDistanceToCurrentTarget(final IAgent vehicle, final double dist) { + vehicle.setAttribute(DISTANCE_TO_CURRENT_TARGET, dist); + } + @getter(MIN_SECURITY_DISTANCE) public static double getMinSecurityDistance(final IAgent vehicle) { return (Double) vehicle.getAttribute(MIN_SECURITY_DISTANCE); @@ -912,6 +933,11 @@ public static void setLeadingVehicle(final IAgent vehicle, final IAgent leadingV vehicle.setAttribute(LEADING_VEHICLE, leadingVehicle); } + @getter(LEADING_DISTANCE) + public static double getLeadingDistance(final IAgent vehicle) { + return (double) vehicle.getAttribute(LEADING_DISTANCE); + } + @setter(LEADING_DISTANCE) public static void setLeadingDistanceReadOnly(final IAgent vehicle, final double leadingDist) { // read-only @@ -930,6 +956,10 @@ public static void setLeadingSpeed(final IAgent vehicle, final double leadingSpe vehicle.setAttribute(LEADING_SPEED, leadingSpeed); } + public static void setFollower(final IAgent vehicle, final IAgent follower) { + vehicle.setAttribute(FOLLOWER, follower); + } + @action( name = "advanced_follow_driving", args = { @@ -1328,10 +1358,12 @@ public void primDriveRandom(final IScope scope) throws GamaRuntimeException { // Choose a lane on the new road int firstSegment = !violatingOneway ? 0 : RoadSkill.getNumSegments(newRoad) - 1; - int secondPtIdx = !violatingOneway ? 1 : RoadSkill.getNumSegments(newRoad) - 1; - GamaPoint firstSegmentEndPt = new GamaPoint(newRoad.getInnerGeometry().getCoordinates()[secondPtIdx]); - double firstSegmentLength = loc.euclidianDistanceTo(firstSegmentEndPt); - laneAndAccPair = MOBIL.chooseLane(scope, vehicle, newTarget, newRoad, firstSegment, firstSegmentLength); +// int secondPtIdx = !violatingOneway ? 1 : RoadSkill.getNumSegments(newRoad) - 1; +// GamaPoint firstSegmentEndPt = new GamaPoint(newRoad.getInnerGeometry().getCoordinates()[secondPtIdx]); + double firstSegmentLength = RoadSkill.getSegmentLengths(newRoad).get(firstSegment); + double newRoadLength = RoadSkill.getTotalLength(newRoad); + laneAndAccPair = MOBIL.chooseLane(scope, vehicle, newTarget, newRoad, firstSegment, firstSegmentLength, + newRoadLength); if (laneAndAccPair == null) { return; } @@ -1371,7 +1403,9 @@ public void primDriveRandom(final IScope scope) throws GamaRuntimeException { IAgent currentTarget = getCurrentTarget(vehicle); int currentSegment = getSegmentIndex(vehicle); double distToGoal = getDistanceToGoal(vehicle); - laneAndAccPair = MOBIL.chooseLane(scope, vehicle, currentTarget, currentRoad, currentSegment, distToGoal); + double distToCurrentTarget = getDistanceToCurrentTarget(vehicle); + laneAndAccPair = MOBIL.chooseLane(scope, vehicle, currentTarget, currentRoad, currentSegment, distToGoal, + distToCurrentTarget); } int lowestLane = laneAndAccPair.getLeft(); double accel = laneAndAccPair.getRight(); @@ -1471,12 +1505,16 @@ public void primDrive(final IScope scope) throws GamaRuntimeException { GamaPoint srcNodeLoc = (GamaPoint) RoadSkill.getSourceNode(newRoad).getLocation(); boolean violatingOneway = !loc.equals(srcNodeLoc); + int firstSegment = !violatingOneway ? 0 : RoadSkill.getNumSegments(newRoad) - 1; - int secondPtIdx = !violatingOneway ? 1 : RoadSkill.getNumSegments(newRoad) - 1; - GamaPoint firstSegmentEndPt = new GamaPoint(newRoad.getInnerGeometry().getCoordinates()[secondPtIdx]); - double firstSegmentLength = loc.euclidianDistanceTo(firstSegmentEndPt); +// int secondPtIdx = !violatingOneway ? 1 : RoadSkill.getNumSegments(newRoad) - 1; +// GamaPoint firstSegmentEndPt = new GamaPoint(newRoad.getInnerGeometry().getCoordinates()[secondPtIdx]); + double firstSegmentLength = RoadSkill.getSegmentLengths(newRoad).get(firstSegment); + double newRoadLength = RoadSkill.getTotalLength(newRoad); // Choose a lane on the new road - laneAndAccPair = MOBIL.chooseLane(scope, vehicle, newTarget, newRoad, firstSegment, firstSegmentLength); + laneAndAccPair = MOBIL.chooseLane(scope, vehicle, newTarget, newRoad, firstSegment, firstSegmentLength, + newRoadLength); + if (laneAndAccPair == null) { return; } @@ -1523,7 +1561,9 @@ public void primDrive(final IScope scope) throws GamaRuntimeException { IAgent currentTarget = getCurrentTarget(vehicle); int currentSegment = getSegmentIndex(vehicle); double distToGoal = getDistanceToGoal(vehicle); - laneAndAccPair = MOBIL.chooseLane(scope, vehicle, currentTarget, currentRoad, currentSegment, distToGoal); + double distToCurrentTarget = getDistanceToCurrentTarget(vehicle); + laneAndAccPair = MOBIL.chooseLane(scope, vehicle, currentTarget, currentRoad, currentSegment, distToGoal, + distToCurrentTarget); } int lowestLane = laneAndAccPair.getLeft(); double accel = laneAndAccPair.getRight(); @@ -1662,28 +1702,19 @@ private void updateLaneSegment(final IScope scope, final int newLowestLane, Set newLanes = IntStream.range(newLowestLane, newLowestLane + numLanesOccupied) .boxed().collect(Collectors.toCollection(HashSet::new)); - // Small optimization to not touch the lists associated with unchanged lanes - Set lanesToRemove, lanesToAdd; - if (newSegment != currentSegment) { - // if entering new segment, we have to update the lists for all related lanes and segments - lanesToRemove = oldLanes; - lanesToAdd = newLanes; - } else { - // otherwise the vehicle is still in the same segment, so we only update (possibly few) relevant lists - lanesToRemove = Sets.difference(oldLanes, newLanes); - lanesToAdd = Sets.difference(newLanes, oldLanes); - } - IAgent correctRoad = getCurrentRoad(vehicle); - for (int lane : lanesToRemove) { - List oldVehicleList = RoadSkill.getVehiclesOnLaneSegment( - scope, correctRoad, lane, currentSegment); - oldVehicleList.remove(vehicle); + int numLanesCorrect = RoadSkill.getNumLanes(correctRoad); + double dist; + for (int lane : oldLanes) { + RoadSkill.getVehiclesOnLaneSegment(scope, correctRoad, lane).remove(vehicle); } - for (int lane : lanesToAdd) { - List newVehicleList = RoadSkill.getVehiclesOnLaneSegment( - scope, correctRoad, lane, newSegment); - newVehicleList.add(vehicle); + for (int lane : newLanes) { + if (lane < numLanesCorrect) { + dist = getDistanceToCurrentTarget(vehicle); + } else { + dist = RoadSkill.getTotalLength(correctRoad) - getDistanceToCurrentTarget(vehicle); + } + RoadSkill.getVehiclesOnLaneSegment(scope, correctRoad, lane).put(vehicle, dist); } setLowestLane(vehicle, newLowestLane); @@ -1731,6 +1762,9 @@ private double move(final IScope scope, GamaPoint endPt = !violatingOneway ? new GamaPoint(coords[currentSegment + 1]) : new GamaPoint(coords[currentSegment]); if (distMoved > distToGoal || distToGoal < EPSILON) { + setDistanceToCurrentTarget(vehicle, + getDistanceToCurrentTarget(vehicle) - distToGoal); + updateLaneSegment(scope, newLowestLane, currentSegment); if (endPt.equals(currentTarget.getLocation())) { // Move to a new road setLocation(vehicle, endPt); @@ -1738,13 +1772,14 @@ private double move(final IScope scope, // Return to the main loop in `drive` to continue moving across the intersection return distToGoal < EPSILON ? time : distToGoal / newSpeed; } else { + // TODO: need a loop to skip multiple segments // Move to a new segment distMoved -= distToGoal; loc = endPt; currentSegment = !violatingOneway ? currentSegment + 1 : currentSegment - 1; endPt = !violatingOneway ? new GamaPoint(coords[currentSegment + 1]) : new GamaPoint(coords[currentSegment]); - distToGoal = loc.distance(endPt); + distToGoal = RoadSkill.getSegmentLengths(currentRoad).get(currentSegment); } } @@ -1752,10 +1787,12 @@ private double move(final IScope scope, double newX = loc.getX() + ratio * (endPt.getX() - loc.getX()); double newY = loc.getY() + ratio * (endPt.getY() - loc.getY()); setLocation(vehicle, new GamaPoint(newX, newY)); - updateLaneSegment(scope, newLowestLane, currentSegment); - setDistanceToGoal(vehicle, distToGoal - distMoved); setSpeed(vehicle, newSpeed); setAcceleration(vehicle, accel); + setDistanceToGoal(vehicle, distToGoal - distMoved); + setDistanceToCurrentTarget(vehicle, getDistanceToCurrentTarget(vehicle) - distMoved); + + updateLaneSegment(scope, newLowestLane, currentSegment); return 0.0; } diff --git a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/RoadNodeSkill.java b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/RoadNodeSkill.java index 7e0e073f97..009a4c301c 100644 --- a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/RoadNodeSkill.java +++ b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/RoadNodeSkill.java @@ -73,22 +73,22 @@ public class RoadNodeSkill extends Skill { public static final String BLOCK = "block"; @getter(ROADS_IN) - public static List getRoadsIn(final IAgent agent) { - return (List) agent.getAttribute(ROADS_IN); + public static List getRoadsIn(final IAgent agent) { + return (List) agent.getAttribute(ROADS_IN); } @getter(ROADS_OUT) - public static List getRoadsOut(final IAgent agent) { - return (List) agent.getAttribute(ROADS_OUT); + public static List getRoadsOut(final IAgent agent) { + return (List) agent.getAttribute(ROADS_OUT); } @setter(ROADS_IN) - public static void setRoadsIn(final IAgent agent, final List rds) { + public static void setRoadsIn(final IAgent agent, final List rds) { agent.setAttribute(ROADS_IN, rds); } @setter(ROADS_OUT) - public static void setRoadsOut(final IAgent agent, final List rds) { + public static void setRoadsOut(final IAgent agent, final List rds) { agent.setAttribute(ROADS_OUT, rds); } diff --git a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/RoadSkill.java b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/RoadSkill.java index 8dbfcfd669..a79b62b38e 100644 --- a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/RoadSkill.java +++ b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/RoadSkill.java @@ -10,9 +10,20 @@ ********************************************************************************************************/ package simtools.gaml.extensions.traffic; +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.HashSet; +import java.util.LinkedList; import java.util.List; +import java.util.Set; + +import org.apache.commons.collections4.OrderedBidiMap; +import org.locationtech.jts.geom.Coordinate; +import org.locationtech.jts.geom.Geometry; import msi.gama.common.geometry.GeometryUtils; +import msi.gama.common.interfaces.IKeyword; import msi.gama.metamodel.agent.IAgent; import msi.gama.metamodel.shape.GamaPoint; import msi.gama.precompiler.GamlAnnotations.action; @@ -34,6 +45,7 @@ import msi.gaml.skills.Skill; import msi.gaml.types.IType; import msi.gaml.types.Types; +import simtools.gaml.extensions.traffic.carfollowing.CustomDualTreeBidiMap; @vars({ @variable( @@ -58,14 +70,6 @@ type = IType.AGENT, doc = @doc("the target node of the road") ), - @variable( - name = RoadSkill.LANES, - type = IType.INT, - doc = @doc( - value = "the number of lanes", - deprecated = "use num_lanes instead" - ) - ), @variable( name = RoadSkill.NUM_LANES, type = IType.INT, @@ -87,7 +91,19 @@ type = IType.FLOAT, init = "50#km/#h", doc = @doc("the maximal speed on the road") - ) + ), + @variable( + name = RoadSkill.SEGMENT_LENGTHS, + type = IType.LIST, + doc = @doc("stores the length of each road segment. " + + "The index of each element corresponds to the segment index.") + ), + @variable( + name = RoadSkill.VEHICLE_ORDERING, + type = IType.LIST, + depends_on = {RoadSkill.NUM_LANES}, + doc = @doc("provides information about the ordering of vehicle on any given lane") + ), }) @skill( name = RoadSkill.SKILL_ROAD, @@ -109,6 +125,8 @@ public class RoadSkill extends Skill { @Deprecated public static final String LANES = "lanes"; public static final String NUM_LANES = "num_lanes"; public static final String NUM_SEGMENTS = "num_segments"; + public static final String SEGMENT_LENGTHS = "segment_lengths"; + public static final String VEHICLE_ORDERING = "vehicle_ordering"; @getter(AGENTS_ON) public static List getAgentsOn(final IAgent agent) { @@ -121,13 +139,17 @@ public static void setAgentsOn(final IAgent agent, final List agents) { } @getter(ALL_AGENTS) - public static List getAgents(final IAgent agent) { - return (List) agent.getAttribute(ALL_AGENTS); + public static List getAgents(final IAgent agent) { + Set res = new HashSet<>(); + for (OrderedBidiMap map : getVehicleOrdering(agent)) { + res.addAll(map.keySet()); + } + return new ArrayList<>(res); } @setter(ALL_AGENTS) public static void setAgents(final IAgent agent, final List agents) { - agent.setAttribute(ALL_AGENTS, agents); + // read-only } @getter(SOURCE_NODE) @@ -150,16 +172,6 @@ public void setTargetNode(final IAgent agent, final IAgent nd) { agent.setAttribute(TARGET_NODE, nd); } - @getter(LANES) - public static int getLanes(final IAgent agent) { - return getNumLanes(agent); - } - - @setter(LANES) - public static void setLanes(final IAgent agent, final int numLanes) { - setNumLanes(agent, numLanes); - } - @getter(NUM_LANES) public static int getNumLanes(final IAgent agent) { return (int) agent.getAttribute(NUM_LANES); @@ -168,11 +180,17 @@ public static int getNumLanes(final IAgent agent) { @setter(NUM_LANES) public static void setNumLanes(final IAgent agent, final int numLanes) { if (numLanes == 0) { - GamaRuntimeException.warning(agent.getName() + " has zero lanes", + throw GamaRuntimeException.error(agent.getName() + " has zero lanes", GAMA.getRuntimeScope()); } agent.setAttribute(NUM_LANES, numLanes); } + + public static int getNumLanesTotal(final IAgent road) { + IAgent linkedRoad = getLinkedRoad(road); + int numLanesLinked = linkedRoad != null ? getNumLanes(linkedRoad) : 0; + return getNumLanes(road) + numLanesLinked; + } @getter(MAXSPEED) public static Double getMaxSpeed(final IAgent agent) { @@ -204,6 +222,39 @@ public static void setNumSegments(final IAgent road, final int numSegments) { // read-only } + @getter(value = SEGMENT_LENGTHS) + public static List getSegmentLengths(final IAgent road) { + List res = (List) road.getAttribute(SEGMENT_LENGTHS); + if (res.isEmpty()) { + Geometry geom = road.getInnerGeometry(); + if (road.getInnerGeometry() == null) { + throw GamaRuntimeException.error( + "The shape of the road has not been initialized", + GAMA.getRuntimeScope()); + } + Coordinate[] coords = geom.getCoordinates(); + for (int i = 0; i < coords.length - 1; i += 1) { + res.add(coords[i].distance(coords[i + 1])); + } + } + return res; + } + + public static double getTotalLength(final IAgent road) { + List lengths = getSegmentLengths(road); + return lengths.stream().reduce(0.0, Double::sum); + } + + @getter(VEHICLE_ORDERING) + public static List> getVehicleOrdering(final IAgent road) { + return (List>) road.getAttribute(VEHICLE_ORDERING); + } + + @setter(VEHICLE_ORDERING) + public static void setVehicleOrdering(final IAgent road, List> list) { + road.setAttribute(VEHICLE_ORDERING, list); + } + //TODO: update doc string /** * Converts the "overflow" lane to the correct lane index on the linked road. @@ -213,28 +264,24 @@ public static void setNumSegments(final IAgent road, final int numSegments) { * @param lane the input lane index * @return */ - public static List getVehiclesOnLaneSegment(final IScope scope, + public static OrderedBidiMap getVehiclesOnLaneSegment(final IScope scope, final IAgent correctRoad, - final int lane, - final int segment) { + final int lane) { int numLanesCorrect = getNumLanes(correctRoad); IAgent actualRoad; - int actualLane, actualSegment; + int actualLane; if (lane < numLanesCorrect) { actualRoad = correctRoad; actualLane = lane; - actualSegment = segment; } else { IAgent linkedRoad = getLinkedRoad(correctRoad); int numLanesLinked = getNumLanes(linkedRoad); actualRoad = linkedRoad; actualLane = numLanesCorrect + numLanesLinked - 1 - lane; - actualSegment = getNumSegments(actualRoad) - segment - 1; } - List>> vehicles = RoadSkill.getAgentsOn(actualRoad); - return vehicles.get(actualLane).get(actualSegment); + return getVehicleOrdering(actualRoad).get(actualLane); } @action( @@ -281,31 +328,28 @@ public static void register(IScope scope, IAgent vehicle, IAgent road, int lowes if (vehicle == null) return; int numLanesOccupied = DrivingSkill.getNumLanesOccupied(vehicle); + int numSegments = getNumSegments(road); + List lengths = getSegmentLengths(road); GamaPoint roadEndPt = (GamaPoint) getTargetNode(road).getLocation(); boolean violatingOneway = !DrivingSkill.getCurrentTarget(vehicle).getLocation().equals(roadEndPt); - // TODO: why not just set to 0 - int indexSegment = !violatingOneway ? 0 : getNumSegments(road) - 1; + int segmentIdx = !violatingOneway ? 0 : getNumSegments(road) - 1; for (int i = 0; i < numLanesOccupied; i += 1) { int lane = lowestLane + i; - List newVehicleList = getVehiclesOnLaneSegment(scope, road, lane, indexSegment); - newVehicleList.add(vehicle); + getVehiclesOnLaneSegment(scope, road, lane).put(vehicle, getTotalLength(road)); } - getAgents(road).add(vehicle); - + DrivingSkill.setViolatingOneway(vehicle, violatingOneway); if (!violatingOneway) { - DrivingSkill.setDistanceToGoal(vehicle, - vehicle.getLocation().euclidianDistanceTo(GeometryUtils.getPointsOf(road)[1])); + DrivingSkill.setDistanceToGoal(vehicle, lengths.get(0)); } else { - int numSegments = getNumSegments(road); - DrivingSkill.setDistanceToGoal(vehicle, - vehicle.getLocation().euclidianDistanceTo(GeometryUtils.getPointsOf(road)[numSegments - 1])); + DrivingSkill.setDistanceToGoal(vehicle, lengths.get(numSegments - 1)); } + DrivingSkill.setDistanceToCurrentTarget(vehicle, getTotalLength(road)); DrivingSkill.setCurrentRoad(vehicle, road); DrivingSkill.setLowestLane(vehicle, lowestLane); - DrivingSkill.setSegmentIndex(vehicle, indexSegment); + DrivingSkill.setSegmentIndex(vehicle, segmentIdx); } @action( @@ -343,84 +387,10 @@ public static void unregister(IScope scope, final IAgent driver) Integer lowestLane = (Integer) driver.getAttribute(DrivingSkill.LOWEST_LANE); int numLanesOccupied = (int) driver.getAttribute(DrivingSkill.NUM_LANES_OCCUPIED); - int currentSegment = DrivingSkill.getSegmentIndex(driver); for (int i = 0; i < numLanesOccupied; i += 1) { int lane = lowestLane + i; - List oldVehicleList = getVehiclesOnLaneSegment( - scope, currentRoad, lane, currentSegment); - oldVehicleList.remove(driver); + getVehiclesOnLaneSegment(scope, currentRoad, lane).remove(driver); } getAgents(currentRoad).remove(driver); } - - @action( - name = "update_lanes", - args = { - @arg( - name = "lanes", - type = IType.INT, - optional = false, - doc = @doc("the new number of lanes.") - ) - }, - doc = @doc( - value = "change the number of lanes of the road", - examples = { @example ("do update_lanes lanes: 2") } - ) - ) - public void primChangeLaneNumber(final IScope scope) throws GamaRuntimeException { - // TODO: update this for multi-lane - final IAgent road = getCurrentAgent(scope); - final Integer lanes = scope.getIntArg("lanes"); - setLanes(road, lanes); - if (lanes == 0) { - return; - } - int prev = getNumLanes(road); - final IList agentsOn = (IList) road.getAttribute(RoadSkill.AGENTS_ON); - if (prev == 0){ - for (int i = 0; i < prev; i++) { - final int nbSeg = road.getInnerGeometry().getNumPoints() - 1; - final IList lisSg = GamaListFactory.create(Types.NO_TYPE); - for (int j = 0; j < nbSeg; j++) { - lisSg.add(GamaListFactory.create(Types.NO_TYPE)); - } - agentsOn.add(lisSg); - } - } else if (prev < lanes) { - IList>> newAgentsOn = GamaListFactory.create(); - int nb_seg = ((IList) agentsOn.get(0)).size(); - for(int i = 0; i > agsPerLanes = null; - if (i < prev) { - agsPerLanes = (IList>) agentsOn.get(i); - } else { - agsPerLanes = GamaListFactory.create(); - for (int j = 0; j lanes) { - IList newAgentsOn = GamaListFactory.create(); - int nb_seg = ((IList) agentsOn.get(0)).size(); - for (int i = 0; i ags = (IList) agsPerLanes.get(j); - for (IAgent ag: ags) { - ((List)((List)newAgentsOn.get(lanes - 1)).get(j)).add(ag); - ag.setAttribute(DrivingSkill.LOWEST_LANE, lanes - 1); - } - } - } - } - setAgentsOn(road, newAgentsOn); - } - } } diff --git a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/Utils.java b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/Utils.java deleted file mode 100644 index adb4a28993..0000000000 --- a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/Utils.java +++ /dev/null @@ -1,305 +0,0 @@ -package simtools.gaml.extensions.traffic; - -import static simtools.gaml.extensions.traffic.DrivingSkill.getCurrentTarget; -import static simtools.gaml.extensions.traffic.DrivingSkill.getDistanceToGoal; -import static simtools.gaml.extensions.traffic.DrivingSkill.getMinSafetyDistance; -import static simtools.gaml.extensions.traffic.DrivingSkill.getNextRoad; -import static simtools.gaml.extensions.traffic.DrivingSkill.getNumLanesOccupied; -import static simtools.gaml.extensions.traffic.DrivingSkill.getVehicleLength; -import static simtools.gaml.extensions.traffic.DrivingSkill.isViolatingOneway; -import static simtools.gaml.extensions.traffic.DrivingSkill.readyToCross; - -import java.util.HashSet; -import java.util.Set; - -import org.apache.commons.lang3.tuple.ImmutablePair; -import org.apache.commons.lang3.tuple.ImmutableTriple; -import org.apache.commons.lang3.tuple.Triple; -import org.locationtech.jts.geom.Coordinate; - -import msi.gama.metamodel.agent.IAgent; -import msi.gama.metamodel.shape.GamaPoint; -import msi.gama.runtime.IScope; - -public class Utils { - /** - * Attempts to make lane changing probabilities timestep-agnostic - * - * @param probaInOneSecond a probability with respect to one second - * @param timeStep the duration of a simulation step - * @return the rescaled probability - */ - public static double rescaleProba(final double probaInOneSecond, - final double timeStep) { - return Math.min(probaInOneSecond * timeStep, 1.0); - } - - /** - * Find the leading vehicle (closest vehicle ahead) and the back vehicle - * (closest vehicle behind) which are moving on the same lanes as the current - * vehicle. - * - * For each of the above vehicle, the method returns a triplet containing: - * 1. The vehicle agent itself - * 2. The bumper-to-bumper gap between that vehicle and the current vehicle - * 3. Whether that vehicle is moving in the same direction - * - * If no leading vehicle is found on the current segment, - * it tries to find one in the next segment or the first segment of the next road. - * If none is found still, the bumper-to-bumper gap is set to a big value (to eliminate the deceleration term in IDM). - * - * On the other hand, if no back vehicle is found on the current segment, - * the method does NOT consider the previous segment or the last segment of the previous road. - * (a possible TODO?) - * - * If either the leading or back vehicle overlaps with the current one, the return value will be null. - * - * @param scope - * @param vehicle the vehicle whose leading and back vehicle is to be found - * @param target the target of the concerned vehicle - * @param road the road which the vehicle is moving on - * @param segment the index of the current road segment that the vehicle is on - * @param distToSegmentEnd the distance from the vehicle to the segment endpoint - * @param lowestLane the current "lowest" lane index of the vehicle - * @return a pair containing two triplets, one for the leading vehicle and one - * for the back vehicle - */ - public static ImmutablePair, Triple> - findLeadingAndBackVehicle(final IScope scope, - final IAgent vehicle, - final IAgent target, - final IAgent road, - final int segment, - final double distToSegmentEnd, - final int lowestLane) { - double vL = getVehicleLength(vehicle); - double minSafetyDist = getMinSafetyDistance(vehicle); - boolean violatingOneway = isViolatingOneway(vehicle); - - int endPtIdx = !violatingOneway ? segment + 1 : segment; - GamaPoint segmentEndPt = new GamaPoint(road.getInnerGeometry().getCoordinates()[endPtIdx]); - - int numLanesOccupied = getNumLanesOccupied(vehicle); - Set neighbors = new HashSet<>(); - for (int i = 0; i < numLanesOccupied; i += 1) { - neighbors.addAll( - RoadSkill.getVehiclesOnLaneSegment(scope, road, lowestLane + i, segment) - ); - } - - // finding the closest vehicle ahead & behind - IAgent leadingVehicle = null; - double minLeadingDist = Double.MAX_VALUE; - boolean leadingSameDirection = false; - IAgent backVehicle = null; - double minBackDist = Double.MAX_VALUE; - boolean backSameDirection = false; - Triple leadingTriple; - Triple backTriple; - - // TODO: rework this spaghetti bowl - // Should probably construct a map of Agent -> gapBetweenCentroids first - // (including agents from other segments & roads), - // then iterate through the keys to find leader & follower - for (IAgent otherVehicle : neighbors) { - if (otherVehicle == vehicle || otherVehicle == null || otherVehicle.dead()) { - continue; - } - boolean sameDirection = target == getCurrentTarget(otherVehicle); - double otherVL = getVehicleLength(otherVehicle); - double otherDistToSegmentEnd; - if (sameDirection) { - otherDistToSegmentEnd = getDistanceToGoal(otherVehicle); - } else { - GamaPoint otherLocation = (GamaPoint) otherVehicle.getLocation(); - otherDistToSegmentEnd = otherLocation.distance(segmentEndPt); - } - - // Calculate bumper-to-bumper distances - double gapBetweenCentroids = distToSegmentEnd - otherDistToSegmentEnd; - double gap = Math.abs(gapBetweenCentroids) - 0.5 * vL - 0.5 * otherVL; - - if (Math.abs(gapBetweenCentroids) < 0.5 * vL + 0.5 * otherVL) { - //Patrick: I removed that to avoid stuck vehicles - - // Overlap with another vehicle - //return null; - if (gapBetweenCentroids >= 0) - gap = + 0.5 * vL + 0.5 * otherVL; - else { - gap = - 0.5 * vL - 0.5 * otherVL; - } - } - if (gapBetweenCentroids > 0 && gap < minLeadingDist) { - leadingVehicle = otherVehicle; - minLeadingDist = Math.abs(gap); - leadingSameDirection = sameDirection; - } else if (gapBetweenCentroids < 0 && gap < minBackDist) { - backVehicle = otherVehicle; - minBackDist = Math.abs(gap); - backSameDirection = sameDirection; - } - } - - if (backVehicle == null) { - int numSegments = RoadSkill.getNumSegments(road); - // TODO: check for previous roads as well (all roads connected to last target?) - if ((!violatingOneway && segment > 0) || - (violatingOneway && segment < numSegments - 1)) { - int prevSegment; - Coordinate prevPoint; - Coordinate coords[] = road.getInnerGeometry().getCoordinates(); - if (!violatingOneway) { - prevSegment = segment - 1; - prevPoint = coords[segment]; - } else { - prevSegment = segment + 1; - prevPoint = coords[segment + 1]; - } - GamaPoint loc = (GamaPoint) vehicle.getLocation(); - double distToPrevTarget = loc.distance(prevPoint); - Set prevNeighbors = new HashSet<>(); - for (int i = 0; i < numLanesOccupied; i += 1) { - prevNeighbors.addAll( - RoadSkill.getVehiclesOnLaneSegment(scope, road, lowestLane + i, prevSegment) - ); - } - for (IAgent otherVehicle : prevNeighbors) { - boolean sameDirection = target == getCurrentTarget(otherVehicle); - double otherVL = getVehicleLength(otherVehicle); - double otherDistToSegmentEnd; - if (sameDirection) { - otherDistToSegmentEnd = getDistanceToGoal(otherVehicle); - } else { - GamaPoint otherLocation = (GamaPoint) otherVehicle.getLocation(); - otherDistToSegmentEnd = otherLocation.distance(segmentEndPt); - } - - double gap = otherDistToSegmentEnd + distToPrevTarget - 0.5 * vL - 0.5 * otherVL; - - if (gap < 0) { - gap = 0.0; - } - //Patrick: I removed that to avoid stuck vehicles - // crash - //return null; - //} else - if (gap < minBackDist) { - backVehicle = otherVehicle; - minBackDist = gap; - backSameDirection = sameDirection; - } - } - } - } - - if (backVehicle == null) { - backTriple = null; - } else { - backTriple = ImmutableTriple.of(backVehicle, minBackDist, backSameDirection); - } - - // No leading vehicle is found on the current segment - if (leadingVehicle == null) { - IAgent nextRoad = getNextRoad(vehicle); - // Check if vehicle is approaching an intersection - int numSegments = RoadSkill.getNumSegments(road); - if ((!violatingOneway && segment == numSegments - 1) || - (violatingOneway && segment == 0)) { - // Return a virtual leading vehicle of length 0 to simulate deceleration at intersections - // NOTE: the added minSafetyDist is necessary for the vehicle to ignore the safety dist when stopping at an endpoint - // TODO: make the vehicles stop in front of the lights, - // this would require changes in the drive loop as well - IAgent stoppingNode = target; - leadingTriple = ImmutableTriple.of(stoppingNode, distToSegmentEnd + minSafetyDist, false); - // Slowing down at final target, since at this point we don't know which road will be taken next - if (nextRoad == null) { - return ImmutablePair.of(leadingTriple, backTriple); - // Might need to slow down at the intersection if it is not possible to enter the next road - } else { - if (!readyToCross(scope, vehicle, stoppingNode, nextRoad)) { - return ImmutablePair.of(leadingTriple, backTriple); - } - } - } - - // Continue to find leading vehicle on next segment or next road in path - IAgent roadToCheck = null; - IAgent targetToCheck = null; - int lowestLaneToCheck = 0; - int segmentToCheck = 0; - if ((!violatingOneway && segment < numSegments - 1) || - (violatingOneway && segment > 0)) { - roadToCheck = road; - targetToCheck = target; - segmentToCheck = !violatingOneway ? segment + 1 : segment - 1; - lowestLaneToCheck = lowestLane; - } else if (road != nextRoad) { // road == nextRoad when vehicle is at an intersection - roadToCheck = nextRoad; - boolean willViolateOneway = target == RoadSkill.getTargetNode(nextRoad); - targetToCheck = !willViolateOneway ? - RoadSkill.getTargetNode(nextRoad) : RoadSkill.getSourceNode(nextRoad); - segmentToCheck = !willViolateOneway ? 0 : RoadSkill.getNumSegments(nextRoad) - 1; - // TODO: is this the right lane to check? - int numLanesTotal = RoadSkill.getNumLanes(roadToCheck); - IAgent linkedRoadToCheck = RoadSkill.getLinkedRoad(roadToCheck); - if (linkedRoadToCheck != null) { - numLanesTotal += RoadSkill.getNumLanes(linkedRoadToCheck); - } - lowestLaneToCheck = Math.min(lowestLane, numLanesTotal - numLanesOccupied); - } - - if (roadToCheck != null) { - Set furtherVehicles = new HashSet<>(); - for (int i = 0; i < numLanesOccupied; i += 1) { - furtherVehicles.addAll( - RoadSkill.getVehiclesOnLaneSegment(scope, - roadToCheck, lowestLaneToCheck + i, segmentToCheck) - ); - } - - for (IAgent otherVehicle : furtherVehicles) { - if (otherVehicle == vehicle || otherVehicle == null || otherVehicle.dead()) { - continue; - } - // check if the other vehicle going in opposite direction - double targetToOtherVehicle; - boolean sameDirection = targetToCheck == getCurrentTarget(otherVehicle); - if (sameDirection) { - Coordinate coords[] = roadToCheck.getInnerGeometry().getCoordinates(); - // TODO: verify the correctness of this, regarding one-way - double segmentLength = coords[segmentToCheck].distance(coords[segmentToCheck + 1]); - targetToOtherVehicle = segmentLength - getDistanceToGoal(otherVehicle); - } else { - targetToOtherVehicle = getDistanceToGoal(otherVehicle); - } - double otherVL = getVehicleLength(otherVehicle); - double gap = distToSegmentEnd + targetToOtherVehicle - 0.5 * vL - 0.5 * otherVL; - - if (gap < 0) { - gap = 0.0; - } - //Patrick: I removed that to avoid stuck vehicles - // crash - //return null; - //} else - if (gap < minLeadingDist) { - leadingVehicle = otherVehicle; - minLeadingDist = gap; - leadingSameDirection = sameDirection; - } - } - } - } - - if (leadingVehicle == null || leadingVehicle.dead()) { - // the road ahead seems to be completely clear - leadingTriple = ImmutableTriple.of(null, 1e6, false); - return ImmutablePair.of(leadingTriple, backTriple); - } else { - // Found a leading vehicle on the next segment/next road - leadingTriple = ImmutableTriple.of(leadingVehicle, minLeadingDist, leadingSameDirection); - return ImmutablePair.of(leadingTriple, backTriple); - } - } -} diff --git a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/CustomDualTreeBidiMap.java b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/CustomDualTreeBidiMap.java new file mode 100644 index 0000000000..d40e6f9a3d --- /dev/null +++ b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/CustomDualTreeBidiMap.java @@ -0,0 +1,40 @@ +package simtools.gaml.extensions.traffic.carfollowing; + +import java.util.Comparator; +import java.util.Map; + +import org.apache.commons.collections4.BidiMap; +import org.apache.commons.collections4.bidimap.DualTreeBidiMap; + +public class CustomDualTreeBidiMap extends DualTreeBidiMap { + protected CustomDualTreeBidiMap(Map normalMap, Map reverseMap, BidiMap inverseBidiMap) { + super(normalMap, reverseMap, inverseBidiMap); + } + + public CustomDualTreeBidiMap(Comparator keyComparator, Comparator valueComparator) { + super(keyComparator, valueComparator); + } + + protected CustomDualTreeBidiMap createBidiMap(Map normalMap, Map reverseMap, BidiMap inverseMap) { + return new CustomDualTreeBidiMap(normalMap, reverseMap, inverseMap); + } + + /** + * The original method in DualTreeBidiMap does not work correctly when + * the key is not present in the map. + */ + @Override + public K nextKey(K key) { + if (containsKey(key) || size() == 0) { + return super.nextKey(key); + } else { + K last = lastKey(); + if (comparator().compare(key, last) > 0) { + return null; + } else { + K next = super.nextKey(key); + return next == null ? last : previousKey(next); + } + } + } +} diff --git a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/Utils.java b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/Utils.java new file mode 100644 index 0000000000..7f6a2ab330 --- /dev/null +++ b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/Utils.java @@ -0,0 +1,270 @@ +package simtools.gaml.extensions.traffic.carfollowing; + +import static simtools.gaml.extensions.traffic.DrivingSkill.getDistanceToCurrentTarget; +import static simtools.gaml.extensions.traffic.DrivingSkill.getCurrentRoad; +import static simtools.gaml.extensions.traffic.DrivingSkill.getCurrentTarget; +import static simtools.gaml.extensions.traffic.DrivingSkill.getMinSafetyDistance; +import static simtools.gaml.extensions.traffic.DrivingSkill.getNextRoad; +import static simtools.gaml.extensions.traffic.DrivingSkill.getNumLanesOccupied; +import static simtools.gaml.extensions.traffic.DrivingSkill.getVehicleLength; +import static simtools.gaml.extensions.traffic.DrivingSkill.isViolatingOneway; +import static simtools.gaml.extensions.traffic.DrivingSkill.readyToCross; + +import org.apache.commons.collections4.OrderedBidiMap; +import org.apache.commons.lang3.tuple.ImmutableTriple; +import org.apache.commons.lang3.tuple.Triple; + +import java.util.ArrayList; +import java.util.List; + +import msi.gama.metamodel.agent.IAgent; +import msi.gama.runtime.IScope; +import simtools.gaml.extensions.traffic.RoadNodeSkill; +import simtools.gaml.extensions.traffic.RoadSkill; + +public class Utils { + /** + * Attempts to make lane changing probabilities timestep-agnostic + * + * @param probaInOneSecond a probability with respect to one second + * @param timeStep the duration of a simulation step + * @return the rescaled probability + */ + public static double rescaleProba(final double probaInOneSecond, + final double timeStep) { + return Math.min(probaInOneSecond * timeStep, 1.0); + } + + // TODO: this and findFollower don't need this many params + public static Triple findLeader(final IScope scope, + final IAgent vehicle, + final IAgent target, + final IAgent road, + final int segment, + final int lowestLane, + final double distToSegmentEnd, + final double distToCurrentTarget) { + double vL = getVehicleLength(vehicle); + double minSafetyDist = getMinSafetyDistance(vehicle); + boolean violatingOneway = isViolatingOneway(vehicle); + int numRoadLanes = RoadSkill.getNumLanes(road); + int numLanesOccupied = getNumLanesOccupied(vehicle); + + + double tmpDistQuery; + IAgent leader = null; + double minGap = Double.MAX_VALUE; + boolean sameDirection = false; + for (int i = 0; i < numLanesOccupied; i += 1) { + int lane = lowestLane + i; + OrderedBidiMap distMap = + RoadSkill.getVehiclesOnLaneSegment(scope, road, lane).inverseBidiMap(); + boolean wrongDirection = lane < numRoadLanes ? false : true; + wrongDirection = violatingOneway ? !wrongDirection : wrongDirection; + tmpDistQuery = !wrongDirection ? distToCurrentTarget : + RoadSkill.getTotalLength(road) - distToCurrentTarget; + + // Another vehicle already occupied the exact same longitudinal spot + // in this lane, which will lead to a crash if switch + if (distMap.containsKey(tmpDistQuery) && distMap.get(tmpDistQuery) != vehicle) { + return null; + } + + Double k = !wrongDirection ? distMap.nextKey(tmpDistQuery) + : distMap.previousKey(tmpDistQuery); + // No leader on this lane + if (k == null) { + continue; + } + double tmpLeaderDist = k; + IAgent tmpLeader = distMap.get(tmpLeaderDist); + if (tmpLeader == null || tmpLeader.dead()) { + continue; + } + double otherVL = getVehicleLength(tmpLeader); + double gap = Math.abs(tmpLeaderDist - tmpDistQuery) - 0.5 * vL - 0.5 * otherVL; + + if (gap < 0) { + return null; + } else if (gap < minGap) { + minGap = gap; + leader = tmpLeader; + sameDirection = target == getCurrentTarget(tmpLeader); + } + + } + if (leader != null && !leader.dead()) { + return ImmutableTriple.of(leader, minGap, sameDirection); + } + + // The methods continue down here if no leading vehicle is found on the current road + + IAgent nextRoad = getNextRoad(vehicle); + // If vehicle is approaching an intersection, we need to slow down if + // 1. The intersection is the final target, the vehicle hasn't decide the next road yet + // 2. It is not possible to enter the next road (e.g. traffic lights) + if (nextRoad == null || !readyToCross(scope, vehicle, target, nextRoad)) { + // Return a virtual leading vehicle of length 0 to simulate deceleration at intersections + // NOTE: the added minSafetyDist is necessary for the vehicle to ignore the safety dist when stopping at an endpoint + return ImmutableTriple.of(target, distToCurrentTarget + minSafetyDist, false); + } else if (nextRoad != road) { // nextRoad == road when vehicle is trying to cross intersection + boolean willViolateOneway = target == RoadSkill.getTargetNode(nextRoad); + IAgent nextTarget = !willViolateOneway ? + RoadSkill.getTargetNode(nextRoad) : RoadSkill.getSourceNode(nextRoad); + int numLanesNext = RoadSkill.getNumLanes(nextRoad); + int numLanesTotalNext = RoadSkill.getNumLanesTotal(nextRoad); + int lowestLaneToCheck = Math.min(lowestLane, numLanesTotalNext - numLanesOccupied); + + for (int i = 0; i < numLanesOccupied; i += 1) { + int lane = lowestLaneToCheck + i; + OrderedBidiMap distMap = + RoadSkill.getVehiclesOnLaneSegment(scope, nextRoad, lane).inverseBidiMap(); + boolean wrongDirection = lane < numLanesNext ? false : true; + wrongDirection = willViolateOneway ? !wrongDirection : wrongDirection; + + if (distMap.isEmpty()) { + continue; + } + + double tmpLeaderDist = !wrongDirection ? distMap.firstKey() : distMap.lastKey(); + IAgent tmpLeader = distMap.get(tmpLeaderDist); + if (tmpLeader == null || tmpLeader.dead()) { + continue; + } + double extraGap = !wrongDirection ? + RoadSkill.getTotalLength(nextRoad) - tmpLeaderDist : tmpLeaderDist; + double otherVL = getVehicleLength(tmpLeader); + double gap = distToCurrentTarget + extraGap - 0.5 * vL - 0.5 * otherVL; + + if (gap < 0) { + return null; + } + if (gap < minGap) { + minGap = gap; + leader = tmpLeader; + sameDirection = nextTarget == getCurrentTarget(tmpLeader); + } + } + } + + if (leader != null && !leader.dead()) { + return ImmutableTriple.of(leader, minGap, sameDirection); + } else { + // the road ahead seems to be completely clear + return ImmutableTriple.of(null, 1e6, false); + } + } + + + public static Triple findFollower(final IScope scope, + final IAgent vehicle, + final IAgent target, + final IAgent dummyRoad, + final int segment, + final int lowestLane, + final double distToSegmentEnd, + final double dummyDistToCurrentTarget) { + IAgent road = getCurrentRoad(vehicle); + if (road == null) { + return ImmutableTriple.of(null,null,null); + } + double vL = getVehicleLength(vehicle); + double distToCurrentTarget = getDistanceToCurrentTarget(vehicle); + + boolean violatingOneway = isViolatingOneway(vehicle); + int numRoadLanes = RoadSkill.getNumLanes(road); + int numLanesOccupied = getNumLanesOccupied(vehicle); + + IAgent follower = null; + double minGap = Double.MAX_VALUE; + boolean sameDirection = false; + for (int i = 0; i < numLanesOccupied; i += 1) { + int lane = lowestLane + i; + OrderedBidiMap distMap = + RoadSkill.getVehiclesOnLaneSegment(scope, road, lane).inverseBidiMap(); + boolean wrongDirection = lane < numRoadLanes ? false : true; + wrongDirection = violatingOneway ? !wrongDirection : wrongDirection; + double tmpDistQuery = !wrongDirection ? distToCurrentTarget : + RoadSkill.getTotalLength(road) - distToCurrentTarget; + + if (distMap.containsKey(tmpDistQuery) && distMap.get(tmpDistQuery) != vehicle) { + return null; + } + + Double k = !wrongDirection ? distMap.previousKey(tmpDistQuery) + : distMap.nextKey(tmpDistQuery); + + // No follower on this lane + if (k == null) { + continue; + } + double tmpLeaderDist = k; + IAgent tmpFollower = distMap.get(tmpLeaderDist); + if (tmpFollower == null || tmpFollower.dead()) { + continue; + } + double otherVL = getVehicleLength(tmpFollower); + double gap = Math.abs(tmpLeaderDist - tmpDistQuery) - 0.5 * vL - 0.5 * otherVL; + + if (gap < 0) { + return null; + } else if (gap < minGap) { + minGap = gap; + follower = tmpFollower; + sameDirection = target == getCurrentTarget(tmpFollower); + } + + } + if (follower != null && !follower.dead()) { + return ImmutableTriple.of(follower, minGap, sameDirection); + } + + // Find followers on previous roads + IAgent sourceNode = (target == RoadSkill.getTargetNode(road)) ? + RoadSkill.getSourceNode(road) : target; + for (IAgent prevRoad : RoadNodeSkill.getRoadsIn(sourceNode)) { + int numLanes = RoadSkill.getNumLanes(prevRoad); + int numLanesTotal = RoadSkill.getNumLanesTotal(prevRoad); + for (int i = 0; i < numLanesOccupied; i += 1) { + int lane = lowestLane + i; + if (lane >= numLanesTotal - numLanesOccupied) { + break; + } + + boolean isLinkedLane = lane >= numLanes; + if (isLinkedLane) { + prevRoad = RoadSkill.getLinkedRoad(prevRoad); + } + OrderedBidiMap distMap = + RoadSkill.getVehiclesOnLaneSegment(scope, prevRoad, lane).inverseBidiMap(); + if (distMap.isEmpty()) { + continue; + } + double distQuery = !isLinkedLane ? distMap.lastKey() : distMap.firstKey(); + + IAgent tmpFollower = distMap.get(distQuery); + if (getCurrentTarget(tmpFollower) != sourceNode) { + // This vehicle is not following the current one, but it's going the other way + continue; + } + + double extraGap = !isLinkedLane ? distQuery : RoadSkill.getTotalLength(prevRoad) - distQuery; + double gap = RoadSkill.getTotalLength(road) - distToCurrentTarget + + extraGap - 0.5 * vL - 0.5 * getVehicleLength(tmpFollower); + if (gap < 0) { + return null; + } else if (gap < minGap) { + minGap = gap; + follower = tmpFollower; + sameDirection = sourceNode == getCurrentTarget(tmpFollower); + } + } + } + + if (follower != null && !follower.dead()) { + return ImmutableTriple.of(follower, minGap, sameDirection); + } else { + return ImmutableTriple.of(null, null, null); + } + } +} diff --git a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/lanechange/MOBIL.java b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/lanechange/MOBIL.java index d0ecd682ee..25c5916ec6 100644 --- a/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/lanechange/MOBIL.java +++ b/simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/lanechange/MOBIL.java @@ -1,11 +1,13 @@ package simtools.gaml.extensions.traffic.lanechange; import static simtools.gaml.extensions.traffic.DrivingSkill.getAccBias; +import static simtools.gaml.extensions.traffic.DrivingSkill.setFollower; import static simtools.gaml.extensions.traffic.DrivingSkill.getAccGainThreshold; import static simtools.gaml.extensions.traffic.DrivingSkill.getAllowedLanes; import static simtools.gaml.extensions.traffic.DrivingSkill.getLCCooldown; import static simtools.gaml.extensions.traffic.DrivingSkill.getLaneChangeLimit; import static simtools.gaml.extensions.traffic.DrivingSkill.getLeadingVehicle; +import static simtools.gaml.extensions.traffic.DrivingSkill.getLeadingDistance; import static simtools.gaml.extensions.traffic.DrivingSkill.getLinkedLaneLimit; import static simtools.gaml.extensions.traffic.DrivingSkill.getLowestLane; import static simtools.gaml.extensions.traffic.DrivingSkill.getMaxSafeDeceleration; @@ -20,8 +22,9 @@ import static simtools.gaml.extensions.traffic.DrivingSkill.setLeadingSpeed; import static simtools.gaml.extensions.traffic.DrivingSkill.setLeadingVehicle; import static simtools.gaml.extensions.traffic.DrivingSkill.setTimeSinceLC; -import static simtools.gaml.extensions.traffic.Utils.findLeadingAndBackVehicle; -import static simtools.gaml.extensions.traffic.Utils.rescaleProba; +import static simtools.gaml.extensions.traffic.carfollowing.Utils.findFollower; +import static simtools.gaml.extensions.traffic.carfollowing.Utils.findLeader; +import static simtools.gaml.extensions.traffic.carfollowing.Utils.rescaleProba; import java.util.List; import java.util.stream.Collectors; @@ -53,7 +56,8 @@ public static ImmutablePair chooseLane(final IScope scope, final IAgent target, final IAgent road, final int segment, - final double distToSegmentEnd) { + final double distToSegmentEnd, + final double distToCurrentTarget) { double VL = getVehicleLength(vehicle); int numLanesOccupied = getNumLanesOccupied(vehicle); int currentLowestLane = getLowestLane(vehicle); @@ -85,25 +89,25 @@ public static ImmutablePair chooseLane(final IScope scope, boxed().collect(Collectors.toList()); // Compute acceleration if the vehicle stays on the same lane - ImmutablePair, Triple> pair = - findLeadingAndBackVehicle(scope, vehicle, target, road, segment, distToSegmentEnd, currentLowestLane); - IAgent currentBackVehicle = null; + Triple leaderTriple = findLeader( + scope, vehicle, target, road, segment, currentLowestLane, distToSegmentEnd, distToCurrentTarget); + Triple followerTriple = findFollower( + scope, vehicle, target, road, segment, currentLowestLane, distToSegmentEnd, distToCurrentTarget); + IAgent currentBackVehicle = followerTriple != null ? followerTriple.getLeft() : null; double stayAccelM; - if (pair == null) { + if (leaderTriple == null) { // || followerTriple == null) { stayAccelM = -Double.MAX_VALUE; } else { - if (pair.getValue() != null) { - currentBackVehicle = pair.getValue().getLeft(); - } // Find the leading vehicle on current lanes - IAgent leadingVehicle = pair.getKey().getLeft(); - double leadingDist = pair.getKey().getMiddle(); - boolean leadingSameDirection = pair.getKey().getRight(); + IAgent leadingVehicle = leaderTriple.getLeft(); + double leadingDist = leaderTriple.getMiddle(); + boolean leadingSameDirection = leaderTriple.getRight(); double leadingSpeed = getSpeed(leadingVehicle); leadingSpeed = leadingSameDirection ? leadingSpeed : -leadingSpeed; setLeadingVehicle(vehicle, leadingVehicle); setLeadingDistance(vehicle, leadingDist); setLeadingSpeed(vehicle, leadingSpeed); + setFollower(vehicle, currentBackVehicle); // Calculate acc(M) - Acceleration of current vehicle M if no lane change occurs stayAccelM = IDM.computeAcceleration(scope, vehicle, leadingDist, leadingSpeed); // Do not allow changing lane when approaching intersections @@ -128,13 +132,6 @@ public static ImmutablePair chooseLane(final IScope scope, continue; } - // Evaluate probabilities to switch to tmpLowestLane - // boolean canChangeDown = tmpLowestLane < lowestLane && - // scope.getRandom().next() < probaChangeLaneDown; - // // NOTE: in canChangeUp, the 2nd condition prevents moving from current road to linked road - // boolean canChangeUp = tmpLowestLane > lowestLane && - // !(lowestLane <= numCurrentLanes - numLanesOccupied && tmpLowestLane > numCurrentLanes - numLanesOccupied) && - // scope.getRandom().next() < probaChangeLaneUp; if (currentLowestLane <= numCurrentLanes - numLanesOccupied && tmpLowestLane > numCurrentLanes - numLanesOccupied) { if (scope.getRandom().next() > probaUseLinkedRoad) { @@ -142,17 +139,19 @@ public static ImmutablePair chooseLane(final IScope scope, } } - pair = findLeadingAndBackVehicle(scope, vehicle, target, road, segment, distToSegmentEnd, tmpLowestLane); - if (pair == null) { + Triple newLeaderTriple = findLeader( + scope, vehicle, target, road, segment, tmpLowestLane, distToSegmentEnd, distToCurrentTarget); + Triple newFollowerTriple = findFollower( + scope, vehicle, target, road, segment, tmpLowestLane, distToSegmentEnd, distToCurrentTarget); + if (newLeaderTriple == null || newFollowerTriple == null) { // Will crash into another vehicle if switch to this lane continue; } // Find the leading vehicle of M on this new lane - Triple leadingTriple = pair.getKey(); - IAgent leadingVehicle = leadingTriple.getLeft(); - double leadingDist = leadingTriple.getMiddle(); - boolean leadingSameDirection = leadingTriple.getRight(); + IAgent leadingVehicle = newLeaderTriple.getLeft(); + double leadingDist = newLeaderTriple.getMiddle(); + boolean leadingSameDirection = newLeaderTriple.getRight(); double leadingSpeed = getSpeed(leadingVehicle); leadingSpeed = leadingSameDirection ? leadingSpeed : -leadingSpeed; @@ -162,19 +161,18 @@ public static ImmutablePair chooseLane(final IScope scope, // Find back vehicle B' on new lane double stayAccelB; double changeAccelB; - Triple backTriple = pair.getValue(); - if (backTriple == null || !backTriple.getRight() || - backTriple.getLeft() == currentBackVehicle || - getLeadingVehicle(backTriple.getLeft()) != vehicle) { - // IF no back vehicle OR back vehicle is moving in opposite direction OR - // back vehicle on new lanes is the same one on old lanes OR - // back vehicle's leading vehicle is not the current vehicle - // THEN acceleration change of B is irrelevant + // Ignore follower in incentive criterion if: + // 1. No follower was found + // 2. New follower if switch lanes is still the old one + // 3. The follower is actually following another vehicle + if (newFollowerTriple.getLeft() == null || + newFollowerTriple.getLeft() == currentBackVehicle || + getLeadingDistance(newFollowerTriple.getLeft()) < newFollowerTriple.getMiddle()) { stayAccelB = 0; changeAccelB = 0; } else { - IAgent backVehicle = backTriple.getLeft(); - double backDist = backTriple.getMiddle(); + IAgent backVehicle = newFollowerTriple.getLeft(); + double backDist = newFollowerTriple.getMiddle(); // Calculate acc(B') - acceleration of B' if M does not change to this lane // NOTE: in this case, the leading vehicle is the one we have found above for M stayAccelB = IDM.computeAcceleration(scope, backVehicle, backDist + VL + leadingDist, leadingSpeed); @@ -206,6 +204,7 @@ public static ImmutablePair chooseLane(final IScope scope, setLeadingVehicle(vehicle, leadingVehicle); setLeadingDistance(vehicle, leadingDist); setLeadingSpeed(vehicle, leadingSpeed); + setFollower(vehicle, newFollowerTriple.getLeft()); } }