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
# destLane = Sim.getLaneInfo(egoLane)[2][0]
# err += Sim.moveVehicleAlong('ego',egoDist,destLane)
elif egoDist > 0.:
err += Sim.moveVehicleAlong('ego', egoDist)
leftDist = leftSpeed*DELTAT
if Sim.getLaneInfo(leftLane)[0] - leftLanePos < leftDist:
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']
trajectory = []
for crossIndex in range(intersectionInfo.shape[0]):
thisCross = intersectionInfo.iloc[crossIndex]
if thisCross['lane'] == car['ilane']:
timeToCrossingStart = (thisCross['begin_lp'] -
initDist) / car['speed']
timeToCrossingEnd = (thisCross['end_lp'] -
initDist) / car['speed']
trajectory += [[thisCross['lane2'],
timeToCrossingStart, timeToCrossingEnd]]
trajectories[carID] = trajectory
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
# destLane = Sim.getLaneInfo(egoLane)[2][0]
# err += Sim.moveVehicleAlong('ego',egoDist,destLane)
elif egoDist > 0.:
err += Sim.moveVehicleAlong('ego', egoDist)
leftDist = leftSpeed * DELTAT
if Sim.getLaneInfo(leftLane)[0] - leftLanePos < leftDist:
egoControl = Controllers.RearEndRandom(vehicleSpeed(),maxdecel=.5)
err += Sim.createVehicle('lead','main_0',50+VEHsize[0])
#leadControl = Controllers.RearEndBrake(vehicleSpeed(), vehicleAccel())
leadControl = Controllers.RearEndRandom(vehicleSpeed(),maxdecel=5.)
# params_iter = pd.DataFrame([[egoControl.speed, leadControl.speed,
# egoControl.accel, leadControl.accel,
# leadControl.brakeMagnitude, leadControl.brakeTime,
# -1.]], columns=ParameterColumns)
params_iter = pd.DataFrame([[egoControl.speed, leadControl.speed, -1.]],
columns=ParameterColumns)
output = None
ttime = 0
maxTime = 100 # seconds
lanelength = Sim.getLaneInfo('main_0')[0]
## run simulation
while ttime < maxTime:
# get info on this step
aa,egoLanePos,egoPos,egoAngle = Sim.getVehicleState('ego')
aa,leadLanePos,leadPos,leadAngle = Sim.getVehicleState('lead')
egoControl.update(DELTAT)
leadControl.update(DELTAT)
egoSpeed = egoControl.nextStep()
leadSpeed = leadControl.nextStep()
# save
egoState = [ttime, 'ego', egoPos[0], egoPos[1], egoAngle, egoSpeed]
leadState = [ttime, 'lead', leadPos[0], leadPos[1], leadAngle, leadSpeed]
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']
trajectory = []
for crossIndex in range(intersectionInfo.shape[0]):
thisCross = intersectionInfo.iloc[crossIndex]
if thisCross['lane'] == car['ilane']:
timeToCrossingStart = (thisCross['begin_lp'] -
initDist) / car['speed']
timeToCrossingEnd = (thisCross['end_lp'] -
initDist) / car['speed']
trajectory += [[
thisCross['lane2'], timeToCrossingStart,
timeToCrossingEnd
]]
