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