def doStep():
state.step += 1
if setting.verbose:
print("step", state.step)
traci.simulationStep()
# adding new vehicles, if any
departed = traci.simulation.getSubscriptionResults()[
tc.VAR_DEPARTED_VEHICLES_IDS]
for v in departed:
traci.vehicle.subscribe(v, dataFromTraciState)
vehicleSetParams[v] = getSetParams(v)
if v == 'ego':
thisv = egov
else:
thisv = alterv
controllers[v] = Controllers.Inter1Constant(v, thisv)
#getController(v, vehicleSetParams[v])
if controllers[v] is not None:
if setting.verbose:
print("allowing forward and lane crashes for", v)
traci._sendIntCmd(tc.CMD_SET_VEHICLE_VARIABLE, tc.VAR_SPEEDSETMODE,
v, controllers[v].speedMode)
traci.vehicle.setLaneChangeMode(v, controllers[v].laneChangeMode)
# gather vehicle states for this step
vStates = {}
for vehID, subs in traci.vehicle.getSubscriptionResults().iteritems():
tempParams = getFromSubscription(vehID, subs, dataFromTraciState)
vStates[vehID] = VState(tempParams + vehicleSetParams[vehID])
#
for vehID, vState in vStates.iteritems():
sensor = sensorToUse(vState, realign=True)
#
for otherID, otherState in vStates.iteritems():
if vehID != otherID:
#
# check for collisions
if collisionCheck.check(vState, otherState):
#print "collision! pos",vState.x,vState.y,"step",state.step
if state.collisionOccurred <= 0:
state.collisionOccurred = state.step * .1
break
#
# update sensor
sensor.addObstacle(otherState)
#
if setting.verbose:
for vstat in sensor.getObstacles():
print(vehID, "detects", vstat.x, vstat.y, "speed", vstat.speed)
# store this vehicle movement
output.add([[
state.step * .1, vehID, vState.x, vState.y, vState.angle,
vState.speed
]])
# use data to decide speed
if controllers[vehID] is not None:
controllers[vehID].updateSpeed(vState.speed)
commandedSpeed = controllers[vehID].nextStep(sensor.getObstacles())
traci.vehicle.setSpeed(vehID, commandedSpeed)
if setting.verbose:
print("setting speed", commandedSpeed)
def nextStep(self, vehicles):
if len(self.speedCommand)==0:
self.newSpeedCommand()
mystate = None # first find own information
for vstate in vehicles:
if vstate.vehID == self.ID:
mystate = vstate2df(vstate)
if mystate is None:
print "no find self"
return self.speedCommand.pop()
mystate.time = 1
myfuture = self.egoPredictor.predict(mystate, np.arange(0.5,2.1,.1))
for vstate in vehicles: # check each other vehicle for collision
if vstate.vehID != self.ID:
otherfuture = self.predictor.predict(vstate2df(vstate),
np.arange(0.5,2.1,.1))
for ptime in range(myfuture.shape[0]):
myfutureloc = myfuture.iloc[ptime]
otherfutureloc = otherfuture.iloc[ptime]
#print 'checking'
#print str(myfutureloc.x)+", "+str(myfutureloc.y)
#print str(otherfutureloc.x)+", "+str(otherfutureloc.y)
if collisionCheck.check(myfutureloc, otherfutureloc):
print "collision detected in "+str(ptime/10.+.3)+" s"
self.speedCommand = ( list(np.arange(20/.447,0,-.1)) +
[0.]*5 +
list(np.arange(0,self.speed,.5)) )
self.speedCommand.pop()
return self.speedCommand.pop()
return self.speedCommand.pop()
def nextStep(self, vehicles):
if len(self.speedCommand) == 0:
self.newSpeedCommand()
mystate = None # first find own information
for vstate in vehicles:
if vstate.vehID == self.ID:
mystate = vstate2df(vstate)
if mystate is None:
print "no find self"
return self.speedCommand.pop()
mystate.time = 1
myfuture = self.egoPredictor.predict(mystate, np.arange(0.5, 2.1, .1))
for vstate in vehicles: # check each other vehicle for collision
if vstate.vehID != self.ID:
otherfuture = self.predictor.predict(vstate2df(vstate),
np.arange(0.5, 2.1, .1))
for ptime in range(myfuture.shape[0]):
myfutureloc = myfuture.iloc[ptime]
otherfutureloc = otherfuture.iloc[ptime]
#print 'checking'
#print str(myfutureloc.x)+", "+str(myfutureloc.y)
#print str(otherfutureloc.x)+", "+str(otherfutureloc.y)
if collisionCheck.check(myfutureloc, otherfutureloc):
print "collision detected in " + str(ptime / 10. +
.3) + " s"
self.speedCommand = (
list(np.arange(20 / .447, 0, -.1)) + [0.] * 5 +
list(np.arange(0, self.speed, .5)))
self.speedCommand.pop()
return self.speedCommand.pop()
return self.speedCommand.pop()
def doStep():
state.step += 1
if setting.verbose:
print("step",state.step)
traci.simulationStep()
# adding new vehicles, if any
departed = traci.simulation.getSubscriptionResults()[tc.VAR_DEPARTED_VEHICLES_IDS]
for v in departed:
traci.vehicle.subscribe(v,dataFromTraciState)
vehicleSetParams[v] = getSetParams(v)
if v=='ego':
thisv = egov
else:
thisv = alterv
controllers[v] = Controllers.Inter1Constant(v, thisv)
#getController(v, vehicleSetParams[v])
if controllers[v] is not None:
if setting.verbose:
print("allowing forward and lane crashes for",v)
traci._sendIntCmd(tc.CMD_SET_VEHICLE_VARIABLE, tc.VAR_SPEEDSETMODE,
v, controllers[v].speedMode)
traci.vehicle.setLaneChangeMode(v, controllers[v].laneChangeMode)
# gather vehicle states for this step
vStates = {}
for vehID, subs in traci.vehicle.getSubscriptionResults().iteritems():
tempParams = getFromSubscription(vehID, subs, dataFromTraciState)
vStates[vehID] = VState( tempParams + vehicleSetParams[vehID] )
#
for vehID, vState in vStates.iteritems():
sensor = sensorToUse(vState, realign=True)
#
for otherID, otherState in vStates.iteritems():
if vehID != otherID:
#
# check for collisions
if collisionCheck.check(vState, otherState):
#print "collision! pos",vState.x,vState.y,"step",state.step
if state.collisionOccurred <= 0:
state.collisionOccurred = state.step*.1
break
#
# update sensor
sensor.addObstacle(otherState)
#
if setting.verbose:
for vstat in sensor.getObstacles():
print(vehID, "detects", vstat.x, vstat.y,"speed",vstat.speed)
# store this vehicle movement
output.add([[state.step*.1, vehID, vState.x, vState.y, vState.angle,
vState.speed]])
# use data to decide speed
if controllers[vehID] is not None:
controllers[vehID].updateSpeed(vState.speed)
commandedSpeed = controllers[vehID].nextStep(sensor.getObstacles())
traci.vehicle.setSpeed(vehID, commandedSpeed)
if setting.verbose:
print("setting speed", commandedSpeed)
def doStep():
step.step += 1
if setting.verbose:
print "step", step.step
traci.simulationStep()
# adding new vehicles, if any
departed = traci.simulation.getSubscriptionResults()[tc.VAR_DEPARTED_VEHICLES_IDS]
for v in departed:
traci.vehicle.subscribe(v,dataFromTraciState)
vehicleSetParams[v] = getSetParams(v)
controllers[v] = getController(v, vehicleSetParams[v])
if controllers[v] is not None: #True
if setting.verbose:
print "allowing forward and lane crashes for",v
traci._sendIntCmd(tc.CMD_SET_VEHICLE_VARIABLE, tc.VAR_SPEEDSETMODE, v, 22)
traci.vehicle.setLaneChangeMode(v, 85)
#traci.vehicle.changeLane(v, 0, 10000)
# gather vehicle states for this step
vStates = {}
for vehID, subs in traci.vehicle.getSubscriptionResults().iteritems():
tempParams = getFromSubscription(vehID, subs, dataFromTraciState)
vStates[vehID] = VState( tempParams + vehicleSetParams[vehID] )
#
for vehID, vState in vStates.iteritems():
sensor = sensorToUse(vState)
#
for otherID, otherState in vStates.iteritems():
if vehID != otherID:
#
# check for collisions
if collisionCheck.check(vState, otherState):
print "collision! pos",vState.x,vState.y,"step",step.step
setting.collisionOccurred = True
break
#
# update sensor
sensor.addObstacle(otherState)
#
if setting.verbose:
for vstat in sensor.getObstacles():
print vehID, "detects", vstat.x, vstat.y,"speed",vstat.speed
# use data to decide speed
if controllers[vehID] is not None:
controllers[vehID].updateSpeed(vState.speed)
commandedSpeed = controllers[vehID].nextStep(sensor.getObstacles())
traci.vehicle.setSpeed(vehID, commandedSpeed)
if setting.verbose:
print "setting speed", commandedSpeed
# save
egoState = [ttime, 'ego', egoPos[0], egoPos[1], egoAngle, egoSpeed]
leftState = [ttime, 'left', leftPos[0], leftPos[1], leftAngle, leftSpeed]
if output is None:
output = pd.DataFrame([egoState, leftState])
else:
output = output.append([egoState, leftState])
# check for first collision - others too difficult to store..
if params_iter['Collision Time'].iloc[0] < 0:
egoObject = pd.Series(egoState[2:5] + list(VEHsize),
index=collisionCheck.collisionVars)
leftObject = pd.Series(leftState[2:5] + list(VEHsize),
index=collisionCheck.collisionVars)
if collisionCheck.check(egoObject,leftObject):
params_iter['Collision Time'].iloc[0] = ttime
params_iter['Collision Vehicle'].iloc[0] = 'left'
# construct next step
ttime += DELTAT
egoDist = egoSpeed*DELTAT
if Sim.getLaneInfo(egoLane)[0] - egoLanePos <= egoDist:
if egoLane[1]=='i': # ego entering intersection
if uniform(0,2)<1:
err += Sim.moveVehicleAlong('ego',egoDist, '2o_0')
else:
err += Sim.moveVehicleAlong('ego',egoDist, '2o_1')
elif egoLane[1]=='o': # remove vehicle or exit simulation
break
#else: # ego leaving intersection
def timeToAvoid(ego_v, ego_a, ego_wheel, alter_v, alter_a, alter_head,
alter_wheel, emergencyMoves):
# inputs are ints, except
# emergencyMoves = [... (accel, wheel), ...]
# output = shortest time at which collision can be avoided
# outputs None if time can't be calculated (no collision possible)
"""
# per-simulation parameters
ego_v = 1.
ego_a = 1.
ego_wheel = .3
alter_v = 1.
alter_a = 1.
alter_head = 1. # in radians
alter_wheel = .3
max_accel = 1.
max_decel = 1.
max_left = 1.
max_right = 1.
emergencyMoves = ((max_accel, 0), (max_accel, max_left), (max_accel, -max_right),
(-max_decel, 0), (-max_decel, max_left), (-max_decel, -max_right))
"""
# start at center facing East
ego = Vehicle(0, 0, ego_v, 0, ego_a, ego_wheel)
vsize = (VEHlength, VEHwidth)
# to initiate alter vehicle, need to find the exact beginning of collision
# this is computationally intractable
# instead start with some randomness and search for time of first collision
xcenter = ego.center()
alter = None
emergEscape = 0
while alter == None and emergEscape < 1000:
temp_alter = Vehicle(xcenter[0] + uniform(-VEHlength, VEHlength),
xcenter[1] + uniform(-VEHlength, VEHlength),
alter_v, alter_head, alter_a, alter_wheel)
# check recent past, find first moment of collision
timesBackwards = np.arange(-5, DELT, DELT)
egoPositions = ego.moveCA(timesBackwards)
alterPositions = temp_alter.moveCA(timesBackwards)
collisions = collisionCheck.check(egoPositions,alterPositions,vsize,vsize)
aa = next((i+1 for i in range(len(collisions)-1) if collisions[i+1]
and not collisions[i]), None)
if not aa is None: # never was a collision
ego.moveSelf(timesBackwards[aa])
alter = temp_alter
alter.moveSelf(timesBackwards[aa])
emergEscape = emergEscape + 1
if emergEscape == 1000:
return None
## now can use emergency movements
ttA = 0
avoided = False
while not avoided and ttA < 20:
ttA = ttA + DELT
past_ego = ego.copy()
past_ego.moveSelf(-ttA)
past_alter = alter.copy()
past_alter.moveSelf(-ttA)
# check up to 3 seconds past the original collision
timesAhead = np.arange(DELT, ttA+3., DELT)
alterPositions = past_alter.moveCA(timesAhead)
for movement in emergencyMoves:
past_ego.accel = movement[0]
past_ego.wheel = movement[1]
egoPositions = past_ego.moveCA(timesAhead)
collisions = collisionCheck.check(egoPositions,alterPositions,vsize,vsize)
if not np.any(collisions): # this movement was entirely safe
avoided = True
break
if not avoided:
return None
angleDifference = ego.head - alter.head
if angleDifference < -np.pi:
angleDifference = angleDifference + 2*np.pi
elif angleDifference > np.pi:
angleDifference = angleDifference - 2*np.pi
return [ego.speed, ego.accel, ego.wheel, angleDifference,
alter.speed, alter.accel, alter.wheel, ttA]
for bn in range(an):
print "checking " + str(an) + ", " + str(bn)
starta, enda, roada = intersectionLookUp[an]
startb, endb, roadb = intersectionLookUp[bn]
cara = allcars[roada]
carb = allcars[roadb]
abegin = 50
aend = -1
bbegin = 50
bend = -1
for acount in range(cara.shape[0]):
apos = cara['lp'].iloc[acount]
for bcount in range(carb.shape[0]):
bpos = carb['lp'].iloc[bcount]
if collisionCheck.check(cara.iloc[acount], carb.iloc[bcount]):
if abegin > apos:
abegin = apos
if bbegin > bpos:
bbegin = bpos
if aend < apos:
aend = apos
if bend < bpos:
bend = bpos
if aend > 0 and not starta == startb: # ignore cars from same lane
collisions.add([[roada, roadb, abegin, aend],
[roadb, roada, bbegin, bend]])
collisions.out().to_csv('collisionsFile_sumo_rect.csv',
sep=",",
header=True,
index=False)
# check for collisions
activeCars = np.where(cars['status'] == 0)[0]
for carNum in range(len(activeCars)):
carID = activeCars[carNum]
car = cars.iloc[carID]
for altNum in range(carNum):
altID = activeCars[altNum]
alt = cars.iloc[altID]
carObject = pd.Series(
[car['x'], car['y'], car['angle'], VEHsize[0], VEHsize[1]],
index=collisionCheck.collisionVars)
altObject = pd.Series(
[alt['x'], alt['y'], alt['angle'], VEHsize[0], VEHsize[1]],
index=collisionCheck.collisionVars)
if collisionCheck.check(carObject, altObject):
cars.loc[carID, 'status'] = 2 + altID
cars.loc[altID, 'status'] = 2 + carID
collisionCount += 1
zeroCollisionCount += car['zeroAction'] + alt['zeroAction']
carAtFault = findFault(cars.iloc[carID], cars.iloc[altID])
# carAtFault = 0 -> carID at fault
# carAtFault = 1 -> altID at fault
if carAtFault == 0:
cars.loc[carID, 'caratfault'] = 1
elif carAtFault == 1:
cars.loc[altID, 'caratfault'] = 1
elif carAtFault == 2:
cars.loc[carID, 'caratfault'] = 1
cars.loc[altID, 'caratfault'] = 1
collisionCount += 1
else:
cars['ilane'].iloc[carID] = carLane
# check for collisions
activeCars = np.where(cars['status']==0)[0]
for carNum in range(len(activeCars)):
carID = activeCars[carNum]
car = cars.iloc[carID]
for altNum in range(carNum):
altID = activeCars[altNum]
alt = cars.iloc[altID]
carObject = pd.Series([car['x'],car['y'],car['angle'],5.,2.],
index=collisionCheck.collisionVars)
altObject = pd.Series([alt['x'],alt['y'],alt['angle'],5.,2.],
index=collisionCheck.collisionVars)
if collisionCheck.check(carObject, altObject):
cars['status'].iloc[carID] = 2
cars['status'].iloc[altID] = 2
# gather when the vehicles reach potential collision points
trajectories = {}
for carID in np.where(cars['status']==0)[0]:
car = cars.iloc[carID]
route,grade,ln = splitRoad(car['lane'])
if grade=='i': # not in intersection yet
initDist = car['lanepos'] - Sim.getLaneInfo(car['lane'])[0]
else:
initDist = car['lanepos']
predictTimes = list( egoVehicleTrue[predictZone]['time'] )
# for ego vehicle, predict path
egoPredicted = egoPredictors.predict(egoVehicle, predictTimes)
# for each other vehicle, predict path
for vehID in otherIDs:
predictedPath = predictors[vehID].predict(allSensors[vehID],
predictTimes)
if not np.any(allSensors[vehID]['time'] <= time): # break if vehicle hasn't been sensed
break
# check for collision
for prediction in range(len(predictTimes)):
thisEgo = egoPredicted.iloc[prediction]
thisPath = predictedPath.iloc[prediction]
if collisionCheck.check(thisEgo, thisPath) and predictedCollision < 0:
predictedCollision = predictTimes[prediction]
predictedCollisionVehID = vehID
# !!!!!!!! Think about how to deal with multiple collision
# truth += [trueCollision]r
pred += [predictedCollision]
predVehID += [predictedCollisionVehID]
# basic scoring
nTP = 0
nTN = 0
nT = 0
nP = 0
ahead = 0
def applyPredictor(vehicleData, sensorData, egoVehicleID, minPredict, maxPredict,
trajectoryPredictor, trajectoryPredictorSettings,
egoPredictor = Predictors.GenericNoisePredict,
egoPredictorSettings = [0.]):
# print " Loading and modifying data ..."
vehicleData['length'] = pd.Series(VEHsize[0], index=vehicleData.index)
vehicleData['width'] = pd.Series(VEHsize[1], index=vehicleData.index)
sensorData['length'] = pd.Series(VEHsize[0], index=sensorData.index)
sensorData['width'] = pd.Series(VEHsize[1], index=sensorData.index)
# rearrange vehicle data into list of dataframes (by time)
timeVehicleData = []
currentTime = vehicleData.iloc[0]["time"]
timeList = [currentTime] # keeping ordered time seperately
lastIndex = 0
for ind in range(vehicleData.shape[0]):
if vehicleData.iloc[ind]["time"] > currentTime:
timeVehicleData += [ vehicleData.iloc[lastIndex:ind] ]
lastIndex = ind
currentTime = vehicleData.iloc[ind]["time"]
timeList = timeList + [currentTime]
if ind == vehicleData.shape[0] - 1:
timeVehicleData += [ vehicleData.iloc[lastIndex:ind+1] ]
# print " Finding collision times ..."
# # find collision times:
# trueCollision = -1
# for ind in range(len(timeList)):
# currentData = timeVehicleData[ind]
# isEgo = currentData['vehID']==egoVehicleID
# if np.any(isEgo) and not np.all(isEgo):
# egoVehicle = currentData[isEgo].iloc[0]
# otherVehs = currentData[isEgo==False]
# currentCollides = currentData[isEgo==False].apply(collisionCheck.check,
# axis=1, args=(egoVehicle,))
# # only store first instance of collision
# if np.any(currentCollides) and trueCollision < 0:
# trueCollision = timeList[ind]F
#print " Predicting collision ..."
# set up predictors, with true data for all time steps
egoVehicleTrue = vehicleData[vehicleData['vehID']==egoVehicleID]
egoPredictors = egoPredictor(egoVehicleTrue, *egoPredictorSettings)
predictors = {}
for vehID in np.unique(vehicleData['vehID']):
predictors[vehID] = trajectoryPredictor(
vehicleData[vehicleData['vehID']==vehID],
*trajectoryPredictorSettings)
for time in timeList[1:]:
# load data until current time
currSensorData = sensorData[sensorData['time'] <= time]
egoSensor = currSensorData[currSensorData['vehID']==egoVehicleID]
if egoSensor.shape[0]==0: # if no self-sensor, use true data
egoSensor = egoVehicleTrue[egoVehicleTrue['time'] <= time]
# rearrange sensor data into dict with names
allSensors = {} # All sensored data until current time
otherIDs = np.unique(currSensorData['vehID'])
otherIDs = list(vid for vid in otherIDs if not vid == egoVehicleID)
for vehID in otherIDs:
allSensors[vehID] = currSensorData[currSensorData['vehID']==vehID]
# Time to predict: current Time+min ~ current Time+max (s)
# time steps are extracted from ego Vehicle
predictZone = (egoVehicleTrue['time'] >= time+minPredict)&(
egoVehicleTrue['time'] <= time+maxPredict)
predictTimes = list( egoVehicleTrue[predictZone]['time'] )
# for ego vehicle, predict path
egoPredicted = egoPredictors.predict(egoSensor, predictTimes)
# for each other vehicle, predict path
for vehID in otherIDs:
predictedPath = predictors[vehID].predict(allSensors[vehID],
predictTimes)
if not np.any(allSensors[vehID]['time'] <= time): # break if vehicle hasn't been sensed
break
# check for collision
for prediction in range(len(predictTimes)):
thisEgo = egoPredicted.iloc[prediction]
thisPath = predictedPath.iloc[prediction]
if collisionCheck.check(thisEgo, thisPath):
return [predictTimes[prediction], vehID]
# !!!!!!!! Think about how to deal with multiple collision
return [-1, '']
#
#collisions.out().to_csv('collisionsFile_OwnSim.csv', sep=",",
# header=True, index=False)
collisions = WriteFrame(['lane','lane2','p1','p2','p3','p4'])
for an in range(len(intersections)):
for bn in range(an):
print "checking "+str(an)+", "+str(bn)
starta,enda = intersections[an]
startb,endb = intersections[bn]
roada = combineRoad(starta,enda)
roadb = combineRoad(startb,endb)
cara = allcars[roada]
carb = allcars[roadb]
points = []
for acount in range(cara.shape[0]):
for bcount in range(carb.shape[0]):
apos = cara['lp'].iloc[acount]
bpos = carb['lp'].iloc[bcount]
if collisionCheck.check(cara.iloc[acount], carb.iloc[bcount]):
points += [(apos, bpos)]
if len(points) > 0:
points = collisionHull.getHull(points)
collisions.add([[roada,roadb, points[0][0],points[1][0],
points[2][0],points[3][0]], [roadb,roada,
points[3][1],points[2][1],points[1][1],
points[0][1]]])
collisions.out().to_csv('collisionsFile_Quad.csv',sep=",",
header=True, index=False)
def timeToAvoid(ego_v, ego_a, ego_wheel, alter_v, alter_a, alter_head,
alter_wheel, emergencyMoves):
# inputs are ints, except
# emergencyMoves = [... (accel, wheel), ...]
# output = shortest time at which collision can be avoided
# outputs None if time can't be calculated (no collision possible)
"""
# per-simulation parameters
ego_v = 1.
ego_a = 1.
ego_wheel = .3
alter_v = 1.
alter_a = 1.
alter_head = 1. # in radians
alter_wheel = .3
max_accel = 1.
max_decel = 1.
max_left = 1.
max_right = 1.
emergencyMoves = ((max_accel, 0), (max_accel, max_left), (max_accel, -max_right),
(-max_decel, 0), (-max_decel, max_left), (-max_decel, -max_right))
"""
# start at center facing East
ego = Vehicle(0, 0, ego_v, 0, ego_a, ego_wheel)
vsize = (VEHlength, VEHwidth)
# to initiate alter vehicle, need to find the exact beginning of collision
# this is computationally intractable
# instead start with some randomness and search for time of first collision
xcenter = ego.center()
alter = None
emergEscape = 0
while alter == None and emergEscape < 1000:
temp_alter = Vehicle(xcenter[0] + uniform(-VEHlength, VEHlength),
xcenter[1] + uniform(-VEHlength, VEHlength),
alter_v, alter_head, alter_a, alter_wheel)
# check recent past, find first moment of collision
timesBackwards = np.arange(-5, DELT, DELT)
egoPositions = ego.moveCA(timesBackwards)
alterPositions = temp_alter.moveCA(timesBackwards)
collisions = collisionCheck.check(egoPositions, alterPositions, vsize,
vsize)
aa = next((i + 1 for i in range(len(collisions) - 1)
if collisions[i + 1] and not collisions[i]), None)
if not aa is None: # never was a collision
ego.moveSelf(timesBackwards[aa])
alter = temp_alter
alter.moveSelf(timesBackwards[aa])
emergEscape = emergEscape + 1
if emergEscape == 1000:
return None
## now can use emergency movements
ttA = 0
avoided = False
while not avoided and ttA < 20:
ttA = ttA + DELT
past_ego = ego.copy()
past_ego.moveSelf(-ttA)
past_alter = alter.copy()
past_alter.moveSelf(-ttA)
# check up to 3 seconds past the original collision
timesAhead = np.arange(DELT, ttA + 3., DELT)
alterPositions = past_alter.moveCA(timesAhead)
for movement in emergencyMoves:
past_ego.accel = movement[0]
past_ego.wheel = movement[1]
egoPositions = past_ego.moveCA(timesAhead)
collisions = collisionCheck.check(egoPositions, alterPositions,
vsize, vsize)
if not np.any(collisions): # this movement was entirely safe
avoided = True
break
if not avoided:
return None
angleDifference = ego.head - alter.head
if angleDifference < -np.pi:
angleDifference = angleDifference + 2 * np.pi
elif angleDifference > np.pi:
angleDifference = angleDifference - 2 * np.pi
return [
ego.speed, ego.accel, ego.wheel, angleDifference, alter.speed,
alter.accel, alter.wheel, ttA
]
# save
egoState = [ttime, 'ego', egoPos[0], egoPos[1], egoAngle, egoSpeed]
leadState = [ttime, 'lead', leadPos[0], leadPos[1], leadAngle, leadSpeed]
if output is None:
output = pd.DataFrame([egoState, leadState])
else:
output = output.append([egoState, leadState])
# check for first collision - others too difficult to store..
if params_iter['Collision Time'].iloc[0] < 0:
egoObject = pd.Series(egoState[2:5] + list(VEHsize),
index=collisionCheck.collisionVars)
leadObject = pd.Series(leadState[2:5] + list(VEHsize),
index=collisionCheck.collisionVars)
if collisionCheck.check(egoObject,leadObject):
params_iter['Collision Time'].iloc[0] = ttime
# construct next step
ttime += DELTAT
if egoSpeed > 0.:
err += Sim.moveVehicleAlong('ego', egoSpeed * DELTAT)
if leadSpeed > 0.:
err += Sim.moveVehicleAlong('lead', leadSpeed * DELTAT)
# check for ending of simulation
if lanelength - max(leadLanePos, egoLanePos) < 10:
# either vehicle is near the end of the road
break
if egoLanePos - leadLanePos > 100:
# ego passed lead a while ago
def applyPredictor(vehicleData,
sensorData,
egoVehicleID,
minPredict,
maxPredict,
trajectoryPredictor,
trajectoryPredictorSettings,
egoPredictor=Predictors.GenericNoisePredict,
egoPredictorSettings=[0.]):
# print " Loading and modifying data ..."
vehicleData['length'] = pd.Series(VEHsize[0], index=vehicleData.index)
vehicleData['width'] = pd.Series(VEHsize[1], index=vehicleData.index)
sensorData['length'] = pd.Series(VEHsize[0], index=sensorData.index)
sensorData['width'] = pd.Series(VEHsize[1], index=sensorData.index)
# rearrange vehicle data into list of dataframes (by time)
timeVehicleData = []
currentTime = vehicleData.iloc[0]["time"]
timeList = [currentTime] # keeping ordered time seperately
lastIndex = 0
for ind in range(vehicleData.shape[0]):
if vehicleData.iloc[ind]["time"] > currentTime:
timeVehicleData += [vehicleData.iloc[lastIndex:ind]]
lastIndex = ind
currentTime = vehicleData.iloc[ind]["time"]
timeList = timeList + [currentTime]
if ind == vehicleData.shape[0] - 1:
timeVehicleData += [vehicleData.iloc[lastIndex:ind + 1]]
# print " Finding collision times ..."
# # find collision times:
# trueCollision = -1
# for ind in range(len(timeList)):
# currentData = timeVehicleData[ind]
# isEgo = currentData['vehID']==egoVehicleID
# if np.any(isEgo) and not np.all(isEgo):
# egoVehicle = currentData[isEgo].iloc[0]
# otherVehs = currentData[isEgo==False]
# currentCollides = currentData[isEgo==False].apply(collisionCheck.check,
# axis=1, args=(egoVehicle,))
# # only store first instance of collision
# if np.any(currentCollides) and trueCollision < 0:
# trueCollision = timeList[ind]F
#print " Predicting collision ..."
# set up predictors, with true data for all time steps
egoVehicleTrue = vehicleData[vehicleData['vehID'] == egoVehicleID]
egoPredictors = egoPredictor(egoVehicleTrue, *egoPredictorSettings)
predictors = {}
for vehID in np.unique(vehicleData['vehID']):
predictors[vehID] = trajectoryPredictor(
vehicleData[vehicleData['vehID'] == vehID],
*trajectoryPredictorSettings)
for time in timeList[1:]:
# load data until current time
currSensorData = sensorData[sensorData['time'] <= time]
egoSensor = currSensorData[currSensorData['vehID'] == egoVehicleID]
if egoSensor.shape[0] == 0: # if no self-sensor, use true data
egoSensor = egoVehicleTrue[egoVehicleTrue['time'] <= time]
# rearrange sensor data into dict with names
allSensors = {} # All sensored data until current time
otherIDs = np.unique(currSensorData['vehID'])
otherIDs = list(vid for vid in otherIDs if not vid == egoVehicleID)
for vehID in otherIDs:
allSensors[vehID] = currSensorData[currSensorData['vehID'] ==
vehID]
# Time to predict: current Time+min ~ current Time+max (s)
# time steps are extracted from ego Vehicle
predictZone = (egoVehicleTrue['time'] >= time + minPredict) & (
egoVehicleTrue['time'] <= time + maxPredict)
predictTimes = list(egoVehicleTrue[predictZone]['time'])
# for ego vehicle, predict path
egoPredicted = egoPredictors.predict(egoSensor, predictTimes)
# for each other vehicle, predict path
for vehID in otherIDs:
predictedPath = predictors[vehID].predict(allSensors[vehID],
predictTimes)
if not np.any(allSensors[vehID]['time'] <= time
): # break if vehicle hasn't been sensed
break
# check for collision
for prediction in range(len(predictTimes)):
thisEgo = egoPredicted.iloc[prediction]
thisPath = predictedPath.iloc[prediction]
if collisionCheck.check(thisEgo, thisPath):
return [predictTimes[prediction], vehID]
# !!!!!!!! Think about how to deal with multiple collision
return [-1, '']
