Skip to content

Commit

Permalink
Refactor findLeader and findFollower
Browse files Browse the repository at this point in the history
  • Loading branch information
@minhduc0711
minhduc0711 committed Aug 10, 2021
1 parent fd2d4b4 commit 8d9a66a
Show file tree
Hide file tree
Showing 3 changed files with 133 additions and 136 deletions.
2 changes: 1 addition & 1 deletion simtools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/DrivingSkill.java
View file
Expand Up @@ -53,7 +53,7 @@
import msi.gaml.statements.IStatement;
import msi.gaml.types.IType;
import msi.gaml.types.Types;
import simtools.gaml.extensions.traffic.lanechange.MOBIL;
import simtools.gaml.extensions.traffic.carfollowing.MOBIL;
import ummisco.gama.dev.utils.DEBUG;

@vars({
Expand Down
31 changes: 10 additions & 21 deletions .../extensions/traffic/lanechange/MOBIL.java → ...xtensions/traffic/carfollowing/MOBIL.java
View file
@@ -1,4 +1,4 @@
package simtools.gaml.extensions.traffic.lanechange;
package simtools.gaml.extensions.traffic.carfollowing;

import static simtools.gaml.extensions.traffic.DrivingSkill.getAccBias;
import static simtools.gaml.extensions.traffic.DrivingSkill.setFollower;
Expand Down Expand Up @@ -39,8 +39,6 @@
import simtools.gaml.extensions.traffic.DrivingSkill;
import simtools.gaml.extensions.traffic.RoadNodeSkill;
import simtools.gaml.extensions.traffic.RoadSkill;
import simtools.gaml.extensions.traffic.carfollowing.IDM;
import ummisco.gama.dev.utils.DEBUG;

public class MOBIL {
/**
Expand Down Expand Up @@ -98,12 +96,12 @@ public static ImmutablePair<Integer, Double> chooseLane(final IScope scope,

// Compute acceleration if the vehicle stays on the same lane
Triple<IAgent, Double, Boolean> leaderTriple = findLeader(
scope, vehicle, target, road, segment, currentLowestLane, distToSegmentEnd, distToCurrentTarget);
scope, vehicle, currentLowestLane);
Triple<IAgent, Double, Boolean> followerTriple = findFollower(
scope, vehicle, target, road, segment, currentLowestLane, distToSegmentEnd, distToCurrentTarget);
scope, vehicle, currentLowestLane);
IAgent currentBackVehicle = followerTriple != null ? followerTriple.getLeft() : null;
double stayAccelM;
if (leaderTriple == null) { // || followerTriple == null) {
if (leaderTriple.getMiddle() < 0) { // || followerTriple == null) {
stayAccelM = -Double.MAX_VALUE;
} else {
// Find the leading vehicle on current lanes
Expand Down Expand Up @@ -147,22 +145,13 @@ public static ImmutablePair<Integer, Double> chooseLane(final IScope scope,
}
}

Triple<IAgent, Double, Boolean> newLeaderTriple = findLeader(
scope, vehicle, target, road, segment, tmpLowestLane, distToSegmentEnd, distToCurrentTarget);
Triple<IAgent, Double, Boolean> newLeaderTriple =
findLeader(scope, vehicle, tmpLowestLane);
Triple<IAgent, Double, Boolean> newFollowerTriple =
findFollower(scope, vehicle, tmpLowestLane);

// If there are more lanes on the new road, we don't need to find a follower
// TODO: temp workaround, need to refactor these methods later
Triple<IAgent, Double, Boolean> newFollowerTriple;
IAgent currentRoad = DrivingSkill.getCurrentRoad(vehicle);
if (currentRoad == null ||
tmpLowestLane > RoadSkill.getNumLanesTotal(currentRoad) - numLanesOccupied) {
newFollowerTriple = ImmutableTriple.of(null, null, null);
} else {
newFollowerTriple = findFollower(
scope, vehicle, target, road, segment, tmpLowestLane, distToSegmentEnd, distToCurrentTarget);
}

if (newLeaderTriple == null || newFollowerTriple == null) {
if (newLeaderTriple.getMiddle() < 0 ||
newFollowerTriple.getMiddle() < 0) {
// Will crash into another vehicle if switch to this lane
continue;
}
Expand Down
236 changes: 122 additions & 114 deletions ...ools.gaml.extensions.traffic/src/simtools/gaml/extensions/traffic/carfollowing/Utils.java
View file
Expand Up @@ -9,6 +9,8 @@
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 static simtools.gaml.extensions.traffic.DrivingSkill.getLinkedLaneLimit;
import static simtools.gaml.extensions.traffic.DrivingSkill.getProbaUseLinkedRoad;

import org.apache.commons.collections4.OrderedBidiMap;
import org.apache.commons.lang3.tuple.ImmutableTriple;
Expand All @@ -35,70 +37,47 @@ public static double rescaleProba(final double probaInOneSecond,
return Math.min(probaInOneSecond * timeStep, 1.0);
}

public static int computeLinkedLaneLimit(final IAgent vehicle,
final IAgent road) {
double probaUseLinkedRoad = getProbaUseLinkedRoad(vehicle);
int linkedLaneLimit = getLinkedLaneLimit(vehicle);

IAgent linkedRoad = RoadSkill.getLinkedRoad(road);
int numLinkedLanes = (linkedRoad != null) ? RoadSkill.getNumLanes(linkedRoad) : 0;

if (linkedLaneLimit == -1) {
linkedLaneLimit = numLinkedLanes;
} else if (probaUseLinkedRoad == 0.0) {
linkedLaneLimit = 0;
} else {
linkedLaneLimit = Math.min(linkedLaneLimit, numLinkedLanes);
}
return linkedLaneLimit;
}

// TODO: this and findFollower don't need this many params
public static Triple<IAgent, Double, Boolean> 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) {
final int lowestLane) {
IAgent road = getCurrentRoad(vehicle);
IAgent target = getCurrentTarget(vehicle);
double distToCurrentTarget = getDistanceToCurrentTarget(vehicle);
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<Double, IAgent> 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;
if (road != null) {
Triple<IAgent, Double, Boolean> triple = findNeighborOnCurrentRoad(scope,
vehicle, lowestLane, true);
if (triple != null) {
return triple;
}
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

// No leading vehicle is found on the current road
IAgent leader = null;
double minGap = -Double.MAX_VALUE;
boolean sameDirection = false;
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
Expand Down Expand Up @@ -131,23 +110,24 @@ public static Triple<IAgent, Double, Boolean> findLeader(final IScope scope,
if (tmpLeader == null || tmpLeader.dead()) {
continue;
}
boolean tmpSameDirection = nextTarget == getCurrentTarget(tmpLeader);
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;
return ImmutableTriple.of(tmpLeader, gap, tmpSameDirection);
}
if (gap < minGap) {
minGap = gap;
leader = tmpLeader;
sameDirection = nextTarget == getCurrentTarget(tmpLeader);
sameDirection = tmpSameDirection;
}
}
}

if (leader != null && !leader.dead()) {
if (leader != null) {
return ImmutableTriple.of(leader, minGap, sameDirection);
} else {
// the road ahead seems to be completely clear
Expand All @@ -158,68 +138,26 @@ public static Triple<IAgent, Double, Boolean> findLeader(final IScope scope,

public static Triple<IAgent, Double, Boolean> 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) {
final int lowestLane) {
IAgent road = getCurrentRoad(vehicle);
IAgent target = getCurrentTarget(vehicle);
double distToCurrentTarget = getDistanceToCurrentTarget(vehicle);
if (road == null) {
return ImmutableTriple.of(null,null,null);
return ImmutableTriple.of(null, 1e6, false);
}
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<Double, IAgent> 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);
Triple<IAgent, Double, Boolean> triple = findNeighborOnCurrentRoad(scope,
vehicle, lowestLane, false);
if (triple != null) {
return triple;
}

// Find followers on previous roads
IAgent follower = null;
double minGap = Double.MAX_VALUE;
boolean sameDirection = false;
IAgent sourceNode = (target == RoadSkill.getTargetNode(road)) ?
RoadSkill.getSourceNode(road) : target;
for (IAgent prevRoad : RoadNodeSkill.getRoadsIn(sourceNode)) {
Expand Down Expand Up @@ -248,23 +186,93 @@ public static Triple<IAgent, Double, Boolean> findFollower(final IScope scope,
continue;
}

boolean tmpSameDirection = sourceNode == getCurrentTarget(tmpFollower);
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;
return ImmutableTriple.of(tmpFollower, gap, tmpSameDirection);
} else if (gap < minGap) {
minGap = gap;
follower = tmpFollower;
sameDirection = sourceNode == getCurrentTarget(tmpFollower);
sameDirection = tmpSameDirection;
}
}
}

if (follower != null && !follower.dead()) {
return ImmutableTriple.of(follower, minGap, sameDirection);
} else {
return ImmutableTriple.of(null, null, null);
return ImmutableTriple.of(null, 1e6, false);
}
}

private static Triple<IAgent, Double, Boolean> findNeighborOnCurrentRoad(
final IScope scope,
final IAgent vehicle,
final int lowestLane,
final boolean isLeader) {
IAgent road = getCurrentRoad(vehicle);
IAgent target = getCurrentTarget(vehicle);
double distToCurrentTarget = getDistanceToCurrentTarget(vehicle);
double vL = getVehicleLength(vehicle);
boolean violatingOneway = isViolatingOneway(vehicle);
int numRoadLanes = RoadSkill.getNumLanes(road);
int numLanesOccupied = getNumLanesOccupied(vehicle);

IAgent neighbor = null;
double minGap = Double.MAX_VALUE;
boolean sameDirection = false;
for (int i = 0; i < numLanesOccupied; i += 1) {
int lane = lowestLane + i;
OrderedBidiMap<Double, IAgent> 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;

// 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 ImmutableTriple.of(distMap.get(tmpDistQuery), -1.0, false);
}

Double k;
if (isLeader) {
k = !wrongDirection ? distMap.nextKey(tmpDistQuery)
: distMap.previousKey(tmpDistQuery);
} else {
k = !wrongDirection ? distMap.previousKey(tmpDistQuery)
: distMap.nextKey(tmpDistQuery);
}
// No neighbor on this lane
if (k == null) {
continue;
}
double tmpNeighborDist = k;
IAgent tmpNeighbor = distMap.get(tmpNeighborDist);
if (tmpNeighbor == null || tmpNeighbor.dead()) {
continue;
}
boolean tmpSameDirection = target == getCurrentTarget(tmpNeighbor);
double otherVL = getVehicleLength(tmpNeighbor);
double gap = Math.abs(tmpNeighborDist - tmpDistQuery) - 0.5 * vL - 0.5 * otherVL;

if (gap < 0) {
// Return immediately if crashing into another vehicle
return ImmutableTriple.of(tmpNeighbor, gap, tmpSameDirection);
} else if (gap < minGap) {
minGap = gap;
neighbor = tmpNeighbor;
sameDirection = tmpSameDirection;
}
}

if (neighbor != null) {
return ImmutableTriple.of(neighbor, minGap, sameDirection);
} else {
return null;
}
}
}
}

0 comments on commit 8d9a66a

Please sign in to comment.