def testNormal(self):
m = motion.search.ForwardZigZag(legTime = 6, sweepAngle = 35, speed = 8)
self.motionManager.setMotion(m)
# Complete on short leg
orientation = math.Quaternion(math.Degree(35/2.0),
math.Vector3.UNIT_Z)
self.controller.publishAtOrientation(orientation)
self.qeventHub.publishEvents()
mockTimer = MockTimer.LOG[motion.search.ForwardZigZag.LEG_COMPLETE]
mockTimer.finish()
self.qeventHub.publishEvents()
# Now test the that normal part of the leg completes
# Test turn first
expectedYaw = -35
self.assertEqual(expectedYaw , self.controller.yawChange)
self.assertEqual(0, self.controller.speed)
orientation = math.Quaternion(math.Degree(expectedYaw),
math.Vector3.UNIT_Z)
self.controller.publishAtOrientation(orientation)
self.qeventHub.publishEvents()
# Ensure we only go full time of the rest of the legs
mockTimer = MockTimer.LOG[motion.search.ForwardZigZag.LEG_COMPLETE]
self.assert_(mockTimer.started)
self.assertEqual(6, mockTimer.sleepTime)
self.assertEqual(8, self.controller.speed)
def testOrientationUpdate(self):
"""Make sure we update when we get an orientation event"""
self.vehicle.orientation = math.Quaternion.IDENTITY
m = motion.search.ZigZag(legTime = 6, sweepAngle = 90, speed = 8)
# Start the motion
self.motionManager.setMotion(m)
orientation = math.Quaternion(math.Degree(-90), math.Vector3.UNIT_Z)
self.vehicle.publishOrientationUpdate(orientation)
self.qeventHub.publishEvents()
# Make sure the speeds result from the updated orientation not the
# starting one
expectedSpeed = pmath.sqrt(2)/2.0 * 8
self.assertAlmostEqual(expectedSpeed, self.controller.speed, 6)
self.assertAlmostEqual(-expectedSpeed, self.controller.sidewaysSpeed, 6)
self.motionManager.stopCurrentMotion()
# Another test
m = motion.search.ZigZag(legTime = 6, sweepAngle = 180, speed = 8)
self.motionManager.setMotion(m)
self.assertAlmostEqual(0, self.controller.speed, 6)
self.assertAlmostEqual(8, self.controller.sidewaysSpeed, 6)
# Flip the vehicle around
orientation = math.Quaternion(math.Degree(180), math.Vector3.UNIT_Z)
self.vehicle.publishOrientationUpdate(orientation)
self.qeventHub.publishEvents()
self.assertAlmostEqual(0, self.controller.speed, 6)
self.assertAlmostEqual(-8, self.controller.sidewaysSpeed, 6)
def testGenerator(self):
# Test the generator for this motion works
m = motion.basic.MotionManager.generateMotion(
'ram.motion.basic.MoveDistance',
desiredHeading=5,
distance=10,
speed=3)
self.assertEqual(m._direction,
math.Quaternion(math.Degree(5), math.Vector3.UNIT_Z))
self.assertEqual(m._distance, 10)
self.assertEqual(m._speed, 3)
m = motion.basic.MotionManager.generateMotion(
'ram.motion.basic.MoveDistance',
complete=True,
desiredHeading=5,
distance=10,
speed=3)
self.assertEqual(m._direction,
math.Quaternion(math.Degree(5), math.Vector3.UNIT_Z))
self.assertEqual(m._distance, 10)
self.assertEqual(m._speed, 3)
def testChangeOrientation(self):
# this doesnt need to test how we get to the result, it only
# tests that we get there without crashing along the way
m = motion.basic.ChangeOrientation(\
StepTrajectory(initialValue = math.Quaternion(1,0,0,0),
finalValue = math.Quaternion(0,0,0,1)),
updateRate = 1)
self.estimator.orientation = math.Quaternion(0, 0, 0, 1)
self.qeventHub.subscribeToType(motion.basic.Motion.FINISHED,
self.handleFinished)
# set the motion
self.motionManager.setMotion(m)
# publish the event to update
timer = support.MockTimer.LOG[
motion.basic.ChangeOrientation.ORIENTATION_TRAJECTORY_UPDATE]
timer.finish()
self.qeventHub.publishEvents()
# see if the motion finished
self.assert_(self.motionFinished)
# Make sure we have reached the final value
self.assert_(self.controller.atOrientation())
def enter(self):
if(self.ai.data['fakeGate']):
self._distance = self.ai.data['fakeGateDistance']
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = math.Quaternion(
math.Degree(self.ai.data['gateOrientation']), math.Vector3.UNIT_Z),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = math.Vector3.ZERO)
forwardTrajectory = motion.trajectories.Vector2CubicTrajectory(
initialValue = math.Vector2.ZERO,
finalValue = math.Vector2(self._distance,0),
initialRate = self.stateEstimator.getEstimatedVelocity(),
avgRate = self._avgRate)
forwardMotion = motion.basic.Translate(
trajectory = forwardTrajectory,
frame = Frame.LOCAL)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
# Full speed ahead!!
self.motionManager.setMotion(yawMotion, forwardMotion)
def enter(self):
self.visionSystem.buoyDetectorOn()
buoyDepth = self.ai.data['config'].get('buoyDepth', -1)
self._orientation = self.ai.data['buoyOrientation']
self.ai.data['buoyData'] = {}
self.ai.data['ignoreBuoy'] = {}
for color in self.ai.data['buoyList']:
self.ai.data['buoyData'][color.lower()] = []
self.ai.data[color.lower() + 'FoundNum'] = 0
self.ai.data['ignoreBuoy'][color.lower()] = False
# Compute trajectories
diveTrajectory = motion.trajectories.ScalarCubicTrajectory(
initialValue=self.stateEstimator.getEstimatedDepth(),
finalValue=buoyDepth,
initialRate=self.stateEstimator.getEstimatedDepthRate(),
avgRate=self._diveRate)
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue=currentOrientation,
finalValue=math.Quaternion(math.Degree(self._orientation),
math.Vector3.UNIT_Z),
initialRate=self.stateEstimator.getEstimatedAngularRate(),
finalRate=math.Vector3.ZERO)
# Dive yaw and translate
diveMotion = motion.basic.ChangeDepth(trajectory=diveTrajectory)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
self.motionManager.setMotion(diveMotion, yawMotion)
def __init__(self,
desiredHeading,
distance,
speed,
threshold=0.1,
absolute=True):
"""
@type desiredHeading: double
@param desiredHeading: compass heading in degrees (0 = north, + counter clockwise)
@type distance: double
@param distance: distance to travel in meters
@type speed: int
@param speed: the speed (0-5) to travel
@type threshold: double
@param threshold: the radius of where the robot is considered at
the location
@type absolute: boolean
@param absolute: the heading value as an absolute or relative value
"""
Motion.__init__(self, _type=Motion.IN_PLANE)
self._speed = speed
self._direction = math.Quaternion(math.Degree(desiredHeading),
math.Vector3.UNIT_Z)
self._threshold = threshold
self._absolute = absolute
self._distance = distance
self._connections = []
def nextStep(self):
if( self.STEPNUM == 0 ):
self.dive(-self._height)
elif( self.STEPNUM == 1 ):
self.move(self._distance)
elif( self.STEPNUM == 2 ):
self.dive(self._height)
elif( self.STEPNUM == 3 ):
currentOrientation = self.stateEstimator.getEstimatedOrientation()
yawTrajectory = motion.trajectories.StepTrajectory(
initialValue = currentOrientation,
finalValue = currentOrientation * math.Quaternion(
math.Degree(180), math.Vector3.UNIT_Z),
initialRate = self.stateEstimator.getEstimatedAngularRate(),
finalRate = math.Vector3.ZERO)
yawMotion = motion.basic.ChangeOrientation(yawTrajectory)
self.motionManager.setMotion(yawMotion)
else:
self.timer = self.timerManager.newTimer(MoveOver.DONE, self._delay)
self.timer.start()
self.STEPNUM += 1
def testRight(self):
self.vehicle.orientation = math.Quaternion(math.Degree(10),
math.Vector3.UNIT_Z)
# Go to 60 degrees, at 10 degrees a second, with a 10Hz update rate
m = motion.basic.RateChangeHeading(desiredHeading=-50,
speed=10,
rate=10)
self.qeventHub.subscribeToType(motion.basic.Motion.FINISHED,
self.handleFinished)
# Start
self.motionManager.setMotion(m)
mockTimer = \
support.MockTimer.LOG[motion.basic.RateChangeHeading.NEXT_HEADING]
self.assert_(mockTimer.repeat)
self.assertEqual(mockTimer.sleepTime, 0.1)
# Check thirty steps
expectedHeading = 10
for i in xrange(0, 60):
expectedHeading -= 1
mockTimer.finish()
self.qeventHub.publishEvents()
self.assertAlmostEqual(expectedHeading,
self._getControllerHeading(), 3)
self.assertAlmostEqual(-50, self._getControllerHeading(), 1)
# Make sure more events don't let it keep going
mockTimer.finish()
self.qeventHub.publishEvents()
self.assertAlmostEqual(-50, self._getControllerHeading(), 1)
self.assertEqual(True, self.motionFinished)
def testNoEventAfterStop(self):
m = motion.search.ForwardZigZag(legTime = 6, sweepAngle = 35, speed = 8)
self.motionManager.setMotion(m)
# Register for the leg complete event
self.legComplete = False
def handler(event):
self.legComplete = True
self.eventHub.subscribeToType(motion.search.ForwardZigZag.LEG_COMPLETE,
handler)
# Start the short leg
orientation = math.Quaternion(math.Degree(35/2.0),
math.Vector3.UNIT_Z)
self.controller.publishAtOrientation(orientation)
self.qeventHub.publishEvents()
# Stop motion
self.motionManager.stopCurrentMotion()
mockTimer = MockTimer.LOG[motion.search.ForwardZigZag.LEG_COMPLETE]
mockTimer.finish()
self.qeventHub.publishEvents()
# Make sure we didn't get the event
self.assert_(mockTimer.started)
self.assertEqual(3, mockTimer.sleepTime)
self.assertEquals(False, self.legComplete)
def _start(self):
# Register to receive ORIENTATION_UPDATE events
#conn = self._eventHub.subscribe(vehicle.IVehicle.ORIENTATION_UPDATE,
# self._vehicle, self._onOrientation)
#self._connections.append(conn)
# Register to receive POSITION_UPDATE events
conn = self._eventHub.subscribeToType(vehicle.IVehicle.POSITION_UPDATE,
self._onUpdate)
self._connections.append(conn)
conn = self._eventHub.subscribeToType(MoveDistance.COMPLETE,
self._onComplete)
self._connections.append(conn)
# Find the current position
currentPosition = self._vehicle.getPosition()
#current = math.Quaternion(currentPosition.x, currentPosition.y, 0, 0)
# Set the desired direction if it's not absolute
if not self._absolute:
heading = self._vehicle.getOrientation().getYaw()
orientation = math.Quaternion(math.Degree(heading),
math.Vector3.UNIT_Z)
self._direction = orientation * self._direction
#
unit = self._unitvector(self._direction.getYaw(True).valueDegrees())
mult = unit * self._distance
self._desiredPosition = currentPosition + mult
pathVector = self._desiredPosition - currentPosition
self._setSpeeds(pathVector)
def testOppositeDirection(self):
# Once it is in the correct direction,
# then it acts like a normal zigZag motion
m = motion.search.ForwardZigZag(legTime = 6, sweepAngle = 35,
speed = 8, direction = -60)
# Start the motion
self.motionManager.setMotion(m)
# Ensure we only turn half way on the first turn and the
# direction is correct
expectedYaw = -60 - 35/2.0
self.assertEqual(expectedYaw, self.controller.yawChange)
self.assertEqual(0, self.controller.speed)
orientation = math.Quaternion(math.Degree(expectedYaw),
math.Vector3.UNIT_Z)
self.controller.publishAtOrientation(orientation)
self.qeventHub.publishEvents()
# Ensure we only go half time of the first leg and at the right speed
mockTimer = MockTimer.LOG[motion.search.ForwardZigZag.LEG_COMPLETE]
self.assert_(mockTimer.started)
self.assertEqual(3, mockTimer.sleepTime)
self.assertEqual(8, self.controller.speed)
def testLostTimeout(self):
expected = math.Quaternion(math.Degree(25), math.Vector3.UNIT_Z)
self.ai.data['gateOrientation'] = expected
self.assertCurrentState(course.PipeStaged)
self.injectEvent(course.PipeStaged.LOST_TIMEOUT)
self.qeventHub.publishEvents()
self.assertCurrentState(course.LightStaged)
def _setDirection(self):
"""
Sets the direction of the search path based on the current sweep angle
"""
self._sweepAngle *= -1
dir = math.Quaternion(math.Degree(self._sweepAngle / 2.0),
math.Vector3.UNIT_Z)
self._direction = dir * self._forwardDirection
def setUp(self):
cfg = initializeConfig('buoy', [-2.5, 2.5, 4, 0])
support.AITestCase.setUp(self, cfg = cfg)
# Point the vehicle west
self.vehicle.orientation = math.Quaternion(math.Degree(90),
math.Vector3.UNIT_Z)
self.machine.start(light.SearchZigZag)
def testDirectionAfterTurn(self):
self.vehicle.orientation = math.Quaternion(math.Degree(60),
math.Vector3.UNIT_Z)
m = self.makeClass(desiredHeading=0, speed=5, absolute=False)
self.motionManager.setMotion(m)
self.assertEqual(5, self.controller.speed)
self.assertAlmostEqual(0, self.controller.sidewaysSpeed, 5)
# Now turn the vehicle 90 degrees and make sure it's still heading
# the same direction
self.vehicle.orientation = math.Quaternion(math.Degree(150),
math.Vector3.UNIT_Z)
self.vehicle.publishOrientationUpdate(self.vehicle.orientation)
self.qeventHub.publishEvents()
self.assertAlmostEqual(0, self.controller.speed, 5)
self.assertEqual(5, self.controller.sidewaysSpeed)
def testRight(self):
# Turning away from light at -30 Degrees off North
self.controller.desiredOrientation = \
math.Quaternion(math.Degree(10), math.Vector3.UNIT_Z)
self.estimator.orientation = \
math.Quaternion(math.Degree(-15), math.Vector3.UNIT_Z)
# Buoy at same depth and 15 degrees right of vehicle's heading
# Make sure we only rotate relative to the controllers desired
# orientation
self.checkCommand(azimuth=-15, elevation=0, yawChange=-40, newDepth=0)
# Turning toward light at 55 Degrees off North
self.controller.desiredOrientation = \
math.Quaternion(math.Degree(-5), math.Vector3.UNIT_Z)
self.estimator.orientation = \
math.Quaternion(math.Degree(10), math.Vector3.UNIT_Z)
# Buoy dead ahead and 20 degrees right of vehicle's heading
self.checkCommand(azimuth=-20, elevation=0, yawChange=-5, newDepth=0)
def testStoreDesiredQuaternion(self):
# Setup a desired orientation
expected = math.Quaternion(math.Degree(45), math.Vector3.UNIT_Z)
self.controller.desiredOrientation = expected
# Restart state machine so it loads the orientation
self.machine.stop()
self.machine.start(course.Gate)
# Make sure we have the desired quaterion saved properly
self.assertAIDataValue('gateOrientation', expected)
def interpret(self, text):
if text.strip() == 'exit':
return True
m = self._quatexp.match(text.strip())
try:
quat = math.Quaternion(float(m.group('x')), float(m.group('y')),
float(m.group('z')), float(m.group('w')))
print 'orientation:', quat2axis(quat)
except Exception, e:
print traceback.print_exc()
def testStoreDesiredQuaternion(self):
# Setup a desired orientation
expected = math.Quaternion(math.Degree(45), math.Vector3.UNIT_Z)
self.controller.desiredOrientation = expected
# Send SETTLED event
self.injectEvent(randombin.Centering.SETTLED)
# Make sure we have the desired quaterion saved properly
self.assertAIDataValue('binArrayOrientation', expected)
self.assertEqual(1, self.controller.headingHolds)
def testBinArrayOrientation(self):
# Setup data for turn hold
expected = math.Quaternion(math.Degree(25), math.Vector3.UNIT_Z)
self.ai.data['binArrayOrientation'] = expected
# Restart the state machine
self.machine.stop()
self.machine.start(self.myState)
# Make sure we have the proper orientation
self.assertEqual(expected, self.controller.desiredOrientation)
def testAngle(self):
# Dead center, with yawGain = 1
pipe = motion.pipe.Pipe(x=0, y=0, relativeAngle=15)
m = self.makeClass(pipe=pipe, maxSpeed=1, maxSidewaysSpeed=1)
self.motionManager.setMotion(m)
self.assertAlmostEqual(15, self.controller.yawChange, 3)
# Make sure bad yaw gains are protected
self.assertRaises(TypeError,
motion.pipe.Hover,
x=0,
y=0,
relativeAngle=0,
yawGain=2)
# Test a yaw gain
pipe = motion.pipe.Pipe(x=0, y=0, relativeAngle=30)
m = self.makeClass(pipe=pipe,
maxSpeed=1,
maxSidewaysSpeed=1,
yawGain=0.5)
self.motionManager.setMotion(m)
self.assertAlmostEqual(15, self.controller.yawChange, 3)
# Turning toward pipe at 30 Degrees off North
self.controller.desiredOrientation = \
math.Quaternion(math.Degree(10), math.Vector3.UNIT_Z)
self.estimator.orientation = \
math.Quaternion(math.Degree(-15), math.Vector3.UNIT_Z)
# Pipe at 45 degress left of vehicle's heading
# Make sure we only rotate relative to the controllers desired
# orientation
pipe = motion.pipe.Pipe(x=0, y=0, relativeAngle=45)
m = self.makeClass(pipe=pipe, maxSpeed=1, maxSidewaysSpeed=1)
self.motionManager.setMotion(m)
self.assertAlmostEqual(20, self.controller.yawChange, 3)
def testOffset(self):
# Vehicle pointed 30 degrees left
self.vehicle.orientation = math.Quaternion(math.Degree(30),
math.Vector3.UNIT_Z)
# Move in a direction 45 degrees left
m = self.makeClass(desiredHeading=75, speed=5)
self.motionManager.setMotion(m)
expectedSpeed = pmath.sqrt(2) / 2.0 * 5
self.assertAlmostEqual(expectedSpeed, self.controller.speed, 4)
self.assertAlmostEqual(-expectedSpeed, self.controller.sidewaysSpeed,
4)
def enter(self):
self._binDirection = self.ai.data['config'].get('Bin', {}).get(
'binDirection', None)
if self._binDirection is None:
SettlingState.enter(self, Centering.SETTLED, 5,
useMultiAngle = True)
else:
SettlingState.enter(self, Centering.SETTLED, 5,
useMultiAngle = True, shouldRotate = False)
self.controller.setDesiredOrientation(
math.Quaternion(math.Degree(self._binDirection),
math.Vector3.UNIT_Z))
def _start(self):
# Grab current State
currentOrient = self._vehicle.getOrientation()
currentHeading = currentOrient.getYaw(True).valueDegrees()
if not self._absolute:
heading = self._vehicle.getOrientation().getYaw().valueDegrees()
self._desiredHeading = heading + self._desiredHeading
# Generate our source and dest orientation
self._srcOrient = math.Quaternion(math.Degree(currentHeading),
math.Vector3.UNIT_Z)
self._destOrient = math.Quaternion(math.Degree(self._desiredHeading),
math.Vector3.UNIT_Z)
# Ensure the controller is consistent with our start state
self._controller.setDesiredOrientation(self._srcOrient)
# Determine slerp variables
absHeadingDifference = pmath.fabs(currentHeading -
self._desiredHeading)
if absHeadingDifference != 0:
stepCount = absHeadingDifference / (self._speed / self._rate)
self._rotFactor = 1.0 / (stepCount)
self._rotProgress = 0.0
self._timer = timer.Timer(self,
RateChangeHeading.NEXT_HEADING,
self._interval,
repeat=True)
# Register to NEXT_HEADING events
self._conn = self._eventHub.subscribeToType(
RateChangeHeading.NEXT_HEADING, self._onTimer)
self._timer.start()
else:
self._finish()
def testEarlyEndQueuedMotions(self):
"""
This tests that queued motions work correctly when the
first and second motion end early
"""
self.vehicle.orientation = math.Quaternion(math.Degree(0),
math.Vector3.UNIT_Z)
m1 = motion.basic.RateChangeHeading(0, 10)
m2 = motion.basic.RateChangeHeading(0, 5)
m3 = motion.basic.RateChangeDepth(5, (1.0 / 3.0))
self.motionManager.setMotion(m1, m2, m3)
self.assertEqual(motion.basic.RateChangeDepth,
type(self.motionManager.currentMotion))
def _start(self):
# Register to receive ORIENTATION_UPDATE events
conn = self._eventHub.subscribe(vehicle.IVehicle.ORIENTATION_UPDATE,
self._vehicle, self._onOrientation)
self._connections.append(conn)
# Set the desired direction if it's not absolute
if not self._absolute:
heading = self._vehicle.getOrientation().getYaw()
orientation = math.Quaternion(math.Degree(heading),
math.Vector3.UNIT_Z)
self._direction = orientation * self._direction
self._update()
def testRelativeDirection(self):
self.vehicle.orientation = math.Quaternion(math.Degree(60),
math.Vector3.UNIT_Z)
m = self.makeClass(desiredHeading=0, speed=5, absolute=False)
self.motionManager.setMotion(m)
self.assertEqual(5, self.controller.speed)
self.assertAlmostEqual(0, self.controller.sidewaysSpeed, 5)
m = self.makeClass(desiredHeading=-180, speed=5, absolute=False)
self.motionManager.setMotion(m)
self.assertEqual(-5, self.controller.speed)
self.assertAlmostEqual(0, self.controller.sidewaysSpeed, 5)
def testDuplicateAtOrientations(self):
m = motion.search.ForwardZigZag(legTime = 6, sweepAngle = 35, speed = 8)
self.motionManager.setMotion(m)
# Comlete on short leg (with duplicate AT_ORIENTATION events)
orientation = math.Quaternion(math.Degree(35/2.0),
math.Vector3.UNIT_Z)
self.controller.publishAtOrientation(orientation)
self.qeventHub.publishEvents()
mockTimerA = MockTimer.LOG[motion.search.ForwardZigZag.LEG_COMPLETE]
self.controller.publishAtOrientation(orientation)
self.qeventHub.publishEvents()
mockTimerB = MockTimer.LOG[motion.search.ForwardZigZag.LEG_COMPLETE]
self.assertEquals(mockTimerA, mockTimerB)
def testPipeLost(self):
expected = math.Quaternion(math.Degree(25), math.Vector3.UNIT_Z)
self.ai.data['gateOrientation'] = expected
self.injectEvent(vision.EventType.PIPE_LOST)
self.assertCurrentState(course.PipeStaged)
# Now the timer is active, make sure we don't create new ones
timer = MockTimer.LOG[course.PipeStaged.LOST_TIMEOUT]
self.injectEvent(vision.EventType.PIPE_LOST)
self.assertCurrentState(course.PipeStaged)
timer2 = MockTimer.LOG[course.PipeStaged.LOST_TIMEOUT]
self.assertEqual(timer, timer2)
# Release the time and make sure we move on
self.releaseTimer(course.PipeStaged.LOST_TIMEOUT)
self.assertCurrentState(course.LightStaged)
