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()
