示例#1
0
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()
示例#3
0
    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()
示例#4
0
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)
示例#5
0
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
示例#6
0
 
 # 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
示例#7
0
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)
示例#9
0
        # 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
示例#10
0
     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']
     
示例#11
0
        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)
示例#14
0
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
    ]
示例#15
0
 
 # 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
示例#16
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, '']