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'))