def testRadianDegree(self):
self.assertEquals(20, math.Degree(20).valueDegrees())
rad = math.Radian(math.Degree(20))
self.assertEquals(20, rad.valueDegrees())
deg = math.Degree(math.Radian(pmath.pi))
self.assertAlmostEqual(180, deg.valueDegrees(), 3)
# Ensure no automatic double to Radian/Degree conversions occur
try:
deg - 1
self.fail("Implicit conversions from float/int not allowed")
except TypeError:
pass
try:
deg - 2.0
self.fail("Implicit conversions from float/int not allowed")
except TypeError:
pass
try:
rad - 3
self.fail("Implicit conversions from float/int not allowed")
except TypeError:
pass
try:
rad - 4.0
self.fail("Implicit conversions from float/int not allowed")
except TypeError:
pass
def testLightFound(self):
"""Make sure new found events move the vehicle"""
# Light dead ahead and below us
self.injectEvent(vision.EventType.LIGHT_FOUND,
vision.RedLightEvent,
0,
0,
y=-0.5,
azimuth=math.Degree(15))
# Bigger numbers = deeper, and we want to go deeper
self.assertGreaterThan(self.controller.depth, self.estimator.depth)
self.assertGreaterThan(self.controller.yawChange, 0)
self.assertEqual(0, self.ai.data['lastLightEvent'].x)
self.assertEqual(-0.5, self.ai.data['lastLightEvent'].y)
# Smaller numbers = shallow, and we want to go shallower
self.injectEvent(vision.EventType.LIGHT_FOUND,
vision.RedLightEvent,
0,
0,
y=0.5,
azimuth=math.Degree(15))
self.assertLessThan(self.controller.depth, self.estimator.depth)
self.assertGreaterThan(self.controller.yawChange, 0)
self.assertEqual(0, self.ai.data['lastLightEvent'].x)
self.assertEqual(0.5, self.ai.data['lastLightEvent'].y)
def testPipeFound(self):
"""Make sure it has an effect on the vehicle, and throws event"""
# The outside angle case
self.publishQueuedPipeFound(x=0.5, y=-0.5, angle=math.Degree(15.0))
self.qeventHub.publishEvents()
# Goes forward even when pipe behind vehicle
self.assertGreaterThan(int(self.controller._velocity.y), 0)
self.assertGreaterThan(self.controller.sidewaysSpeed, 0)
self.assertEqual(0, self.controller.yawChange)
self.controller.speed = 0
self.controller.sidewaysSpeed = 0
# The inside range case
self.publishQueuedPipeFound(x=0.4, y=-0.4, angle=math.Degree(15.0))
self.qeventHub.publishEvents()
self.assertGreaterThan(self.controller.speed, 0)
self.assertGreaterThan(self.controller.sidewaysSpeed, 0)
self.assertGreaterThan(self.controller.yawChange, 0)
self.controller.speed = 0
self.controller.sidewaysSpeed = 0
# Test the transition to a new pipe (ie, now pipes in front)
self.called = False
def handler(event):
self.called = True
self.eventHub.subscribeToType(pipe.AlongPipe.FOUND_NEW_PIPE, handler)
self.publishQueuedPipeFound(x=0.4, y=0.4, angle=math.Degree(15.0))
self.assert_(self.called)
def testBuoyFound(self):
"""Make sure new found events move the vehicle"""
# Light dead ahead and below us
self.injectEvent(vision.EventType.BUOY_FOUND,
vision.BuoyEvent,
0,
0,
vision.Color.YELLOW,
y=-0.5,
azimuth=math.Degree(15))
# Bigger numbers = deeper, and the vehicle should not change depth
self.assertEqual(self.controller.depth, self.estimator.depth)
self.assertGreaterThan(self.controller.yawChange, 0)
self.assertEqual(0, self.ai.data['buoyData'][vision.Color.YELLOW].x)
self.assertEqual(-0.5, self.ai.data['buoyData'][vision.Color.YELLOW].y)
# Smaller numbers = shallow, and the vehicle should not change depth
self.injectEvent(vision.EventType.BUOY_FOUND,
vision.BuoyEvent,
0,
0,
vision.Color.YELLOW,
y=0.5,
azimuth=math.Degree(15))
self.assertEqual(self.controller.depth, self.estimator.depth)
self.assertGreaterThan(self.controller.yawChange, 0)
self.assertEqual(0, self.ai.data['buoyData'][vision.Color.YELLOW].x)
self.assertEqual(0.5, self.ai.data['buoyData'][vision.Color.YELLOW].y)
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 testSeek(self):
"""Make sure we try to hit the light when close"""
# Test that the state doesn't change until it gets a POINT_ALIGNED
self.injectEvent(vision.EventType.LIGHT_FOUND, vision.RedLightEvent, 0,
0, y = 0.5, azimuth = math.Degree(15))
self.injectEvent(vision.EventType.LIGHT_FOUND, vision.RedLightEvent, 0,
0, y = 0.5, azimuth = math.Degree(15))
self.qeventHub.publishEvents()
self.assertCurrentState(light.Align)
# Inject the POINT_ALIGNED event and see if it holds depth
self.injectEvent(ram.motion.seek.SeekPoint.POINT_ALIGNED)
self.qeventHub.publishEvents()
self.assertEqual(1, self.controller.depthHolds)
# Now give another light event with too high of a change on only one
# axis
self.injectEvent(vision.EventType.LIGHT_FOUND, vision.RedLightEvent, 0,
0, y = 0, azimuth = math.Degree(15))
self.qeventHub.publishEvents()
self.assertCurrentState(light.Align)
# Now inject the same event so the change is 0
self.injectEvent(vision.EventType.LIGHT_FOUND, vision.RedLightEvent, 0,
0, y = 0, azimuth = math.Degree(15))
self.qeventHub.publishEvents()
self.assertCurrentState(light.Seek)
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 testFoundPipe(self):
cstate = self.machine.currentState()
# No bias, no threshold
cstate._biasDirection = None
cstate._threshold = None
self.publishQueuedPipeFound(angle=math.Degree(0), id=0)
self.assertNotEqual(None, self._foundPipeEvent)
self._foundPipeEvent = None
# Just bias
cstate._biasDirection = math.Degree(0)
cstate._threshold = None
self.publishQueuedPipeFound(angle=math.Degree(0), id=0)
self.assertNotEqual(False, cstate.PIPE_FOUND(self._foundPipeEvent))
self._foundPipeEvent = None
# Bias and threshold inside threshold
cstate._biasDirection = math.Degree(0)
cstate._threshold = math.Degree(45)
self.publishQueuedPipeFound(angle=math.Degree(35), id=0)
self.assertNotEqual(False, cstate.PIPE_FOUND(self._foundPipeEvent))
self._foundPipeEvent = None
# Bias and threshold outside threshold
cstate._biasDirection = math.Degree(0)
cstate._threshold = math.Degree(45)
self.publishQueuedPipeFound(angle=math.Degree(50), id=0)
self.assertFalse(cstate.PIPE_FOUND(self._foundPipeEvent))
self._foundPipeEvent = None
def testBinFoundCentered(self):
self.ai.data['binData']['currentID'] = 3
self.ai.data['binData']['currentIds'] = set([3])
# Now test centered
self._centered = False
# Test wrong ID
self.injectBinFound(x=0, y=0, id=4, angle=math.Degree(0))
self.assertFalse(self._centered)
# Proper centered (6 is proper ID changed in binFoundHelper)
self.injectBinFound(x=0, y=0, id=3, angle=math.Degree(0))
self.qeventHub.publishEvents()
self.assert_(self._centered)
def BIN_FOUND(self, event):
"""Update the state of the light, this moves the vehicle"""
# Set up the histogram. setdefault will make sure that it sets up
# the histogram if it isn't set up and will get the histogram if
# it already exists
if not self._useMultiAngle:
# Use only the bin angle
if self._first:
self._first = False
self._lastAngle = event.angle
else:
# Only allow sane angles (less then 95)
if pmath.fabs(event.angle.valueDegrees()) < 95:
# Don't accept a new angle if it varies to much from the
# last given angle
lastDegree = self._lastAngle.valueDegrees()
currentDegree = event.angle.valueDegrees()
if (pmath.fabs(lastDegree - currentDegree) > self._filterLimit):
event.angle = math.Degree(lastDegree)
else:
self._lastAngle = event.angle
else:
# Using the array of bin angle instead
event.angle = self._multiAngle
# Only listen to the current bin ID
if self._currentBin(event):
self._bin.setState(event.x, event.y, event.angle, event.timeStamp)
self.ai.data["lastBinX"] = event.x
self.ai.data["lastBinY"] = event.y
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 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 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 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 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 __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 _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 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 _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 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 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 testDroppedFirst(self):
"""Make sure we move on after dropping the second marker"""
self.ai.data['preBinCruiseDepth'] = 5.0
# Try an invalid event first
self.publishQueuedBinFound(id=0, x=0.1, y=0.1, angle=math.Degree(0.5))
# Inject event and test the response
self.publishQueuedBinFound(id=0, x=0, y=0, angle=math.Degree(0.5))
self.qeventHub.publishEvents()
#self.publishQueuedBinFound(id = 0, x = 0, y = 0,
# angle = math.Degree(0.5))
self.releaseTimer(randombin.DropMarker.DROP)
self.qeventHub.publishEvents()
self.assertCurrentState(randombin.SurfaceToCruise)
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 testFound(self):
self.ai.data["lastBinX"] = 0.5
self.ai.data["lastBinY"] = -0.5
self.machine.start(randombin.RecoverSeeking)
self.injectEvent(vision.EventType.BIN_FOUND, vision.BinEvent, 0, 0,
vision.Symbol.UNKNOWN, math.Degree(0))
self.releaseTimer(randombin.Recover.RETURN)
self.assertCurrentState(randombin.Seeking)
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
# 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 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 testPipeDropped(self):
"""Ensure that when the current pipe is dropped its no longer current"""
# Get it tracking a pipe
self.publishQueuedPipeFound(x=0, y=-0, angle=math.Degree(-90), id=1)
self.qeventHub.publishEvents()
self.assertDataValue(self.ai.data['pipeData'], 'currentID', 1)
# Now drop the pipe and make sure the current id is removed
self.publishQueuedPipeDropped(id=1)
self.assertFalse(self.ai.data['pipeData'].has_key('currentID'))
