class Aligning(VaseTrackingState):
CENTERED = core.declareEventType('CENTERED')
NONE_FOUND = core.declareEventType('NONE_FOUND')
@staticmethod
def transitions():
return VaseTrackingState.transitions(Aligning,
{ Aligning.CENTERED : CloseSeeking })
@staticmethod
def getattr():
attrs = {}
attrs.update(VaseTrackingState.getattr())
attrs.update({'delay' : 20})
return attrs
def enter(self):
# Turn on the vision system
self.visionSystem.pipeLineDetectorOn()
VaseTrackingState.enter(self, 90)
self.timer = self.timerManager.newTimer(Aligning.CENTERED,
self._delay)
self.timer.start()
def exit(self):
VaseTrackingState.exit(self)
self.timer.stop()
class PreGrabSettling(TranslationSeeking):
SETTLED = core.declareEventType('_SETTLED')
MOVE_ON = core.declareEventType('_MOVE_ON')
@staticmethod
def transitions():
return TranslationSeeking.transitions(PreGrabSettling,
PingerLostPreGrabSettling,
{ PreGrabSettling.SETTLED : PreGrabSettling,
TranslationSeeking.CLOSE : PreGrabSettling,
PreGrabSettling.MOVE_ON : Grabbing })
@staticmethod
def getattr():
return set(['duration']).union(TranslationSeeking.getattr())
def CLOSE(self, event):
if self._timerDone:
self.publish(PreGrabSettling.MOVE_ON, core.Event())
def _SETTLED(self, event):
self._timerDone = True
def enter(self):
duration = self._config.get('duration', 10)
self._timerDone = False
self.timer = self.timerManager.newTimer(PreGrabSettling.SETTLED,
duration)
self.timer.start()
TranslationSeeking.enter(self)
self._closeZ = self._config.get('closeZ', 0.97)
class SeekingToAligned(HedgeAlignState, state.State):
"""
Holds the hedge at range and in the center of the field of view while
rotating around it. If its rotating the wrong direction is will reverse
that direction and keep going until its close enough to aligned.
"""
ALIGNED = core.declareEventType('ALIGNED')
CHECK_DIRECTION = core.declareEventType('CHECK_DIRECTION_')
@staticmethod
def transitions():
return {
vision.EventType.HEDGE_FOUND: SeekingToAligned,
vision.EventType.HEDGE_LOST: FindAttemptAligned,
SeekingToAligned.CHECK_DIRECTION: SeekingToAligned,
SeekingToAligned.ALIGNED: Aligning
}
@staticmethod
def getattr():
return set(['minSquareNess',
'checkDelay']).union(HedgeAlignState.getattr())
def HEDGE_FOUND(self, event):
# Update motion
HedgeAlignState.HEDGE_FOUND(self, event)
# Record first squareNess and every squareness
squareNess = self._filterdAlign + 2
if self._firstEvent:
self._startSquareNess = squareNess
self._firstEvent = False
self._currentSquareNess = squareNess
# Publish aligned event if needed
if squareNess > self._minSquareNess and \
self._haveLeft and self._haveRight:
self.publish(SeekingToAligned.ALIGNED, core.Event())
def CHECK_DIRECTION_(self, event):
if self._currentSquareNess < self._startSquareNess:
self._alignSign *= -1
def enter(self):
HedgeAlignState.enter(self)
self._firstEvent = True
self._startSquareNess = None
self._currentSquareNess = None
# Start timer
self._minSquareNess = self._config.get('minSquareNess', 1.9)
self._delay = self._config.get('checkDelay', 3)
self._timer = self.timerManager.newTimer(
SeekingToAligned.CHECK_DIRECTION, self._delay)
self._timer.start()
def exit(self):
HedgeAlignState.exit(self)
self._timer.stop()
class SeekingToAligned2(TargetAlignState, state.State, StoreBarbedWireEvent):
"""
Holds the target at range and in the center of the field of view while
rotating around it. If its rotating the wrong direction is will reverse
that direction and keep going until its close enough to aligned.
"""
ALIGNED = core.declareEventType('ALIGNED')
CHECK_DIRECTION = core.declareEventType('CHECK_DIRECTION_')
@staticmethod
def transitions():
return {
vision.EventType.BARBED_WIRE_FOUND: SeekingToAligned2,
vision.EventType.BARBED_WIRE_LOST: FindAttempt,
SeekingToAligned.CHECK_DIRECTION: SeekingToAligned2,
SeekingToAligned.ALIGNED: Aligning
}
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 enter(self):
TargetAlignState.enter(self)
# Record threshold
self._minAlignment = self._config.get('minAlignment', 0.1)
class SeekingToAligned(TargetAlignState, state.State):
"""
Holds the target at range and in the center of the field of view while
rotating around it. If its rotating the wrong direction is will reverse
that direction and keep going until its close enough to aligned.
"""
ALIGNED = core.declareEventType('ALIGNED')
CHECK_DIRECTION = core.declareEventType('CHECK_DIRECTION_')
@staticmethod
def transitions():
return {
vision.EventType.BARBED_WIRE_FOUND: SeekingToAligned,
vision.EventType.BARBED_WIRE_LOST: AlignedFindAttempt,
SeekingToAligned.CHECK_DIRECTION: SeekingToAligned,
SeekingToAligned.ALIGNED: Aligning
}
@staticmethod
def getattr():
return set([
'yZero', 'depthGain', 'maxDepthDt', 'desiredRange', 'maxRangeDiff',
'maxAlignDiff', 'alignGain', 'rangeGain', 'maxSpeed',
'maxSidewaysSpeed', 'yawGain', 'minAlignment', 'desiredRange',
'rangeThreshold'
])
def BARBED_WIRE_FOUND(self, event):
# Publish aligned event if needed
range = 1 - event.topWidth
rangeError = math.fabs(range - self._desiredRange)
inAlignment = (math.fabs(event.topX) < self._xThreshold) and \
(math.fabs(event.topY) < self._yThreshold)
orientation = self.vehicle.getOrientation()
alignment = orientation.getYaw().valueDegrees() - \
self._desiredOrientation
inOrientation = alignment < self._orientationThreshold
inRange = rangeError < self._rangeThreshold
if inAlignment and inRange and inOrientation:
self.publish(SeekingToAligned.ALIGNED, core.Event())
# Update motion
TargetAlignState.BARBED_WIRE_FOUND(self, event)
def enter(self, minAlignment=3, desiredRange=0.5, rangeThreshold=0.05):
TargetAlignState.enter(self)
# Record threshold
self._xThreshold = self._config.get('xThreshold', 0.1)
self._yThreshold = self._config.get('yThreshold', 0.1)
self._orientationThreshold = self._config.get('orientationThreshold',
2)
self._desiredRange = self._config.get('desiredRange', desiredRange)
self._rangeThreshold = self._config.get('rangeThreshold',
rangeThreshold)
class Hit(state.State):
"""
Moves in front of the Buoy we want to hit, to a close position.
"""
DONE = core.declareEventType('DONE')
NOT_DONE = core.declareEventType('NOT_DONE')
@staticmethod
def transitions(foundState = None, lostState = None):
return { Hit.DONE : End,
Hit.NOT_DONE : Approach,
motion.basic.MotionManager.FINISHED : Hit }
@staticmethod
def getattr():
return {'diveRate' : 0.3, 'speed' : 0.15}
def enter(self):
self._orientation = self.ai.data['buoyOrientation']
buoy = self.ai.data['buoyList'].pop(0)
buoyObstacle = getObstacleType(buoy)
buoyLocation = self.stateEstimator.getObstacleLocation(buoyObstacle)
# Compute trajectories
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue = self.stateEstimator.getEstimatedDepth(),
finalValue = buoyLocation.z,
initialRate = self.stateEstimator.getEstimatedDepthRate(),
avgRate = self._diveRate)
translateTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = self.stateEstimator.getEstimatedPosition(),
finalValue = math.Vector2(buoyLocation.y, buoyLocation.x),
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = self._speed)
# Dive and translate
diveMotion = motion.basic.ChangeDepth(
trajectory = diveTrajectory)
translateMotion = ram.motion.basic.Translate(translateTrajectory,
frame = Frame.GLOBAL)
self.motionManager.setMotion(diveMotion, translateMotion)
def FINISHED(self, event):
if len(self.ai.data['buoyList']) > 0:
self.publish(Hit.NOT_DONE, core.Event())
else:
self.publish(Hit.DONE, core.Event())
def exit(self):
self.motionManager.stopCurrentMotion()
class MockEventSource(core.EventPublisher):
THING_UPDATED = core.declareEventType('THING_UPDATED')
ANOTHER_EVT = core.declareEventType('ANOTHER_EVT')
def __init__(self, eventHub = None):
core.EventPublisher.__init__(self, eventHub)
def sendEvent(self, etype, **kwargs):
event = core.Event()
for key, value in kwargs.iteritems():
setattr(event, key, value)
self.publish(etype, event)
class SeekingToAligned(TargetAlignState, state.State):
"""
Holds the target at range and in the center of the field of view while
rotating around it. If its rotating the wrong direction is will reverse
that direction and keep going until its close enough to aligned.
"""
ALIGNED = core.declareEventType('ALIGNED')
CHECK_DIRECTION = core.declareEventType('CHECK_DIRECTION_')
@staticmethod
def transitions(myState, lostState, alignedState):
return {
vision.EventType.BARBED_WIRE_FOUND: myState,
vision.EventType.BARBED_WIRE_LOST: lostState,
SeekingToAligned.CHECK_DIRECTION: myState,
SeekingToAligned.ALIGNED: alignedState
}
@staticmethod
def getattr():
return set([
'yZero', 'depthGain', 'maxDepthDt', 'desiredRange', 'maxRangeDiff',
'maxAlignDiff', 'alignGain', 'rangeGain', 'maxSpeed',
'maxSidewaysSpeed', 'yawGain', 'minAlignment', 'desiredRange',
'rangeThreshold'
])
def BARBED_WIRE_FOUND(self, event):
# Publish aligned event if needed
alignment = math.fabs(event.topX) + math.fabs(event.bottomX)
range = 1 - event.topWidth
rangeError = math.fabs(range - self._desiredRange)
inAlignment = alignment < self._minAlignment
inRange = rangeError < self._rangeThreshold
haveBottomPipe = event.bottomWidth != -1
if haveBottomPipe and inAlignment and inRange:
self.publish(SeekingToAligned.ALIGNED, core.Event())
# Update motion
TargetAlignState.BARBED_WIRE_FOUND(self, event)
def enter(self, minAlignment=0.1, desiredRange=0.5, rangeThreshold=0.05):
TargetAlignState.enter(self)
# Record threshold
self._minAlignment = self._config.get('minAlignment', minAlignment)
self._desiredRange = self._config.get('desiredRange', desiredRange)
self._rangeThreshold = self._config.get('rangeThreshold',
rangeThreshold)
class Fire(state.State):
FIRED = core.declareEventType('FIRED')
DONE = core.declareEventType('DONE')
@staticmethod
def transitions():
return { vision.EventType.BUOY_FOUND : Fire ,
Fire.FIRED : MoveOver ,
Fire.DONE : End }
def BUOY_FOUND(self, event):
self.vehicle.fireTorpedo()
self.publish(Fire.FIRED, core.Event())
class Forward(state.State):
"""
A simple timed forward run through the gate.
"""
DONE = core.declareEventType('DONE')
@staticmethod
def transitions():
return {Forward.DONE: End}
def enter(self):
# Full speed ahead!!
self.controller.setSpeed(self.ai.data['config'].get('Gate', {}).get(
'speed', 3))
# Timer goes off in X seconds then sends off DONE
self.timer = self.timerManager.newTimer(
eventType=Forward.DONE,
duration=self.ai.data['config'].get('Gate', {}).get('time', 10))
self.timer.start()
def exit(self):
self.timer.stop()
self.controller.setSpeed(0)
class Travel(task.Task):
"""
Loads the motion from the config and does it.
"""
FINISHED = core.declareEventType('FINISHED')
@staticmethod
def _transitions():
return {
motion.basic.MotionManager.FINISHED: task.Next,
Travel.FINISHED: task.Next
}
def enter(self):
self._className = type(self).__name__
taskTimeout = self.ai.data['config'].get(self._className,
{}).get('taskTimeout', 30)
task.Task.enter(self, defaultTimeout=taskTimeout)
motions = self.ai.data['config'].get(self._className,
{}).get('motions', {})
motionList = self.motionManager.generateMotionList(motions,
strict=True)
if len(motionList) > 0:
self.motionManager.setMotion(*motionList)
else:
self.publish(Travel.FINISHED, core.Event())
class LightStaged(Light):
"""
Does the light task in such with intermediate timers, that attempt to quit
early if the light is lost.
"""
DO_TIMEOUT = core.declareEventType('DO_TIMEOUT_')
@staticmethod
def _transitions():
trans = Light._transitions(LightStaged)
trans.update({
vision.EventType.LIGHT_FOUND: LightStaged,
LightStaged.DO_TIMEOUT: task.Next
})
return trans
@staticmethod
def getattr():
return set(['doTimeout']).union(task.Task.getattr())
def LIGHT_FOUND(self, event):
# Stop old
self._timer.stop()
if self.doTimer is None:
timeout = self._config.get('doTimeout', 25)
self.doTimer = self.timerManager.newTimer(LightStaged.DO_TIMEOUT,
timeout)
self.doTimer.start()
def enter(self):
Light.enter(self, defaultTimeout=40)
# Set time to none
self.doTimer = None
class Through(state.State, StoreBarbedWireEvent):
FORWARD_DONE = core.declareEventType('FORWARD_DONE')
@staticmethod
def getattr():
return set(['forwardTime'])
@staticmethod
def transitions():
return {
vision.EventType.BARBED_WIRE_FOUND: Under,
Through.FORWARD_DONE: End
}
def enter(self):
#self.visionSystem.barbedWireDetectorOff()
# Timer goes off in 3 seconds then sends off FORWARD_DONE
self.timer = self.timerManager.newTimer(
Through.FORWARD_DONE, self._config.get('forwardTime', 8))
self.timer.start()
self.controller.setSidewaysSpeed(0)
self.controller.setSpeed(3)
def exit(self):
self.timer.stop()
self.controller.setSpeed(0)
class Grabbing(state.State):
"""Does the diving grab of the treasure"""
GRABBED = core.declareEventType('GRABBED')
@staticmethod
def transitions():
return {
Grabbing.GRABBED: Surface,
ram.motion.basic.Motion.FINISHED: Surface
}
@staticmethod
def getattr():
return set(['diveRate', 'duration', 'depthOffset'])
def enter(self):
# Timer to expire motion
self.timer = self.timerManager.newTimer(
Grabbing.GRABBED, self._config.get('duration', 10))
# Setup dive
safeDepth = self.ai.data['config'].get('safeDepth', 22)
offset = self._config.get('depthOffset', 2)
diveRate = self._config.get('diveRate', 0.5)
targetDepth = safeDepth + offset
diveMotion = motion.basic.RateChangeDepth(targetDepth, diveRate)
self.motionManager.setMotion(diveMotion)
self.timer.start()
def exit(self):
self.timer.stop()
class Searching(state.State):
"""
Waits for a ping to happen, and when it gets one, turns toward the source.
Then it waits for 4 seconds, to allow the any disturbances in pinger data
from the vehicles rotation to die down, and then moves on to the next state.
"""
CHANGE = core.declareEventType("CHANGE")
@staticmethod
def transitions():
return { vehicle.device.ISonar.UPDATE : Searching,
Searching.CHANGE : FarSeeking }
def UPDATE(self, event):
if self._first:
pingerOrientation = ext.math.Vector3.UNIT_X.getRotationTo(
event.direction)
self.controller.yawVehicle(pingerOrientation.getYaw(True).valueDegrees())
self.timer = self.timerManager.newTimer(Searching.CHANGE, 4)
self.timer.start()
self._first = False
def enter(self):
self._first = True
self.timer = None
def exit(self):
if self.timer is not None:
self.timer.stop()
class Pipe(task.Task):
"""
Find and hover a pipe in the course
"""
COMPLETE = core.declareEventType('COMPLETE')
@staticmethod
def _transitions(currentState = None):
if currentState is None:
currentState = Pipe
return { pipe.Centering.SETTLED : currentState,
Pipe.COMPLETE : task.Next,
'GO' : state.Branch(pipe.Start) }
@staticmethod
def getattr():
return set(['pipesToFind']).union(task.Task.getattr())
def SETTLED(self, event):
"""
Sends a complete event after enough pipes have been found
"""
# We found a pipe increment the count
self._pipeCount += 1
if self._pipeCount >= self._pipesToFind:
# We found enough pipes move on
self.publish(Pipe.COMPLETE, core.Event())
def enter(self, defaultTimeout = 10):
self._className = type(self).__name__
timeout = self.ai.data['config'].get(self._className, {}).get(
'taskTimeout', defaultTimeout)
task.Task.enter(self, defaultTimeout = timeout)
self._pipesToFind = self._config.get('pipesToFind', 1)
self._pipeCount = 0
self.ai.data['pipeBiasDirection'] = \
self.ai.data['config'].get(self._className, {}).get(
'biasDirection', None)
self.ai.data['pipeThreshold'] = \
self.ai.data['config'].get(self._className, {}).get(
'threshold', None)
# Branch off state machine for finding the pipe
self.stateMachine.start(state.Branch(pipe.Start))
def exit(self):
del self.ai.data['pipeBiasDirection']
del self.ai.data['pipeThreshold']
task.Task.exit(self)
self.stateMachine.stopBranch(pipe.Start)
self.visionSystem.pipeLineDetectorOff()
@property
def pipesToFind(self):
return self._pipesToFind
class Target(task.Task):
"""
Task for completion of the BarbedWire objective within a certain timelimit.
"""
MOVE_ON = core.declareEventType('MOVE_ON')
@staticmethod
def _transitions():
return {
task.TIMEOUT: task.Next,
target.COMPLETE: task.Next,
'GO': state.Branch(target.Start)
}
def enter(self, defaultTimeout=120):
# Initialize task part of class
timeout = self.ai.data['config'].get('Target',
{}).get('taskTimeout',
defaultTimeout)
task.Task.enter(self, defaultTimeout=timeout)
self.stateMachine.start(state.Branch(target.Start))
def exit(self):
task.Task.exit(self)
self.stateMachine.stopBranch(target.Start)
self.visionSystem.targetDetectorOff()
self.motionManager.stopCurrentMotion()
class Hit(state.State):
FORWARD_DONE = core.declareEventType('FORWARD_DONE')
@staticmethod
def transitions():
return {Hit.FORWARD_DONE: Continue}
@staticmethod
def getattr():
return set(['duration', 'speed'])
def enter(self):
self.visionSystem.redLightDetectorOff()
# Timer goes off in 3 seconds then sends off FORWARD_DONE
duration = self._config.get('duration', 3)
speed = self._config.get('speed', 3)
self.timer = self.timerManager.newTimer(Hit.FORWARD_DONE, duration)
self.timer.start()
self.controller.setSpeed(speed)
def exit(self):
self.timer.stop()
self.controller.setSpeed(0)
self.publish(LIGHT_HIT, core.Event())
class RandomBin(task.Task):
MOVE_ON = core.declareEventType('MOVE_ON')
@staticmethod
def _transitions():
return {
bin.COMPLETE: task.Next,
task.TIMEOUT: task.Next,
RandomBin.MOVE_ON: task.Next,
'GO': state.Branch(bin.Start)
}
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 exit(self):
task.Task.exit(self)
if not self.ai.data.get('binComplete', False):
self.stateMachine.stopBranch(randombin.Start)
self.visionSystem.binDetectorOff()
self.motionManager.stopCurrentMotion()
class TimedMoveDirection(MoveDirection):
COMPLETE = core.declareEventType('COMPLETE')
def __init__(self, desiredHeading, speed, duration, absolute=True):
MoveDirection.__init__(self, desiredHeading, speed, absolute)
self._duration = duration
self._timer = None
def _start(self):
# Subscribe to our ending event
conn = self._eventHub.subscribeToType(TimedMoveDirection.COMPLETE,
self._onComplete)
self._connections.append(conn)
self._startTimer()
MoveDirection._start(self)
def _onComplete(self, event):
self.stop()
self._finish()
def _startTimer(self):
self._timer = timer.Timer(self._eventPublisher,
TimedMoveDirection.COMPLETE, self._duration)
self._timer.start()
def stop(self):
if self._timer is not None:
self._timer.stop()
self._timer = None
MoveDirection.stop(self)
@staticmethod
def isComplete():
return True
class Aligning(DuctAlignState, state.State):
SETTLED = core.declareEventType('ALIGNED')
@staticmethod
def transitions():
return { vision.EventType.DUCT_FOUND : Aligning,
vision.EventType.DUCT_LOST : FindAttempt,
Aligning.SETTLED : Through }
@staticmethod
def getattr():
return set(['depthGain', 'desiredRange', 'maxRangeDiff',
'maxAlignDiff', 'alignGain', 'maxSpeed',
'maxSidewaysSpeed', 'yawGain', 'settleTime',
'filterSize'])
def enter(self):
DuctAlignState.enter(self)
self.timer = self.timerManager.newTimer(
Aligning.SETTLED, self._config.get('settleTime', 15))
self.timer.start()
def exit(self):
DuctAlignState.exit(self)
self.timer.stop()
class Settling(SafeTrackingState):
"""Settles over the offseted safe in preperation for the grab"""
SETTLED = core.declareEventType('SETTLED')
@staticmethod
def transitions():
return SafeTrackingState.transitions(Settling,
{Settling.SETTLED: Grabbing})
@staticmethod
def getattr():
return set(['duration',
'safeOffset']).union(SafeTrackingState.getattr())
def SAFE_FOUND(self, event):
event.y = event.y - self._offset
SafeTrackingState.SAFE_FOUND(self, event)
def enter(self):
self.timer = self.timerManager.newTimer(
Settling.SETTLED, self._config.get('duration', 5))
self.timer.start()
self._offset = self.ai.data['config'].get('safeOffset', -0.7)
SafeTrackingState.enter(self)
def exit(self):
SafeTrackingState.exit(self)
self.timer.stop()
class Aligning(HedgeAlignState, state.State):
SETTLED = core.declareEventType('ALIGNED')
@staticmethod
def transitions():
return {
vision.EventType.HEDGE_FOUND: Aligning,
vision.EventType.HEDGE_LOST: FindAttempt,
Aligning.SETTLED: Through
}
@staticmethod
def getattr():
return set(['settleTime']).union(HedgeAlignState.getattr())
def enter(self):
HedgeAlignState.enter(self)
self.timer = self.timerManager.newTimer(
Aligning.SETTLED, self._config.get('settleTime', 15))
self.timer.start()
def exit(self):
HedgeAlignState.exit(self)
self.timer.stop()
class Gate(task.Task):
"""
This State overseas the completion of the gate objective. It turns on the
pipe detector after a certain delay, so it can catch the pipe as it drives
through the gate if needed.
"""
PIPE_ON = core.declareEventType('PIPE_ON_')
@staticmethod
def _transitions():
return { gate.COMPLETE : task.Next,
vision.EventType.PIPE_FOUND : task.Next,
Gate.PIPE_ON : Gate,
'GO' : state.Branch(gate.Start) }
@staticmethod
def getattr():
return set(['pipeDelay', 'gateOrientation']).union(task.Task.getattr())
def PIPE_FOUND(self, event):
self.ai.data['foundPipeEarly'] = True
def PIPE_ON_(self, event):
"""Turn pipe detector on after a delay"""
#self.visionSystem.pipeLineDetectorOn()
return
def enter(self):
task.Task.enter(self)
self.ai.data['foundPipeEarly'] = False
self.exited = False
# Branch of state machine for gate
self.stateMachine.start(state.Branch(gate.Start))
self.ai.data['gateOrientation'] = \
self._config.get('gateOrientation', 0)
if self.ai.data['gateOrientation'] == 0:
# Save current heading
self.ai.data['gateOrientation'] = \
self.stateEstimator.getEstimatedOrientation().getYaw().valueDegrees()
self.ai.data['fakeGate'] = False
# Setup timer to trigger pipe detector after a certain delay
delay = self._config.get('pipeDelay', 30)
self.timer = self.timerManager.newTimer(Gate.PIPE_ON, delay)
self.timer.start()
def exit(self):
task.Task.exit(self)
self.exited = True
if (self.stateMachine.branches.has_key(gate.Start)):
self.stateMachine.stopBranch(gate.Start)
class Centering(PipeFollowingState):
"""
When the vehicle is settling over the pipe
@cvar SETTLED: Event fired when vehicle has settled over the pipe
"""
SETTLED = core.declareEventType('SETTLED')
@staticmethod
def transitions():
return PipeFollowingState.transitions(Centering,
{Centering.SETTLED: AlongPipe})
def _withinRange(self, values):
pos = abs(values[0]) < self._planeThreshold and \
abs(values[1]) < self._planeThreshold and \
abs(values[2]) < self._angleThreshold
vel = abs(values[0] - self._startX) < self._planeChange and \
abs(values[1] - self._startY) < self._planeChange and \
abs(values[2] - self._startAngle) < self._angleChange
return pos and vel
#def FOUND_PIPE(self, event):
# status = PipeFollowingState.FOUND_PIPE(self, event)
# Wait for 'delay' pipe found events to happen first
#self._numEvents += 1
# Check to make sure it's not an outdated event
#if status and self._delay < self._numEvents:
# Check the change over time. If it's low enough,
# declare it settled
# if self._withinRange((event.x, event.y,
# event.angle.valueDegrees())):
# self.publish(Centering.SETTLED, core.Event())
#self._startX, self._startY, self._startAngle = \
# event.x, event.y, event.angle.valueDegrees()
def enter(self):
self._planeThreshold = self._config.get('planeThreshold', 0.03)
self._angleThreshold = self._config.get('angleThreshold', 0.03)
#self._planeChange = self._config.get('planeChange', 0.03)
#self._angleChange = self._config.get('angleChange', 0.03)
#self._delay = self._config.get('delay', 20)
#self._numEvents = 0
self.timer = self.timerManager.newTimer(Centering.SETTLED, 5)
self.timer.start()
PipeFollowingState.enter(self)
def exit(self):
#print '"Exiting Seek, going to follow"'
PipeFollowingState.exit(self)
self.timer.stop()
class NextBin(BinSortingState):
AT_END = core.declareEventType('AT_END')
@staticmethod
def transitions():
return HoveringState.transitions(NextBin,
lostState = RecoverNextBin, recoveryState = LostCurrentBinNextBin,
trans = { BinSortingState.CENTERED_ : Dive,
NextBin.AT_END : SurfaceToCruise })
def _getNextBin(self, sortedBins, currentBinId):
"""
Override default behaviour to return the next bin to the right
"""
# Find where the currentBinId is in the list of sorted bins
try:
startIdx = sortedBins.index(currentBinId) - 1;
endIdx = startIdx + 1;
# Pull out the sub list of length one right after that point
results = sortedBins[startIdx:endIdx]
if len(results) == 0:
# We are at the end
return currentBinId
else:
return results[0]
except ValueError:
# We have lost our shit
#self.publish(vision.EventType.BINS_LOST, core.Event())
# We lost the current ID, recover by finding the next best
# Use the old getNextBin
result = BinSortingState._getNextBin(self, sortedBins, currentBinId)
return result
def BIN_FOUND(self, event):
# Cancel out angle commands (we don't want to control orientation)
event.angle = math.Degree(0)
BinSortingState.BIN_FOUND(self, event)
def enter(self):
# Keep the hover motion going
direction = BinSortingState.RIGHT
if self.ai.data['startSide'] == BinSortingState.RIGHT:
direction = BinSortingState.LEFT
self._binDirection = self.ai.data['config'].get('Bin', {}).get(
'binDirection', None)
if self._binDirection is None:
BinSortingState.enter(self, direction, useMultiAngle = True)
else:
BinSortingState.enter(self, direction, useMultiAngle = True,
shouldRotate = False)
# Fix the current left most bin, as the currently tracked bin
if not self.fixEdgeBin():
# If already there
self.publish(NextBin.AT_END, core.Event())
class LostCurrentBin(state.FindAttempt, HoveringState):
"""
When the vehicle loses its current ID it will attempt to find it.
If it fails to find it, it will choose a new ID and continue.
"""
REFOUND_BIN = core.declareEventType('REFOUND_BIN')
@staticmethod
def transitions(myState, lostState, originalState):
trans = HoveringState.transitions(myState, lostState = lostState)
trans.update(state.FindAttempt.transitions(vision.EventType.BIN_FOUND,
myState, originalState, trans = trans))
trans.update({ LostCurrentBin.REFOUND_BIN : originalState })
return trans
@staticmethod
def getattr():
return set(['timeout', 'recoverThreshold']).union(
HoveringState.getattr()).union(state.FindAttempt.getattr())
def BIN_FOUND(self, event):
HoveringState.BIN_FOUND(self, event)
# Check to see if this bin is new
if event.id not in self._currentIds:
# If it is, check if it's in the threshold
oldX = self.ai.data['lastBinX']
oldY = self.ai.data['lastBinY']
if ((oldX - self._recoverThreshold) < event.x < \
(oldX + self._recoverThreshold)) and \
((oldY - self._recoverThreshold) < \
event.y < (oldY + self._recoverThreshold)):
# We've found the bin
self.ai.data['binData']['currentID'] = event.id
self.publish(LostCurrentBin.REFOUND_BIN, core.Event())
else:
# Add it to the currentIDs and continue searching
self._currentIds.add(event.id)
def TIMEOUT(self, event):
currentIds = self.ai.data['binData']['currentIds']
if currentIds is not None:
newEvent = self._findClosestBinIdEvent()
if newEvent is not None:
self.ai.data['binData']['currentID'] = newEvent.id
else:
# Something went horribly wrong
raise Exception("Lost the Current ID without anyway back!")
def enter(self):
HoveringState.enter(self)
state.FindAttempt.enter(self, timeout = self._config.get('timeout', 3))
self._recoverThreshold = self._config.get('recoverThreshold', 0.2)
self._currentIds = self.ai.data['binData']['currentIds']
class FarSeeking(state.State):
CLOSE = core.declareEventType('CLOSE')
@staticmethod
def transitions():
return { vehicle.device.ISonar.UPDATE : FarSeeking,
FarSeeking.CLOSE : CloseSeeking }
@staticmethod
def getattr():
return set(['closeZ', 'speedGain', 'yawGain', 'maxSpeed', 'timeout',
'maxSidewaysSpeed', 'sidewaysSpeedGain', 'forwardSpeed',
'motionRange', 'distance', 'sonarError'])
def UPDATE(self, event):
if math.fabs(event.direction.z) > math.fabs(self._closeZ):
self.publish(FarSeeking.CLOSE, core.Event())
return
#if not self._first:
# return
direction = event.direction
direction.normalise()
translateTrajectory = motion.trajectories.Vector2VelocityTrajectory(
velocity = ext.math.Vector2(direction.x * 0.3, - direction.y * 0.3),
initialPosition = ext.math.Vector2.ZERO,
maxDistance = 2)
translateMotion = motion.basic.Translate(translateTrajectory,
frame = Frame.LOCAL)
self.motionManager.setMotion(translateMotion)
self._first = False
def _loadSettings(self):
self._closeZ = self._config.get('closeZ', 0.85)
self._speedGain = self._config.get('speedGain', 5)
self._yawGain = self._config.get('yawGain', 1)
self._maxSpeed = self._config.get('maxSpeed', 1)
self._maxSidewaysSpeed = self._config.get('maxSidewaysSpeed', 2)
self._sidewaysSpeedGain = self._config.get('sidewaysSpeedGain', 2)
self._forwardSpeed = self._config.get('forwardSpeed', 0.15)
self._motionRange = self._config.get('motionRange', .02)
self._distance = self._config.get('distance', 1)
self.ai.data['minSonarError'] = self._config.get('sonarError', 1)
def enter(self):
self._loadSettings()
def exit(self):
self.motionManager.stopCurrentMotion()
class FindAttempt(State):
"""
Default state for finding a lost target
"""
TIMEOUT = core.declareEventType("TIMEOUT")
def __init__(self, config=None, **kwargs):
State.__init__(self, config, **kwargs)
@staticmethod
def transitions(foundEvent, foundState, timeoutState, trans=None):
if trans is None:
trans = {}
trans.update({
foundEvent: foundState,
FindAttempt.TIMEOUT: timeoutState
})
return trans
@staticmethod
def getattr():
return {'holdDepth': False, 'timeout': 2}
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.holdCurrentOrientation()
self.controller.holdCurrentPosition()
# 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 exit(self):
if self.timer is not None:
self.timer.stop()
self.motionManager.stopCurrentMotion()
class Octagon(task.Task):
"""
Surface in the octagon with or without the treasure, but with the sonar on
"""
SURFACED = core.declareEventType('SURFACED_')
@staticmethod
def _transitions():
return {
motion.basic.MotionManager.FINISHED: Octagon,
Octagon.SURFACED: task.Next
}
@staticmethod
def getattr():
return set(['depth', 'diveSpeed', 'delay',
'release']).union(task.Task.getattr())
def FINISHED(self, event):
"""
Waits for the vehicle to balance out on the top of the water
before releasing the grabber.
"""
# Safe the thrusters (that way we float to the absolute top)
#self.vehicle.safeThrusters()
self._timer = self.timerManager.newTimer(Octagon.SURFACED, self._delay)
self._timer.start()
def SURFACED_(self, event):
"""
Releases the grabber.
"""
if self._release:
self.vehicle.releaseGrabber()
def enter(self):
task.Task.enter(self)
self._delay = self._config.get('delay', 5)
self._release = self._config.get('release', True)
# Start our dive
diveMotion = motion.basic.RateChangeDepth(
desiredDepth=self._config.get('depth', 0),
speed=self._config.get('diveSpeed', 0.3))
self.motionManager.setMotion(diveMotion)
def exit(self):
task.Task.exit(self)
self.motionManager.stopCurrentMotion()
if self._timer is not None:
self._timer.stop()
def __init__(self, config = None, **subsystems):
# Call the super class
state.State.__init__(self, config, **subsystems)
# Dynamically create our event
self._timeoutEvent = core.declareEventType(
'TIMEOUT_' + self.__class__.__name__)
# From the AI grab our next task
self._nextState = self.ai.getNextTask(type(self))
self._failureState = self.ai.getFailureState(type(self))
# Timeout related values, set later on
self._hasTimeout = False
self._timeoutDuration = None
self._timer = None
def recordClipImpl(addRecorder, removeRecorder, seconds, name, extension, rate):
# All comments are needed. Do not add blank lines!
TIMEOUT = core.declareEventType('TIMEOUT')
conn = None
def stop(event):
removeRecorder(name + extension)
conn.disconnect()
conn = queuedEventHub.subscribeToType(TIMEOUT, stop)
# If no name was given, create one out of the time
if name is None:
timeStamp = datetime.fromtimestamp(timer.time())
name = timeStamp.strftime("%Y%m%d%H%M%S")
# Add the forward recorder
addRecorder(name + extension, rate)
# Create the timer
clipTimer = timerManager.newTimer(TIMEOUT, seconds)
clipTimer.start()
# Project Imports
import ext.core as core
import ext.vehicle as vehicle
import ext.vehicle.device
import ext.math
from ext.control import yawVehicleHelper
from ext.control import holdCurrentHeadingHelper
import ram.ai.state as state
import ram.motion as motion
#import ram.motion.search
import ram.motion.pipe
from ram.motion.basic import Frame
COMPLETE = core.declareEventType('COMPLETE')
class PingerState(state.State):
"""
Base state for the using the Sonar data to hover toward and over the Pinger.
Base classes are required to update the actual target with new Sonar
information.
"""
TIMEOUT = core.declareEventType('PINGER_TIMEOUT')
@staticmethod
def transitions(myState, timeoutState = None, trans = None):
if timeoutState is None:
timeoutState = PingerLost
if trans is None:
# File: packages/python/ram/test/ai/task.py
# Python Imports
import unittest
# Project Imports
import ext.core as core
import ram.ai as ai
import ram.ai.subsystem
import ram.ai.state as state
import ram.ai.task as task
from ram.logloader import resolve
import ram.test.ai.support as support
EVENT_A = core.declareEventType('A')
EVENT_B = core.declareEventType('B')
EVENT_C = core.declareEventType('C')
EVENT_D = core.declareEventType('D')
EVENT_E = core.declareEventType('E')
EVENT_F = core.declareEventType('F')
EVENT_B_FAIL = core.declareEventType('B_FAIL')
EVENT_C_FAIL = core.declareEventType('C_FAIL')
class TaskA(task.Task):
DEFAULT_TIMEOUT = 16
@staticmethod
def _transitions():
return {EVENT_A : task.Next,
EVENT_B : task.Next,
import math as pmath
# Project Imports
import ext.core as core
import ext.vision as vision
import ext.math as math
import ram.filter as filter
import ram.ai.state as state
import ram.ai.tracking as tracking
import ram.motion as motion
import ram.motion.search
import ram.motion.seek
import ram.timer
BUOY_HIT = core.declareEventType('BUOY_HIT')
COMPLETE = core.declareEventType('COMPLETE')
class BuoyTrackingState(state.State):
"""
Set up for any state that tracks a buoy. Filters out incorrect
buoy colors.
"""
FINISHED = core.declareEventType('FINISHED')
@staticmethod
def transitions(foundState = None, lostState = None):
return { vision.EventType.BUOY_FOUND : foundState,
vision.EventType.BUOY_LOST : lostState,
# STD Imports
import math
# Project Imports
import ext.core as core
import ext.vision as vision
import ram.filter as filter
import ram.ai.state as state
import ram.motion as motion
import ram.motion.search
import ram.motion.seek
import ram.motion.duct
THROUGH_DUCT = core.declareEventType('THROUGH_DUCT')
class StoreDuctEvent(object):
"""
Common subclass for states that have a DUCT_FOUND transition, it stores
the event is the ai.data.
"""
def DUCT_FOUND(self, event):
self.ai.data['lastDuctEvent'] = event
class Searching(state.State, StoreDuctEvent):
"""
Runs a zig-zag search pattern until it finds the duct
"""
@staticmethod
# Standard imports
import math as pmath
# Project Imports
import ext.core as core
import ext.vision as vision
import ext.math as math
import ram.filter as filter
import ram.ai.state as state
import ram.motion as motion
import ram.motion.search
import ram.motion.seek
import ram.timer
LIGHT_HIT = core.declareEventType('LIGHT_HIT')
COMPLETE = core.declareEventType('COMPLETE')
class StoreLightEvent(object):
"""
Common subclass for states that have a LIGHT_FOUND transition, it stores
the event in ai.data.
"""
def LIGHT_FOUND(self, event):
self.ai.data['lastLightEvent'] = event
class Start(state.State):
"""
Does all the setup work for the light task. Currently this involves just
going to the proper depth to begin the search for the light.
"""
# Project Imports
import ext.core as core
import ext.vision as vision
import ext.math as math
import ram.ai.state as state
import ram.ai.tracking as tracking
import ram.motion as motion
import ram.motion.basic
import ram.motion.search
# import ram.motion.common
import ram.motion.pipe # For the maneuvering motions
COMPLETE = core.declareEventType("COMPLETE")
def ensureBinTracking(qeventHub, ai):
tracking.ensureItemTracking(qeventHub, ai, "binData", vision.EventType.BIN_FOUND, vision.EventType.BIN_DROPPED)
ai.data["binData"].setdefault("histogram", {})
class HoveringState(state.State):
"""
Base state for hovering over the bins or the array of bins. It can use
either angle of the entire array, or of the current bin.
"""
LOST_CURRENT_BIN = core.declareEventType("LOST_CURRENT_BIN_")
# Copyright (C) 2010 Maryland Robotics Club
# Copyright (C) 2010 Jonathan Sternberg <*****@*****.**>
# All rights reserved.
#
# Author: Jonathan Sternberg <*****@*****.**>
# File: sandbox/statemachine/runner.py
import ext.core as core
import ram.ai.state as state
FINISH = core.declareEventType('FINISH')
import string
for letter in set(string.letters.upper()):
globals()[letter] = core.declareEventType(letter)
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())
# Project Imports
import ext.core as core
import ext.vision as vision
import ext.math as math
import ext.estimation as estimation
from ext.control import yawVehicleHelper
import ram.ai.state as state
import ram.motion as motion
import ram.timer
from ram.motion.basic import Frame
import ram.ai.Utility as util
import ram.ai.Approach as vs
DONE = core.declareEventType('DONE')
class Centering(vs.DHyperApproach):
@staticmethod
def getattr():
return { 'kx' : .15 , 'ky' : .4 , 'kz' : 9, 'x_d' : 0, 'r_d' : 50 , 'y_d' : 0, 'x_bound': .05, 'r_bound': 20, 'y_bound':.025 ,'minvx': .1, 'minvy': .1 ,'minvz' : .1}
def PIPE_FOUND(self,event):
run(event):
def decideZ(self,event):
return 0
@staticmethod
def transitions():
return {vs.DONE : Align , vision.EventType.PIPE_FOUND : Centering}
import ram.ai.state as state
import ram.motion as motion
import ext.core as core
import ext.vision as vision
import ext.math as math
from ram.motion.basic import Frame
from ext.control import yawVehicleHelper
DONE = core.declareEventType('DONE')
YAWED = core.declareEventType('YAWED')
#parameter lookup function
#you is self, name is parameter to look up, default is default output
def getConfig(you,name,default):
return you.ai.data['config'].get(name,default)
#dive to a specified depth
def dive(you, depth, rate):
# Compute trajectories
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue = you.stateEstimator.getEstimatedDepth(),
finalValue = depth,
initialRate = you.stateEstimator.getEstimatedDepthRate(),
avgRate = rate)
# Dive
diveMotion = motion.basic.ChangeDepth(trajectory = diveTrajectory)
you.motionManager.setMotion(diveMotion)
#hold the current position in xy
# Project Imports
import ext.core as core
import ext.vision as vision
import ext.math as math
import ext.estimation as estimation
from ext.control import yawVehicleHelper
import ram.ai.state as state
import ram.motion as motion
import ram.timer
from ram.motion.basic import Frame
import ram.ai.Utility as util
import ram.ai.Approach as vs
COMPLETE = core.declareEventType('COMPLETE')
REDT = core.declareEventType('REDT')
BLUET = core.declareEventType('BLUET')
GREENT = core.declareEventType('GREENT')
YELLOWT = core.declareEventType('YELLOWT')
#convention, window is the entire object, target is the individual cut-outs
#global variables are used here, but it should be noted that python's globals
#have some useful properties
#these will not be modified by any local assignments done and furthermore
#can only be read from if a local assignment hasn't been done
class Start(state.State):
@staticmethod
# Copyright (C) 2008 Maryland Robotics Club
# Copyright (C) 2008 Joseph Lisee <*****@*****.**>
# All rights reserved.
#
# Author: Joseph Lisee <*****@*****.**>
# File: packages/python/ram/ai/state.py
# STD Imports
# Project Imports
import ext.core as core
import ram.ai.state as state
# Special event that denotes
TIMEOUT = core.declareEventType('TIMEOUT')
class Next(state.State):
"""
Special state denotes that the next task should be moved to
"""
pass
class Failure(state.State):
"""
Special state denotes that the task failed in an *unrecoverable* way.
"""
pass
class End(state.State):
"""
Special state that denotes the complete end of the state machine
# File: packages/python/ram/test/ai/tracking.py
# Python Imports
import unittest
# Project Imports
import ram.ai.bin as bin
import ram.ai.tracking as tracking
import ext.core as core
import ext.vision as vision
import ext.math as math
import ram.test.ai.support as aisupport
from ram.test import Mock
OBJECT_FOUND = core.declareEventType('OBJECT_FOUND')
OBJECT_DROPPED = core.declareEventType('OBJECT_DROPPED')
# Helper functions
class TestTracking(aisupport.AITestCase):
def injectEventFound(self, id):
self.publishQueuedEvent(None, OBJECT_FOUND, id = id)
def injectEventDropped(self, id):
self.publishQueuedEvent(None, OBJECT_DROPPED, id = id)
def setUp(self):
aisupport.AITestCase.setUp(self)
tracking.ensureItemTracking(self.qeventHub, self.ai, 'trackingTest',
OBJECT_FOUND, OBJECT_DROPPED)
def testEnsureItemTracking(self):
"""
# STD Imports
import math as pmath
# Project Imports
import ext.core as core
import ext.vision as vision
import ext.math as math
import ram.ai.state as state
import ram.ai.light as legacy
import ram.motion as motion
import ram.timer as timer
COMPLETE = core.declareEventType('COMPLETE')
FAILURE = core.declareEventType('FAILURE')
class Start(state.State):
"""
Makes sure the buoy object is located in the map
"""
SUCCESS = core.declareEventType('SUCCESS')
FAIL = core.declareEventType('FAIL')
@staticmethod
def transitions():
return { Start.SUCCESS : Dive,
Start.FAIL : Failure }
def enter(self):
# Project Imports
import ext.core as core
import ext.vision as vision
import ext.math as math
import ext.estimation as estimation
from ext.control import yawVehicleHelper
import ram.ai.state as state
import ram.motion as motion
import ram.timer
from ram.motion.basic import Frame
import ram.ai.Utility as util
import ram.ai.Approach as vs
COMPLETE = core.declareEventType('COMPLETE')
DROPPED = core.declareEventType('DROPPED')
class Start(util.FiniteState):
def run(self):
self.visionSystem.downwardSafeDetectorOn()
@staticmethod
def transitions():
return {util.DONE : Diving}
class Diving(util.MotionState):
def enter(self):
self.dive(self.stateEstimator.getEstimatedDepth(),.1)
@staticmethod
def transitions():
return {util.DONE : Positioning, motion.basic.MotionManager.FINISHED : Diving}
