def reorderVehicles(vehicles, actualLeaders):
'''
reorderVehicles(list(pVehicle), list(pVehicle)) -> list(pVehicle)
This method reorders the given vehicles such that the newly ordered vehicles fulfill:
[None] + vehicles[:-1] == actualLeaders (if not several vehicles have the same actual leader.
For those it is only guaranteed that one will be associated correctly, not specifying which one)
'''
# if rp.VERBOSITY >= 4:
# report("reorderVehicles(vehicles, actualLeaders)\nvehicles = %s\nactualLeaders=%s" %
# (rp.array2String(vehicles), rp.array2String(actualLeaders)), 3)
done = False
# this is needed as abort criterion if two have the same leader (This cannot be excluded for sublane at least)
iter_count = 0
nVeh = len(vehicles)
# make a copy to write into
actualLeaders = list(actualLeaders)
while(not done and iter_count < nVeh):
newVehOrder = None
registeredLeaders = [None] + vehicles[:-1]
if rp.VERBOSITY >= 4:
report("vehicles: %s" % rp.array2String(vehicles), 3)
report("Actual leaders: %s" % rp.array2String(actualLeaders), 3)
report("registered leaders: %s" % rp.array2String(registeredLeaders), 3)
for (ego, registeredLeader, actualLeader) in reversed(
list(zip(vehicles, registeredLeaders, actualLeaders))):
if (ego == actualLeader):
if rp.VERBOSITY >= 1:
warn(("Platoon %s:\nVehicle '%s' was found as its own leader. " +
"Platoon order might be corrupted.") % (
rp.array2String(vehicles), str(ego)))
return vehicles
if actualLeader is None:
# No actual leader was found. Could be due to platoon LC maneuver or non-connected vehicle
# merging in
# -> no reordering implications.
continue
if actualLeader == registeredLeader:
continue
# intra-platoon order is corrupted
if newVehOrder is None:
# init newVehOrder
newVehOrder = list(vehicles)
# relocate ego
if rp.VERBOSITY >= 4:
report("relocating: %s to follow %s" % (str(ego), str(actualLeader)), 3)
report("prior newVehOrder: %s" % rp.array2String(newVehOrder), 3)
report("prior actualLeaders: %s" % rp.array2String(actualLeaders), 3)
oldEgoIndex = newVehOrder.index(ego)
del newVehOrder[oldEgoIndex]
del actualLeaders[oldEgoIndex]
if rp.VERBOSITY >= 4:
report("immed newVehOrder: %s" % rp.array2String(newVehOrder), 3)
report("immed actualLeaders: %s" % rp.array2String(actualLeaders), 3)
actualLeaderIndex = newVehOrder.index(actualLeader)
newVehOrder.insert(actualLeaderIndex + 1, ego)
actualLeaders.insert(actualLeaderIndex + 1, actualLeader)
if rp.VERBOSITY >= 4:
report("current newVehOrder: %s" % rp.array2String(newVehOrder), 3)
report("current actualLeaders: %s" % rp.array2String(actualLeaders), 3)
done = newVehOrder is None
if not done:
vehicles = newVehOrder
iter_count += 1
if iter_count != 0:
if rp.VERBOSITY >= 3:
report("Ordering within Platoon %s was corrupted.\nNew Order: %s\nLeaders: %s" %
(vehicles[0].getPlatoon().getID(), rp.array2String(vehicles), rp.array2String(actualLeaders)), 3)
return vehicles
def reorderVehicles(vehicles, actualLeaders):
'''
reorderVehicles(list(pVehicle), list(pVehicle)) -> list(pVehicle)
This method reorders the given vehicles such that the newly ordered vehicles fulfill:
[None] + vehicles[:-1] == actualLeaders (if not several vehicles have the same actual leader.
For those it is only guaranteed that one will be associated correctly, not specifying which one)
'''
# if rp.VERBOSITY >= 4:
# report("reorderVehicles(vehicles, actualLeaders)\nvehicles = %s\nactualLeaders=%s" %
# (rp.array2String(vehicles), rp.array2String(actualLeaders)), 3)
done = False
# this is needed as abort criterion if two have the same leader (This cannot be excluded for sublane at least)
iter_count = 0
nVeh = len(vehicles)
# make a copy to write into
actualLeaders = list(actualLeaders)
while(not done and iter_count < nVeh):
newVehOrder = None
registeredLeaders = [None] + vehicles[:-1]
if rp.VERBOSITY >= 4:
report("vehicles: %s" % rp.array2String(vehicles), 3)
report("Actual leaders: %s" % rp.array2String(actualLeaders), 3)
report("registered leaders: %s" % rp.array2String(registeredLeaders), 3)
for (ego, registeredLeader, actualLeader) in reversed(
list(zip(vehicles, registeredLeaders, actualLeaders))):
if (ego == actualLeader):
if rp.VERBOSITY >= 1:
warn(("Platoon %s:\nVehicle '%s' was found as its own leader. " +
"Platoon order might be corrupted.") % (
rp.array2String(vehicles), str(ego)))
return vehicles
if actualLeader is None:
# No actual leader was found. Could be due to platoon LC maneuver or non-connected vehicle
# merging in
# -> no reordering implications.
continue
if actualLeader == registeredLeader:
continue
# intra-platoon order is corrupted
if newVehOrder is None:
# init newVehOrder
newVehOrder = list(vehicles)
# relocate ego
if rp.VERBOSITY >= 4:
report("relocating: %s to follow %s" % (str(ego), str(actualLeader)), 3)
report("prior newVehOrder: %s" % rp.array2String(newVehOrder), 3)
report("prior actualLeaders: %s" % rp.array2String(actualLeaders), 3)
oldEgoIndex = newVehOrder.index(ego)
del newVehOrder[oldEgoIndex]
del actualLeaders[oldEgoIndex]
if rp.VERBOSITY >= 4:
report("immed newVehOrder: %s" % rp.array2String(newVehOrder), 3)
report("immed actualLeaders: %s" % rp.array2String(actualLeaders), 3)
actualLeaderIndex = newVehOrder.index(actualLeader)
newVehOrder.insert(actualLeaderIndex + 1, ego)
actualLeaders.insert(actualLeaderIndex + 1, actualLeader)
if rp.VERBOSITY >= 4:
report("current newVehOrder: %s" % rp.array2String(newVehOrder), 3)
report("current actualLeaders: %s" % rp.array2String(actualLeaders), 3)
done = newVehOrder is None
if not done:
vehicles = newVehOrder
iter_count += 1
if iter_count != 0:
if rp.VERBOSITY >= 3:
report("Ordering within Platoon %s was corrupted.\nNew Order: %s\nLeaders: %s" %
(vehicles[0].getPlatoon().getID(), rp.array2String(vehicles), rp.array2String(actualLeaders)), 3)
return vehicles
