def runMachine(start):
eventHub = core.EventHub()
qeventHub = core.QueuedEventHub(eventHub)
subsystems = core.SubsystemList()
subsystems.append(qeventHub)
machine = state.Machine(deps=subsystems)
sequence = raw_input('Enter sequence: ')
machine.start(start)
assert machine.currentState() is not None, 'State machine not started'
for x in sequence:
eventType = globals()[x.upper()]
qeventHub.publish(eventType, core.Event())
qeventHub.publishEvents()
assert machine.currentState(
) is not None, 'State machine ended prematurely'
# State machine finished, send finish event
qeventHub.publish(FINISH, core.Event())
qeventHub.publishEvents()
assert machine.currentState() is None, 'State machine has not ended'
machine.stop()
def update(self, timestep):
self._currentTime += timestep
sinVal = math.sin(self._currentTime + self._offset)
self.voltage = 2.0 * sinVal + 26
self.current = 5.0 * sinVal + 5
if sinVal >= 0:
self.enabled = True
self.publish(device.IPowerSource.ENABLED, core.Event())
else:
self.enabled = False
self.publish(device.IPowerSource.DISABLED, core.Event())
if math.sin(self._currentTime + self._offset + 1) >= 0:
self.used = True
self.publish(device.IPowerSource.USING, core.Event())
else:
self.used = False
self.publish(device.IPowerSource.NOT_USING, core.Event())
event = ext.math.NumericEvent()
event.number = self.voltage
self.publish(device.IVoltageProvider.UPDATE, event)
event = ext.math.NumericEvent()
event.number = self.current
self.publish(device.ICurrentProvider.UPDATE, event)
def CUPID_FOUND(self, event):
# grab the color, fire the appropriate torpedo
if event.color == vision.Color.ColorType.BLUE:
self.vehicle.fireTorpedo(1)
self.publish(Fire2.FIRED, core.Event())
elif event.color == vision.Color.ColorType.RED:
self.vehicle.fireTorpedo(2)
self.publish(Fire2.FIRED, core.Event())
def TIMEOUT(self, event):
# Figure out what state it was in when it timed out
if self._state == StagedTemplate.SEARCHING:
self.publish(StagedTemplate.EARLY_TIMEOUT, core.Event())
elif self._state == StagedTemplate.LOST:
self.publish(StagedTemplate.LOST_TIMEOUT, core.Event())
else:
# Normal timeout is the default if an improper state was set
self.publish(StagedTemplate.NORMAL_TIMEOUT, core.Event())
def enter(self):
# Interstate data is stored here
# All previous data will be lost
self.ai.data['light'] = {}
# TODO: Make 'buoy' a configurable value
if self.vehicle.hasObject('buoy'):
self.publish(Start.SUCCESS, core.Event())
else:
self.publish(Start.FAIL, core.Event())
def testSubscribeToType(self):
recv = Reciever()
self.ehub.subscribeToType("Type", recv)
self.assertEquals(0, recv.calls)
self.epubA.publish("Type", core.Event())
self.assertEquals(1, recv.calls)
self.epubB.publish("Type", core.Event())
self.assertEquals(2, recv.calls)
def testSubscrie(self):
recv = Reciever()
self.qehub.subscribe('A', self.epubA, recv)
self.assertEquals(0, recv.calls)
self.epubB.publish('A', core.Event())
self.assertEquals(0, recv.calls)
self.epubA.publish('A', core.Event())
self.assertEquals(0, recv.calls)
self.qehub.publishEvents()
self.assertEquals(1, recv.calls)
def testSubscribe(self):
recvA = Reciever()
recvB = Reciever()
self.ehub.subscribe("Type", self.epubA, recvA)
self.ehub.subscribe("Type", self.epubB, recvB)
self.epubA.publish("Type", core.Event())
self.assertEquals(1, recvA.calls)
self.assertEquals(0, recvB.calls)
self.epubB.publish("Type", core.Event())
self.assertEquals(1, recvA.calls)
self.assertEquals(1, recvB.calls)
def enter(self, timeout = 2):
# Turn off all motions, hold the current heading
self.motionManager.stopCurrentMotion()
self.controller.holdCurrentHeading()
if self._config.get('holdDepth', False):
self.controller.holdCurrentDepth()
self.controller.setSpeed(0)
self.controller.setSidewaysSpeed(0)
# Create a timer event
self._timeout = self._config.get('timeout', timeout)
# Timer will only state if the timeout is a positive number
# A timer of 0 will turn it off, along with any negative number
if self._timeout > 0:
self.timer = \
self.timerManager.newTimer(FindAttempt.TIMEOUT, self._timeout)
self.timer.start()
elif self._timeout < 0:
# A negative timer will automatically move to the timeoutState
self.timer = None
self.publish(FindAttempt.TIMEOUT, core.Event())
else:
# A timer of zero will turn off the timer and will only allow
# FindAttempt to exit by finding the target
self.timer = None
def FINISHED(self, event):
#Changes whether the robot strafes or moves forward
if (self._stateOfMovement >= 1):
self._stateOfMovement = 0
else:
self._stateOfMovement = self._stateOfMovement + 1
#Making sure the robot doesn't go past the height bounding box
if (self._tdt < self._boundingHeight):
if (self._stateOfMovement == 0):
self.Forward()
else:
#Switch the direction of the strafe
if (self._strafeDirection == 1):
self._strafeDirection = -1
else:
self._strafeDirection = 1
if (self._firstMovement == 1): #Will only happen once
self._firstMovement = 5
self.Strafe(self._strafeDirection, 2)
else:
self.Strafe(self._strafeDirection, 1)
#end the program
else:
self.publish(ZigZag.DONE, core.Event())
def _finish(self):
"""
Raises, motion finished event.
"""
event = core.Event()
event.motion = self
self.publish(Motion.FINISHED, event)
def testLookupByName(self):
self.called = False
def recieve(event):
self.called = True
# Create a publisher with an event hub
eventHub = core.EventHub()
mypub = core.EventPublisher(eventHub = eventHub,
name = TestEventPublisher.EVTP_NAME)
# Look it up and make sure it works
epub = core.EventPublisher.lookupByName(TestEventPublisher.EVTP_NAME)
self.assertNotEquals(None, epub)
self.assertEquals(TestEventPublisher.EVTP_NAME, epub.getPublisherName())
# Subscribe to the new one (which is wrapper of the same underlying
# C++ object as self.epub)
# BUG: This line below should work instead of the one two down
# eventHub.subscribe("TestEvent", epub, recieve)
eventHub.subscribe("TestEvent", mypub, recieve)
# Send through our local
mypub.publish("TestEvent", core.Event())
# Make sure we got it
self.assert_(self.called)
def WINDOW_FOUND(self, event):
"""Update the state of the target, this moves the vehicle"""
# Check if this event should be vetoed, if it should, pass it on
ret = WindowTrackingState.WINDOW_FOUND(self, event)
if ret is False and not self._average:
return ret
# Average the found windows if specified
if self._average:
x, y, range, squareNess = self.mergeWindows(event)
else:
x, y, range, squareNess = \
event.x, event.y, event.range, event.squareNess
self._updateFilters(x, y, range, squareNess)
y = self._filterdY
if 0 == self._depthGain:
y = 0
# We ignore azimuth and elevation because we aren't using them
self._target.setState(0, 0, self._filterdRange, self._filterdX, self._filterdY, event.timeStamp)
# Only triggered the in range event if we are close and the target is
# centered in the field of view
rangeError = pmath.fabs(self._filterdRange - self._desiredRange)
frontDistance = pmath.sqrt(self._filterdX ** 2 + y ** 2)
if (rangeError < self._rangeThreshold) and \
(frontDistance < self._frontThreshold):
self.publish(SeekingToRange.IN_RANGE, core.Event())
return ret
def BARBED_WIRE_FOUND(self, event):
"""Update the state of the target, this moves the vehicle"""
# Todo: consider filter removal
#self._updateFilters(event)
x = event.topX
y = event.topY
width = event.topWidth
# Determine y value based on whether we are ignoring depth or not,
# also store this event
StoreBarbedWireEvent.BARBED_WIRE_FOUND(self, event)
if self._depthGain == 0:
y = 0
# Width == 1 when we are close, so inversion is needed to get range
range = 1 - width
# Finally set the state (We ignore azimuth and elevation because we
# aren't using them)
self._target.setState(0,
0,
range=range,
x=x,
y=y,
timeStamp=event.timeStamp)
# Only triggered the in range event if we are close and the target is
# centered in the field of view
rangeError = math.fabs(range - self._desiredRange)
frontDistance = math.sqrt(x**2 + y**2)
if (rangeError < self._rangeThreshold) and \
(frontDistance < self._frontThreshold):
self.publish(SeekingToRange.IN_RANGE, core.Event())
def BUOY_FOUND(self, event):
if(self.STEPNUM == 0):
print("Window X: " + str(event.x))
if(event.x <= self._xmin):
print("Moving left to compensate")
self.move(-self._distance)
elif(event.x >= self._xmax):
print("Moving right to compensate")
self.move(self._distance)
self.STEPNUM += 1
elif(self.STEPNUM == 1):
if(event.x > self._xmin and event.x < self._xmax):
print("X Axis Aligned")
self.motionManager.stopCurrentMotion()
self.STEPNUM += 1
elif(self.STEPNUM == 2):
print("Window Y: " + str(event.y))
if(event.y <= self._ymin):
print("Moving down to compensate")
self.dive(self._distance)
elif(event.y >= self._ymax):
print("Moving up to compensate")
self.dive(-self._distance)
self.STEPNUM += 1
elif(self.STEPNUM == 3):
if(event.y > self._ymin and event.y < self._ymax):
print("Y Axis Aligned, All Done")
self.motionManager.stopCurrentMotion()
self.publish(Center.CENTERED, core.Event())
def BIN_FOUND(self, event):
HoveringState.BIN_FOUND(self, event)
# Fire event if we are centered over the bin
if self._currentBin(event):
if math.Vector2(event.x, event.y).length() < self._centeredRange:
self.publish(BinSortingState.CENTERED_, core.Event())
def _enterState(self, newStateClass):
"""
Does all the house keeping when entering a new state
"""
# Look up config based on full dotted name of state class
fullClassName = '%s.%s' % (newStateClass.__module__,
newStateClass.__name__)
stateCfg = self._config.get('States', {})
# Get the superclasses of the state
#stateList = resolve(fullClassName).__mro__
# Load the superclass config values, skipping object
#config = {}
#for index in xrange(len(stateList) - 2, 0, -1):
#print "loading", stateList[index]
# config.update(stateCfg.get(stateList[index], {}))
# Load this states config values
#config.update(stateCfg.get(fullClassName, {}))
config = stateCfg.get(fullClassName, {})
# Add self to the list of subsystems
subsystems = self._subsystems
name = self.getName()
subsystems[name[0].lower() + name[1:]] = self
# Create state instance from class, make sure to pass all subsystems
# along as well
newState = newStateClass(config, **self._subsystems)
newState.publish = self.publish
# Subscribe to every event of the desired type
transitionTable = newState.transitions()
if self._qeventHub is not None:
for eventType in transitionTable.iterkeys():
if type(eventType) == type(self._enterState):
raise Exception("Event type is actually a function")
else:
conn = self._qeventHub.subscribeToType(eventType,
self.injectEvent)
self._connections.append(conn)
# Actual enter the state and record it as our new current state
self._currentState = newState
self._currentState.enter()
# Notify everyone we just entered the state
event = core.StringEvent()
event.string = fullClassName
self.publish(Machine.STATE_ENTERED, event)
# If we are in a state with no way out, exit the state and mark ourself
# complete
if 0 == len(transitionTable):
self._exitState()
self._complete = True
self.publish(Machine.COMPLETE, core.Event())
def BARBED_WIRE_FOUND(self, event):
# Publish aligned event if needed
alignment = math.fabs(event.topX) + math.fabs(event.bottomX)
if alignment < self._minAlignment:
self.publish(SeekingToAligned.ALIGNED, core.Event())
# Update motion
TargetAlignState.BARBED_WIRE_FOUND(self, event)
def BUOY_FOUND(self, event):
"""Update the state of the buoy, this moves the vehicle"""
ret = BuoyTrackingState.BUOY_FOUND(self, event)
if ret is False:
return ret
self._buoy.setState(event.azimuth, event.elevation, event.range,
event.x, event.y, event.timeStamp)
change = self._buoy.changeOverTime()
if self._compareChange((event.x, event.y), (change[3], change[4])):
# Only applies if depthGain is set to 0
if abs(event.y) > self._planeThreshold:
# Need to recorrect the height
self.publish(Align.INCORRECT_DEPTH, core.Event())
else:
# We are properly aligned
self.publish(Align.SEEK_BUOY, core.Event())
def LOVERSLANE_FOUND(self, event):
if pmath.fabs(event.centerX) < self._centerBoundary and \
pmath.fabs(event.centerY) < self._centerBoundary:
self.publish(Aligning.CENTERED, core.Event())
return
self._lane.setState(event.centerX * self._sidewaysSpeed,
event.centerY * self._diveSpeed)
self.changedLane()
def BARBED_WIRE_FOUND(self, event):
TargetAlignState.BARBED_WIRE_FOUND(self, event)
if event.bottomWidth != -1:
if (math.fabs(event.topX) < self._threshold) and \
(math.fabs(event.bottomX) < self._threshold):
if self.timer is not None:
self.timer.stop()
self.publish(Aligning.SETTLED, core.Event())
def enter(self):
# If the offset values exist, delete them
if self.ai.data.has_key('dive_offsetTheOffset'):
del self.ai.data['dive_offsetTheOffset']
if self.ai.data.has_key('closerlook_offsetTheOffset'):
del self.ai.data['closerlook_offsetTheOffset']
self.visionSystem.binDetectorOff()
self.publish(COMPLETE, core.Event())
def _startQueuedMotion(self):
# Check if there are any queued motions
if len(self._queuedMotions) == 0:
# Publish the queued event finish if there's no queued motions
self.publish(MotionManager.FINISHED, core.Event())
else:
# Start the queued motion
motion = self._queuedMotions.pop(0)
self._setMotion(motion)
def LIGHT_FOUND(self, event):
"""Update the state of the light, this moves the vehicle"""
StoreLightEvent.LIGHT_FOUND(self, event)
self._light.setState(event.azimuth, event.elevation, event.range,
event.x, event.y, event.timeStamp)
change = self._light.changeOverTime()
if self._compareChange((event.x, event.y), (change[3], change[4])):
self.publish(Align.SEEK_LIGHT, core.Event())
def BUOY_FOUND(self, event):
ret = BuoyTrackingState.BUOY_FOUND(self, event)
if ret is False:
return ret
if self._recoverMethod == "Close Range":
# Turn off the timer and backwards motion
if self.timer is not None:
self.timer.stop()
self.timer = None
self.motionManager._stopMotion(self._recoverMotion)
# Create the depth motion if needed
newDepth = self.vehicle.getDepth()
changeDepth = False
if event.y > self._yThreshold:
newDepth = newDepth - self._closeDepthChange
changeDepth = True
elif event.y < (0.0 - self._yThreshold):
newDepth = newDepth + self._closeDepthChange
changeDepth = True
# Start the depth motion if necessary
if changeDepth:
self._diveMotion = motion.basic.RateChangeDepth(
desiredDepth=newDepth, speed=self._diveSpeed)
self.motionManager.setMotion(self._diveMotion)
self._finished = False
else:
self._finished = True
# Check if the motion is finished
if self._finished:
self.publish(Recover.REFOUND_BUOY, core.Event())
# Check if we should finish it early
if ((0.0 - self._yThreshold) < event.y < self._yThreshold):
self.motionManager.stopCurrentMotion()
self.controller.holdCurrentDepth()
self.publish(Recover.REFOUND_BUOY, core.Event())
else:
self.publish(Recover.REFOUND_BUOY, core.Event())
def testMultiple(self):
recv = Reciever()
types = ['A', 'B', 'C', 'D', 'E']
for t in types:
self.qehub.subscribeToType(t, recv)
self.assertEquals(0, recv.calls)
self.epubA.publish("A", core.Event())
self.epubA.publish("B", core.Event())
self.epubB.publish("C", core.Event())
self.epubB.publish("D", core.Event())
self.epubA.publish("E", core.Event())
self.assertEquals(0, recv.calls)
self.qehub.publishEvents()
self.assertEquals(5, recv.calls)
for expType, actType in zip(types, recv.etypes):
self.assertEquals(expType, actType)
def enter(self, defaultTimeout=60):
timeout = self.ai.data['config'].get('RandomBin',
{}).get('taskTimeout',
defaultTimeout)
task.Task.enter(self, defaultTimeout=timeout)
if self.ai.data.get('binComplete', False):
self.publish(RandomBin.MOVE_ON, core.Event())
else:
self.stateMachine.start(state.Branch(randombin.Start))
def BUOY_FOUND(self, event):
color = str(event.color).lower()
if (color in self.ai.data['buoyData']):
self.ai.data['buoyData'][color].append(
self.stateEstimator.getEstimatedPosition())
self.ai.data[color + 'FoundNum'] += 1
if (self.ai.data[color + 'FoundNum'] >= self._foundMin
and not self.ai.data['ignoreBuoy'][color]):
self.ai.data['buoyColor'] = color
self.publish(Strafe.FOUND_BUOY, core.Event())
def UPDATE(self, event):
PingerState.UPDATE(self, event)
if self._isNewPing(event):
# Converting from the vehicle reference frame, to the image space
# reference frame used by the pipe motion
self._pipe.setState(-event.direction.y, event.direction.x,
ext.math.Degree(0), event.timeStamp)
if math.fabs(event.direction.z) > math.fabs(self._closeZ):
self.publish(TranslationSeeking.CLOSE, core.Event())
def update(self, timestep):
self._currentTime += timestep
# Depth
self.depth = 10 * math.sin(self._currentTime) + 10
event = core.Event()
event.number = self.depth
self.publish(vehicle.IVehicle.DEPTH_UPDATE, event)
# Orientation
x = 1.0 * math.sin(self._currentTime) + 1.0
y = 1.0 * math.sin(self._currentTime + 5) + 1.0
z = 1.0 * math.sin(self._currentTime + 10) + 1.0
vector = ext.math.Vector3(x, y, z)
vector.normalise()
angle = 2 * math.pi * math.sin(self._currentTime + 15) + 1.0
self._orientation.FromAngleAxis(ext.math.Radian(angle), vector)
self._orientation.normalise()
event = core.Event()
event.orientation = self._orientation
self.publish(vehicle.IVehicle.ORIENTATION_UPDATE, event)
# Position Update
x = 1.0 * math.sin(self._currentTime) + 1.0
y = 1.0 * math.sin(self._currentTime + 5) + 1.0
event = ext.math.Vector2Event()
event.vector2 = ext.math.Vector2(x, y)
self.publish(vehicle.IVehicle.POSITION_UPDATE, event)
# Velocity Update
x = 1.0 * math.sin(self._currentTime + 10) + 1.0
y = 1.0 * math.sin(self._currentTime + 15) + 1.0
event = ext.math.Vector2Event()
event.vector2 = ext.math.Vector2(x, y)
self.publish(vehicle.IVehicle.VELOCITY_UPDATE, event)
# Update Devices
for device in self._devices.itervalues():
device.update(timestep)