def safeInit(self): self.previousBehavior = None self.lastVisionInput = None self.lastArduinoInput = None self.ballManager = BallManager() self.wallManager = WallManager() self.start = time.time() self.timeStackLastEmpty = time.time() self.maxTimeStackFilled = 30 # seconds self.stopPermanently = StopPermanently(self) #ensure that we have some input from other processes self.ard.lock.acquire() while True: self.lastArduinoInput = self.ard.otherConn.recv() if not self.ard.otherConn.poll(): break self.ard.lock.release() self.vision.lock.acquire() while True: self.lastVisionInput = self.vision.otherConn.recv() if not self.vision.otherConn.poll(): break self.vision.lock.release()
class BehaviorManager(StoppableThread):
"""Selects the robot's behavior (explore straight, approach ball, dump balls, etc) based on inputs, robot state, and time remaining in the game. The defaultBehaviors list contains all possible behaviors (it picks the highest priority behavior). If a behavior requires itself or another behavior to be run again it can add itself/other behaviors to the behaviorStack. Note that a behavior could be called indefinitely if it always added itself to the stack. After 3 minutes the BehaviorManager will stop the robot regardless of the contents of the behaviorStack."""
def __init__(self, ard, pilot, vision):
super(BehaviorManager, self).__init__("BehaviorManager")
self.ard = ard
self.pilot = pilot
self.vision = vision
self.defaultBehaviors = [DriveStraight(self), TurnToBall(self), ApproachBall(self)]#, ApproachYellowWall(self)]
self.behaviorStack = []
def safeInit(self):
self.previousBehavior = None
self.lastVisionInput = None
self.lastArduinoInput = None
self.ballManager = BallManager()
self.wallManager = WallManager()
self.start = time.time()
self.timeStackLastEmpty = time.time()
self.maxTimeStackFilled = 30 # seconds
self.stopPermanently = StopPermanently(self)
#ensure that we have some input from other processes
self.ard.lock.acquire()
while True:
self.lastArduinoInput = self.ard.otherConn.recv()
if not self.ard.otherConn.poll():
break
self.ard.lock.release()
self.vision.lock.acquire()
while True:
self.lastVisionInput = self.vision.otherConn.recv()
if not self.vision.otherConn.poll():
break
self.vision.lock.release()
def safeRun(self):
# update input information from other processes
if self.ard.lock.acquire(1):
while self.ard.otherConn.poll():
self.lastArduinoInput = self.ard.otherConn.recv()
self.ard.lock.release()
if self.vision.lock.acquire(1):
while self.vision.otherConn.poll():
self.lastVisionInput = self.vision.otherConn.recv()
self.vision.lock.release()
self.ballManager.update(self.lastVisionInput.balls)
self.wallManager.update(self.lastVisionInput.walls)
# select the next behavior
if time.time()-self.start > 3*60: # Force the stop state when the match ends
behavior = self.stopPermanently
elif time.time()-self.timeStackLastEmpty > self.maxTimeStackFilled: # If the stack has been occupied too long, assume something went wrong and reset stuff
self.behaviorStack = []
self.timeStackLastEmpty = time.time()
behavior = ForcedMarch(self)
elif len(self.behaviorStack) > 0: # take the next behavior off the stack
behavior = self.behaviorStack.pop()
else: # select the default state with the highest priority
self.timeStackLastEmpty = time.time()
behavior = None
priority = None
for b in self.defaultBehaviors:
p = b.getPriority()
if priority==None or p > priority:
behavior = b
priority = p
# print behavior changes
if self.previousBehavior != behavior:
print "Changed behavior: {0}".format(behavior)
self.logger.info("Changed states: {0}".format(behavior))
# execute behavior
if behavior != None:
output = behavior.execute(self.previousBehavior)
if output != None:
if self.pilot.lock.acquire(1):
self.pilot.otherConn.send(output)
self.pilot.lock.release()
self.previousBehavior = behavior
def cleanup(self):
pass