def update_lrfol(veh):
"""After a vehicle's state has been updated, this updates its left and right followers.
It is recommended to use update_all_lrfol_multiple instead of this.
We keep each vehicle's left/right followers updated by doing a single distance compute each timestep.
The current way of dealing with l/r followers is designed for timesteps which are relatively small.
(e.g. .1 or .25 seconds) where we also keep l/rfol updated at all timesteps. Other strategies would be
better for larger timesteps or if l/rfol don't need to always be updated. See block comment
above update_change to discuss alternative strategies for defining leader/follower relationships.
See update_change documentation for explanation of naming conventions.
Called in simulation by update_all_lrfol.
update_all_lrfol_multiple can be used to handle edge cases so that the vehicle order is always correct,
but is slightly more computationally expensive.
The edge cases occur when vehicles overtake multiple vehicles in a single timestep.
Args:
veh: Vehicle to check its lfol/rfol to be updated.
Returns:
None. Modifies veh attributes, attributes of its lfol/rfol, in place.
"""
lfol, rfol = veh.lfol, veh.rfol
if lfol is None:
pass
elif get_dist(veh, lfol) > 0:
# update for veh
veh.lfol = lfol.fol
veh.lfol.rlead.append(veh)
lfol.rlead.remove(veh)
# update for lfol
lfol.rfol.llead.remove(lfol)
lfol.rfol = veh
veh.llead.append(lfol)
# similarly for right
if rfol is None:
pass
elif get_dist(veh, rfol) > 0:
veh.rfol = rfol.fol
veh.rfol.llead.append(veh)
rfol.llead.remove(veh)
rfol.lfol.rlead.remove(rfol)
rfol.lfol = veh
veh.rlead.append(rfol)
def update_rfol_recursive(veh, rfol, rovertaken):
"""Handles edge case 2 for update_all_lrfol_multiple by allowing rfol to update multiple times."""
if get_dist(veh, rfol) > 0:
veh.rfol = rfol.fol
veh.rfol.llead.append(veh)
rfol.llead.remove(veh)
if rfol in rovertaken:
rovertaken[rfol].append(veh)
else:
rovertaken[rfol] = [veh]
update_rfol_recursive(veh, rfol.fol, rovertaken)
def update_lfol_recursive(veh, lfol, lovertaken):
"""Handles edge case 2 for update_all_lrfol_multiple by allowing lfol to update multiple times."""
if get_dist(veh, lfol) > 0:
# update for veh
veh.lfol = lfol.fol
veh.lfol.rlead.append(veh)
lfol.rlead.remove(veh)
# handles edge case 1
if lfol in lovertaken:
lovertaken[lfol].append(veh)
else:
lovertaken[lfol] = [veh]
update_lfol_recursive(veh, lfol.fol)
def update_all_lrfol_multiple(vehicles):
"""Handles edge cases where a single vehicle overtakes 2 or more vehicles in a timestep.
In update_lrfol, when there are multiple overtakes in a single timestep (i.e. an l/rfol passes 2 or more
l/rlead vehicles, OR a vehicle is overtaken by its lfol, lfol.fol, lfol.fol.fol, etc.) the vehicle order
can be wrong for the next timestep. For every overtaken vehicle, 1 additional timestep with no overtakes
is sufficient but not necessary to correct the vehicle order. So if an ego vehicle overtakes 2 vehicles
on timestep 10, timestep 11 will have an incorrect order. More detail in comments.
This version is slightly slower than update_all_lrfol but handles the edge cases.
"""
# edge case 1 - a vehicle veh overtakes 2 llead, llead and llead.lead, in a timestep. If llead.lead
# has its update_lrfol called first, and then llead, the vehicle order will be wrong because veh will
# have llead as a rfol. If llead has its update_lrfol called first, the vehicle order will be correct.
# for the wrong order to be correct, there will need to be 1 timestep where veh does not overtake, so that
# the llead.lead will see veh overtook it and the order will be corrected. If, instead, veh overtakes
# llead.lead.lead, then again whether the order will be correct depends on the update order (it will
# be correct if llead.lead updates first, otherwise it will be behind 1 vehicle like before).
# edge case 2 - a vehicle veh is overtaken by lfol and lfol.fol in the same timestep. Here veh will have
# lfol.fol as its lfol. The order will always be corrected in the next timestep, unless another
# edge case (either 1 or 2) occurs.
# note edge case 2 is rarer and can potentially not be checked for - remove the update_recursive call
# and we won't check for it.
lovertaken = {
} # key with overtaking vehicles as keys, values are a list of vehicles the key overtook
# lovertaken = left overtaken meaning a vehicles lfol overtook
rovertaken = {} # same as lovertaken but for right side
# first loop we update all vehicles l/rfol and keep track of overtaking vehicles
for veh in vehicles:
lfol, rfol = veh.lfol, veh.rfol
if lfol is None:
pass
else:
update_lfol_recursive(veh, lfol, lovertaken)
# same for right side
if rfol is None:
pass
else:
update_rfol_recursive(veh, rfol, rovertaken)
#now to finish the order we have to update all vehicles which overtook
for lfol, overtook in lovertaken.items():
if len(
overtook
) == 1: # we know what lfol new rfol is - it can only be one thing
veh = overtook[0]
else:
distlist = [get_dist(veh, lfol) for veh in overtook]
ind = np.argmin(distlist)
veh = overtook[ind]
# update for lfol
# try/except is for rare edge case when lfol.rfol is None - occurs when you overtake and get new rlane
# in the same timestep
try:
lfol.rfol.llead.remove(lfol)
except:
pass
lfol.rfol = veh
veh.llead.append(lfol)
# same for right side
for rfol, overtook in rovertaken.items():
if len(
overtook
) == 1: # we know what lfol new rfol is - it can only be one thing
veh = overtook[0]
else:
distlist = [get_dist(veh, rfol) for veh in overtook]
ind = np.argmin(distlist)
veh = overtook[ind]
# update for rfol
try:
rfol.lfol.rlead.remove(rfol)
except:
pass
rfol.lfol = veh
veh.rlead.append(rfol)
def update_leadfol_after_lc(veh, lcsidelane, newlcsidelane, side, timeind):
"""Logic for updating all the leader/follower relationships for veh following a lane change.
Args:
veh: Vehicle object which changed lanes.
lcsidelane: The new lane veh changed onto.
newlcsidelane: The new lane on the lane change side for veh.
side: side of lane change, either left ('l') or right ('r').
timeind: int giving the timestep of the simulation (0 indexed)
Returns:
None. Modifies veh and any vehicles which have leader/follower relationships with veh in place.
"""
if side == 'l':
# define lcside/opside
lcsidefol, opsidefol, lcsidelead, opsidelead = 'lfol', 'rfol', 'llead', 'rlead'
else:
lcsidefol, opsidefol, lcsidelead, opsidelead = 'rfol', 'lfol', 'rlead', 'llead'
# update current leader
lead = veh.lead
fol = veh.fol
if lead is None:
pass
else:
lead.fol = fol
# update cur opposite/lc side leaders
for j in getattr(veh, opsidelead):
setattr(j, lcsidefol, fol)
for j in getattr(veh, lcsidelead):
setattr(j, opsidefol, fol)
# update cur follower
getattr(fol, lcsidelead).extend(getattr(veh, lcsidelead))
getattr(fol, opsidelead).extend(getattr(veh, opsidelead))
fol.lead = lead
fol.leadmem.append((lead, timeind + 1))
# update cur opposite side follower
vehopsidefol = getattr(veh, opsidefol)
if vehopsidefol is not None:
getattr(vehopsidefol, lcsidelead).remove(veh)
setattr(veh, opsidefol, fol)
getattr(fol, lcsidelead).append(veh)
# update cur lc side follower for vehicle
lcfol = getattr(veh, lcsidefol)
lclead = lcfol.lead
lcfol.lead = veh
lcfol.leadmem.append((veh, timeind + 1))
getattr(lcfol, opsidelead).remove(veh)
veh.fol = lcfol
# update cur lc side leader
veh.lead = lclead
veh.leadmem.append((lclead, timeind + 1))
if lclead is not None:
lclead.fol = veh
# new opside leaders
newleads = []
oldleads = getattr(lcfol, opsidelead)
for j in oldleads.copy():
curdist = get_dist(veh, j)
if curdist > 0:
setattr(j, lcsidefol, veh)
newleads.append(j)
oldleads.remove(j)
setattr(veh, opsidelead, newleads)
# new lcside leaders
newleads = []
oldleads = getattr(lcfol, lcsidelead)
mindist = math.inf
minveh = None
newlcsidelane_anchor = newlcsidelane.anchor if newlcsidelane is not None else None
for j in oldleads.copy():
curdist = get_dist(veh, j)
if curdist > 0:
setattr(j, opsidefol, veh)
newleads.append(j)
oldleads.remove(j)
if curdist < mindist and j.lane.anchor is newlcsidelane_anchor:
mindist = curdist
minveh = j # minveh is the leader of new lc side follower
setattr(veh, lcsidelead, newleads)
# update new lcside follower
if newlcsidelane is None:
setattr(veh, lcsidefol, None)
elif minveh is not None:
setattr(veh, lcsidefol, minveh.fol)
getattr(minveh.fol, opsidelead).append(veh)
else:
guess = get_guess(lcfol, lclead, veh, lcsidefol, newlcsidelane)
unused, newlcsidefol = lcsidelane.leadfol_find(veh, guess, side)
setattr(veh, lcsidefol, newlcsidefol)
getattr(newlcsidefol, opsidelead).append(veh)
def _chk_leadfol(self, verbose=True):
"""Returns True if the leader/follower relationships of the Vehicle are correct."""
# If verbose = True, we print whether each test is passing or not.
lfolpass = True
lfolmsg = []
if self.lfol is not None:
if self.lfol is self:
lfolpass = False
lfolmsg.append('lfol is self')
if self not in self.lfol.rlead:
lfolpass = False
lfolmsg.append('rlead of lfol is missing self')
if self.lfol.lane.anchor is not self.llane.anchor:
lfolpass = False
lfolmsg.append('lfol is not in left lane')
if get_dist(self, self.lfol) > 0:
lfolpass = False
lfolmsg.append('lfol is in front of self')
if self.lfol.lead is not None:
if get_dist(self, self.lfol.lead) < 0:
lfolpass = False
lfolmsg.append('lfol leader is behind self')
lead, fol = self.lane.leadfol_find(self, self.lfol)
if fol is not self.lfol:
if self.lfol.pos == self.pos:
pass
else:
lfolpass = False
lfolmsg.append('lfol is not correct vehicle - should be ' +
str(fol))
elif self.llane is not None:
unused, fol = self.llane.leadfol_find(self, self.llane.anchor)
lfolpass = False
lfolmsg.append('lfol is None - should be ' + str(fol))
rfolpass = True
rfolmsg = []
if self.rfol is not None:
if self.rfol is self:
rfolpass = False
rfolmsg.append('rfol is self')
if self not in self.rfol.llead:
rfolpass = False
rfolmsg.append('llead of rfol is missing self')
if self.rfol.lane.anchor is not self.rlane.anchor:
rfolpass = False
rfolmsg.append('rfol is not in right lane')
if get_dist(self, self.rfol) > 0:
rfolpass = False
rfolmsg.append('rfol is in front of self')
if self.rfol.lead is not None:
if get_dist(self, self.rfol.lead) < 0:
rfolpass = False
rfolmsg.append('rfol leader is behind self')
lead, fol = self.lane.leadfol_find(self, self.rfol)
if fol is not self.rfol:
if self.rfol.pos == self.pos:
pass
else:
rfolpass = False
rfolmsg.append('rfol is not correct vehicle - should be ' +
str(fol))
elif self.rlane is not None:
unused, fol = self.rlane.leadfol_find(self, self.rlane.anchor)
rfolpass = False
rfolmsg.append('lfol is None - should be ' + str(fol))
rleadpass = True
rleadmsg = []
for i in self.rlead:
if i.lfol is not self:
rleadpass = False
rleadmsg.append('rlead does not have self as lfol')
if get_dist(self, i) < 0:
rleadpass = False
rleadmsg.append('rlead is behind self')
if len(self.rlead) != len(set(self.rlead)):
rleadpass = False
rleadmsg.append('repeated rlead')
lleadpass = True
lleadmsg = []
for i in self.llead:
if i.rfol is not self:
lleadpass = False
lleadmsg.append('llead does not have self as rfol')
if get_dist(self, i) < 0:
lleadpass = False
lleadmsg.append('llead is behind self')
if len(self.llead) != len(set(self.llead)):
lleadpass = False
lleadmsg.append('repeated llead')
leadpass = True
leadmsg = []
if self.lead is not None:
if self.lead.fol is not self:
leadpass = False
leadmsg.append('leader does not have self as follower')
if get_headway(self, self.lead) < 0:
leadpass = False
leadmsg.append('leader is behind self')
folpass = True
folmsg = []
if self.fol.lead is not self:
folpass = False
folmsg.append('follower does not have self as leader')
if get_headway(self, self.fol) > 0:
folpass = False
folmsg.append('follower is ahead of self')
res = lfolpass and rfolpass and rleadpass and lleadpass and leadpass and folpass
if verbose:
if res:
print('passing results for ' + str(self))
else:
print('errors for ' + str(self))
if not lfolpass:
for i in lfolmsg:
print(i)
if not rfolpass:
for i in rfolmsg:
print(i)
if not rleadpass:
for i in rleadmsg:
print(i)
if not lleadpass:
for i in lleadmsg:
print(i)
if not leadpass:
for i in leadmsg:
print(i)
if not folpass:
for i in folmsg:
print(i)
return res