class Handler: def __init__(self, powerLever, disconnect, realWorld): if realWorld: self.powers = {'Discover':95,'Race': 160, 'Brake':95, 'Turn':110, 'SpeedLimit':250} self.smoothGyroZThresh = 2200 #Power increase per step when the car is not moving self.deltaPower = 0.2 else: self.powers = {'Discover':105,'Race': 160, 'Brake':70, 'Turn':100, 'SpeedLimit':280} self.smoothGyroZThresh = 1500 #Power increase per step when the car is not moving self.deltaPower = 0.2 self.maxPower = self.powers['Race'] self.requestedPower = '' self.powerlever = powerLever self.disconnect = disconnect self.discoveryPhase = True self.rediscoveryPhase = False self.trackTokens = "" self.filterM = 5 self.filterSInv = 1.0/(1.0/2.0*self.filterM*(self.filterM+1)) self.filterData = [0 for i in range(self.filterM)] self.tokenstream = ['0'] self.tokendistance = [0] self.trackTokenPath = ['0'] self.currentToken = ['0'] self.lenTrackTokenPath = 0 self.lenTrackTockenPathThresh = 60; #If the TrackTockenPath is larger than this, a new discovery will be triggered self.rediscoveryFlag = False self.rediscoveryCount = 0 self.rediscoveryCountThresh = 3 #Varaiables to recognize that the car has stopped self.carNotMovingFlag = False self.zeroGyroZthresh = 30 self.zeroGyroZcount = 0 self.zeroGyroZcountThresh = 20 self.carNotMovingPiecewiseSpeedIncrement = 0 self.carNotMovingPiecewiseSpeedIncrementThresh = 20 self.carNotMovingSpeedBoostDuration = 0 self.carNotMovingSpeedBoostDurationThresh = 50 self.carNotMovingPowerName = '' #Settingso self.discardedTokenstreamEntries = 5 #The first self.discardedTokenstreamEntries in TokenStream will not be considered for the mapping self.logger = Logger(realWorld) self.startTime = time() self.alphas = [] self.timeRaceStart = 0 self.racing = False self.lastTokens = ['','','',''] self.itLastTokens = 0 self.raceStartTime = 0 def receive(self, msg): print("received: ", msg) def sensorDataArrivedEvent(self, msg): #self.receive(msg) self.logger.logSensorData(msg) gyroZ = msg['g'][2] tmp2 = 0 for i in range(1,self.filterM): tmp2 = tmp2 + (self.filterM-i)*self.filterData[i-1] for i in range(self.filterM-1,0, -1): self.filterData[i] = self.filterData[i-1] self.filterData[0] = gyroZ smoothGyroZ = self.filterSInv*(self.filterM * gyroZ + tmp2) if self.discoveryPhase: self.requestedPower = 'Discover' #self.setPower(self.discoveryPhasePower) #print(gyroZ,' ',smoothGyroZ) if self.growTokenStream(smoothGyroZ): self.analyseTokens() self.tokendistance.append((time()-self.startTime) * self.powers['Discover']) # distance equivalent since start of vehicle elif self.rediscoveryPhase: self.requestedPower = 'Discover' self.rediscoveryFlag = True if self.growTokenStream(smoothGyroZ) and len(self.tokenstream) > self.discardedTokenstreamEntries: print('Looking for unique '+''.join(self.tokenstream[self.discardedTokenstreamEntries:])+' in '+ ''.join(self.trackTokenPath)) # Finds indices of string we are looking for in bigger string. If there is only one index, we found the location tokenstreamOccurrences = [m.start() for m in re.finditer('(?='+''.join(self.tokenstream[self.discardedTokenstreamEntries:])+')', ''.join(self.trackTokenPath))] if(len(tokenstreamOccurrences) == 1): self.currentTokenKey = tokenstreamOccurrences[0] self.rediscoveryPhase = False elif len(self.tokenstream[self.discardedTokenstreamEntries:]) > len(self.trackTokenPath) or len(tokenstreamOccurrences) == 0: print('Rediscovery failed, attempting full discovery') self.rediscoveryPhase = False self.discoveryPhase = True else: # Save the 4 most recent tokens self.lastTokens[self.itLastTokens] = self.trackTokenPath[self.currentTokenKey] self.itLastTokens += 1 if self.itLastTokens == 4: self.itLastTokens = 0 # Detect current situation if smoothGyroZ >= self.smoothGyroZThresh: self.currentToken = 'R' elif smoothGyroZ <= -self.smoothGyroZThresh: self.currentToken = 'L' elif abs(smoothGyroZ) < self.smoothGyroZThresh: self.currentToken = 'S' # If the current situation changes, check whether the new state is the one we expected. if self.currentToken != self.trackTokenPath[self.currentTokenKey % self.lenTrackTokenPath]: if self.currentToken == self.trackTokenPath[(self.currentTokenKey+1) % self.lenTrackTokenPath]: print('Changing to ',self.currentToken,' as predicted') self.currentTokenKey = (self.currentTokenKey+1)% self.lenTrackTokenPath else: if self.trackTokenPath[self.currentTokenKey % self.lenTrackTokenPath] == 'R': expectedSmoothGyroZstr = '<= '+str(self.smoothGyroZThresh) elif self.trackTokenPath[self.currentTokenKey % self.lenTrackTokenPath] == 'L': expectedSmoothGyroZstr = '>= '+str(self.smoothGyroZThresh) else: expectedSmoothGyroZstr = '|| < '+str(self.smoothGyroZThresh) print('Failed to predict state. Expected '+expectedSmoothGyroZstr+', actual: '+str(smoothGyroZ)+' Going back to discovery') self.tokenstream = ['0'] if self.rediscoveryFlag: self.rediscoveryCount = self.rediscoveryCount +1 #If rediscovery fails repeatedly, try a full discovery if self.rediscoveryCount > self.rediscoveryCountThresh: self.discoveryPhase = True print('Rediscovery failed '+str(self.rediscoveryCount)+' times, attempting full recovery') self.rediscoveryCount = 0 else: self.rediscoveryPhase = True return if self.currentToken == 'S' and not self.rediscoveryPhase: self.timeRaceStart = time() self.requestedPower = 'Race' #self.setPower(self.uRace) self.racing = True print("Started racing") else: self.requestedPower = 'Turn' #self.setPower(self.uTurn) self.racing = False if self.racing: distanceAhead = self.tokendistance[(self.currentTokenKey+1) % self.lenTrackTokenPath] - self.tokendistance[self.currentTokenKey] if (time() - self.timeRaceStart) > self.alphas[self.currentTokenKey] * distanceAhead / self.powers['Race']: self.requestedPower = 'Brake' #self.setPower(self.uBrake) self.racing = False #print('We are at ',self.currentToken, ', next will be ',self.trackTokenPath[(self.currentTokenKey+1) % self.lenTrackTokenPath]) #If the car does not move, augment smoothGyroZ,else decrease it if abs(smoothGyroZ) < self.zeroGyroZthresh: self.zeroGyroZcount = min(self.zeroGyroZcount + 1,self.zeroGyroZcountThresh); else: self.zeroGyroZcount = max(self.zeroGyroZcount -1,0) #If the car has not moved for more than self.zeroGyroZcountThresh steps,raise a flag. If the flag has been raised, wait until the car # has been moving for at least self.zeroGyroZcountThresh/2 before removing the flag if not self.carNotMovingFlag: if self.zeroGyroZcount >= self.zeroGyroZcountThresh and time()-self.raceStartTime > 1: self.carNotMovingFlag = True self.carNotMovingPowerName = self.requestedPower print('Car is not moving') else: if self.zeroGyroZcount < self.zeroGyroZcountThresh/2.0: self.carNotMovingSpeedBoostDuration = 0 self.carNotMovingPiecewiseSpeedIncrement = 0 self.carNotMovingFlag = False print('Car is moving again') #If the car is not moving, increment its current speed if self.carNotMovingFlag: if self.carNotMovingPiecewiseSpeedIncrement < self.carNotMovingPiecewiseSpeedIncrementThresh: if self.carNotMovingPowerName == self.requestedPower: self.powers[self.requestedPower] = self.powers[self.requestedPower] + self.deltaPower self.carNotMovingPiecewiseSpeedIncrement = self.carNotMovingPiecewiseSpeedIncrement + self.deltaPower print('Increasing '+self.requestedPower+' to '+str(round(self.powers[self.requestedPower],2))+' to get the car moving again') else: #Stop intervening if the speed changed self.carNotMovingFlag = False self.carNotMovingSpeedBoostDuration = 0 self.carNotMovingPiecewiseSpeedIncrement = 0 print('Speed changed, stopping intervention') else: #If speed was increased but still no movement, try to jerk speed up if self.carNotMovingSpeedBoostDuration < self.carNotMovingSpeedBoostDurationThresh: self.requestedPower = 'Race' self.carNotMovingSpeedBoostDuration = self.carNotMovingSpeedBoostDuration + 1 print('Trying to boost speed to get the car moving again') else: #Go back to increasing speed self.requestedPower = self.carNotMovingPowerName self.carNotMovingSpeedBoostDuration = 0 self.carNotMovingPiecewiseSpeedIncrement = 0 print('Going back to increasing speed') self.setPower(int(self.powers[self.requestedPower])) def growTokenStream(self,smoothGyroZ): if smoothGyroZ >= self.smoothGyroZThresh and self.tokenstream[-1] != 'R': self.tokenstream.append('R') return True elif smoothGyroZ <= -self.smoothGyroZThresh and self.tokenstream[-1] != 'L': self.tokenstream.append('L') return True elif abs(smoothGyroZ) < self.smoothGyroZThresh and self.tokenstream[-1] != 'S': self.tokenstream.append('S') return True def findPattern(self, s): i = (s+s).find(s,1,-1) return None if i == -1 else s[:i] def analyseTokens(self): if len(self.tokenstream) > self.discardedTokenstreamEntries: print(''.join(self.tokenstream[self.discardedTokenstreamEntries:])) tmp = self.findPattern(''.join(self.tokenstream[self.discardedTokenstreamEntries:])) if tmp != None: if len(tmp) > 4: print("Pattern detected!") self.discoveryPhase = False self.trackTokenPath = tmp self.lenTrackTokenPath = len(tmp) index = ''.join(self.tokenstream).find(tmp) self.tokendistance = self.tokendistance[index:(index+self.lenTrackTokenPath+1)] self.alphas = [0.6 for i in range(self.lenTrackTokenPath)] self.currentTokenKey = self.lenTrackTokenPath - 1 else: if len(self.tokenstream) > self.lenTrackTockenPathThresh: self.tokenstream = ['0'] print('tokenstream length exceeds lenTrackTockenPathThresh, restarting discovery and reducing speed') self.powers['Discover'] = self.powers['Discover'] - 10 def setPower(self, power): if power > self.maxPower: print("ATTENTION: power saturation, requested "+str(power)) power = self.maxPower self.logger.logPowerData((localtime(),power)) self.powerlever(power) def raceStoppedEvent(self, msg): self.logger.destructor() def velocityDataArrivedEvent(self, msg): if (self.discoveryPhase or self.rediscoveryPhase): if msg['velocity'] < 75 and (time() - self.raceStartTime) > 0.5: #self.discoveryPhasePower += 10 if self.powers['Discover'] + 10 < self.maxPower: self.powers['Discover'] += 10 print("Increased discovery phase power to",round(self.powers['Discover'],2)) #elif msg['velocity'] > 3.0/4*self.powers['SpeedLimit']: #self.powers['Discover'] -= 5 #print("Decreased discovery phase power to",round(self.powers['Discover'],2)) else: if self.lastTokens[self.itLastTokens-3] == 'S': print("Just left",self.trackTokenPath[self.currentTokenKey]) if self.alphas[self.currentTokenKey] < 0.95 and 0.9 * self.powers['SpeedLimit'] > msg['velocity']: self.alphas[self.currentTokenKey] += 0.05 print('Increasing alpha '+str(self.currentTokenKey)+' to '+str(round(self.alphas[self.currentTokenKey],2))) else: if msg['velocity'] < self.powers['SpeedLimit']/2: if self.powers['Turn'] + 5 < self.maxPower: self.powers['Turn'] += 5 print('Increased turn speed to',round(self.powers['Turn'],2)) else: self.powers['Turn'] -= 3 print('Decreased turn speed to',round(self.powers['Turn'],2)) self.logger.logVelocityData(msg) def penaltyReceivedEvent(self, msg): print("PENALTY", msg['actualSpeed']) # Adapt speed limit self.powers['SpeedLimit'] = 0.9 * msg['speedLimit'] # Adapt current alpha if self.racing: if self.trackTokenPath[self.currentTokenKey] == 'S': self.alphas[self.currentTokenKey] -= 0.09 elif self.trackTokenPath[self.currentTokenKey-1] == 'S': self.alphas[self.currentTokenKey-1] -= 0.09 # Log self.logger.logPenaltyData(msg) def startReceivedEvent(self, msg): self.raceStartTime = time() self.logger.logStartData(msg) def stopReceiveEvent(self, msg): self.logger.destructor() self.disconnect()