def testSubsystemMaker(self): # Test the existing C++ Vehicle if 'Vehicle' in core.SubsystemMaker.getRegisteredKeys(): cfg = { 'depthCalibSlope': 33.01, 'depthCalibIntercept': 94, 'name': 'TestVehicle', 'type': 'Vehicle', 'sensor_board_file': '/dev/DOESNOTEXIST' } cfg = core.ConfigNode.fromString(str(cfg)) veh = core.SubsystemMaker.newObject(cfg, core.SubsystemList()) # Make sure the type was properly converted self.assert_(hasattr(veh, 'applyForcesAndTorques')) self.assert_(hasattr(veh, 'safeThrusters')) self.assertEquals(None, veh.safeThrusters()) # Test a mock Python one cfg = {'name': 'TestVehicle', 'type': 'MockVehicle'} cfg = core.ConfigNode.fromString(str(cfg)) veh = core.SubsystemMaker.newObject(cfg, core.SubsystemList()) # Not valid anymore #self.assertEquals(5, veh.depth) #self.assertEquals(5, veh.getDepth()) self.assert_(isinstance(veh, ext.vehicle.IVehicle))
def testCreate(self): stateMachine = ai.state.Machine({'name': 'MyMachine'}) deps = core.SubsystemList() deps.append(core.Subsystem('A', core.SubsystemList())) deps.append(core.Subsystem('B', core.SubsystemList())) deps.append(ai.state.Machine()) deps.append(core.Subsystem('C', core.SubsystemList())) deps.append(stateMachine) aiSys = ai.subsystem.AI({'AIMachineName': 'MyMachine'}, deps) # Make sure it finds the AI with a custom name self.assertEqual(stateMachine, aiSys.mainStateMachine)
def testDiconnect(self): stateMachine = ai.state.Machine({'name': 'MyMachine'}) deps = core.SubsystemList() deps.append(stateMachine) aiSys = ai.subsystem.AI({'AIMachineName': 'MyMachine'}, deps) # Stub connections def conFuncA(): self.connA = True self.connA = False mockConnA = Mock(disconnect=conFuncA) aiSys.addConnection(mockConnA) def conFuncB(): self.connB = True self.connB = False mockConnB = Mock(disconnect=conFuncB) aiSys.addConnection(mockConnB) # Make sure we disconnect them aiSys.unbackground() self.assert_(self.connA) self.assert_(self.connB)
def __init__(self, cfg, deps=None): if deps is None: deps = core.SubsystemList() estimation.IStateEstimator.__init__( self, 'StateEstimator', core.Subsystem.getSubsystemOfType(core.EventHub, deps)) self._obstacles = {}
def runMachine(start): eventHub = core.EventHub() qeventHub = core.QueuedEventHub(eventHub) subsystems = core.SubsystemList() subsystems.append(qeventHub) machine = state.Machine(deps=subsystems) sequence = raw_input('Enter sequence: ') machine.start(start) assert machine.currentState() is not None, 'State machine not started' for x in sequence: eventType = globals()[x.upper()] qeventHub.publish(eventType, core.Event()) qeventHub.publishEvents() assert machine.currentState( ) is not None, 'State machine ended prematurely' # State machine finished, send finish event qeventHub.publish(FINISH, core.Event()) qeventHub.publishEvents() assert machine.currentState() is None, 'State machine has not ended' machine.stop()
def test(self): cfg = {'name': 'Test', 'type': 'TestSubsystem'} cfg = core.ConfigNode.fromString(str(cfg)) obj = core.SubsystemMaker.newObject(cfg, core.SubsystemList()) self.assertEquals('Test', obj.getName()) self.assertEquals(0, len(obj.deps)) self.assert_(isinstance(obj, core.Subsystem))
def testCreate(self): cfg = {'name': 'Test', 'type': 'TestSubsystem'} cfg = core.ConfigNode.fromString(str(cfg)) deps = core.SubsystemList() cam1 = vision.Camera(640, 480) cam2 = vision.Camera(640, 480) visionSystem = vision.VisionSystem(cam1, cam2, cfg, deps)
def testData(self): stateMachine = ai.state.Machine({'name': 'MyMachine'}) deps = core.SubsystemList() deps.append(stateMachine) aiSys = ai.subsystem.AI({'AIMachineName': 'MyMachine'}, deps) aiSys.data['Bob'] = 10 self.assertEqual(10, aiSys.data['Bob']) self.assertEqual({}, aiSys.data['config'])
def testSubsystemMaker(self): cfg = {'type': 'NetworkHub', 'host': 'localhost'} cfg = core.ConfigNode.fromString(str(cfg)) obj = core.SubsystemMaker.newObject(cfg, core.SubsystemList()) # Make sure we have the right methods on the network object self.assert_(hasattr(obj, 'subscribe')) self.assert_(hasattr(obj, 'subscribeToType')) self.assert_(hasattr(obj, 'subscribeToAll')) self.assert_(hasattr(obj, 'publish'))
def testInit(self): # A subsystemA = SubsystemA({}, []) subsystemA = SubsystemA({'name': 'John'}, [core.EventHub()]) # B subsystemB = SubsystemB({}, None) subsystemB = SubsystemB({'name': 'Bob'}, None) # C subsystemC = SubsystemC({}, None) subsystemC = SubsystemC({}, core.SubsystemList()) subsystemC = SubsystemC({}, []) deps = core.SubsystemList() deps.append(core.EventHub()) subsystemC = SubsystemC({'name': 'Dave'}, deps) subsystemC = SubsystemC({'name': 'Dave'}, [core.EventHub()])
def testMakeEventHub(self): cfg = {'name': 'Test', 'type': 'EventHub'} cfg = core.ConfigNode.fromString(str(cfg)) ehub = core.SubsystemMaker.newObject(cfg, core.SubsystemList()) recv = Reciever() epub = core.EventPublisher(ehub) ehub.subscribeToType("Type", recv) epub.publish("Type", core.Event()) self.assertEquals(1, recv.calls)
def testSubsystemMaker(self): # Must pass an event hub to the network publisher eventHub = core.EventHub() deps = core.SubsystemList() deps.append(eventHub) cfg = {'type': 'NetworkPublisher'} cfg = core.ConfigNode.fromString(str(cfg)) obj = core.SubsystemMaker.newObject(cfg, deps) # Make sure we have the right methods on the network object self.assert_(hasattr(obj, 'subscribe')) self.assert_(hasattr(obj, 'publish'))
def testBuoyDetector(self): # Create a vision system with two mock cameras and an EventHub cfg = { 'testing': 1, 'BuoyDetector': { 'filtRedLMin': 0, 'filtRedLMax': 255, 'filtRedCMin': 0, 'filtRedCMax': 255, 'filtRedHMin': 230, 'filtRedHMax': 20 } } cfg = core.ConfigNode.fromString(str(cfg)) forwardCamera = vision.Camera(640, 480) backwardCamera = vision.Camera(640, 480) eventHub = core.EventHub() deps = core.SubsystemList() deps.append(eventHub) visionSystem = vision.VisionSystem(forwardCamera, backwardCamera, cfg, deps) # Subscribe to our events about the red light eventHub.subscribeToType(vision.EventType.BUOY_FOUND, self.buoyFoundHandler) eventHub.subscribeToType(vision.EventType.BUOY_LOST, self.buoyLostHandler) # Load our test image (really upper right) image = vision.Image.loadFromFile( os.path.join(getConfigRoot(), 'red_light_upper_left.png')) # Start detector then unbackground it visionSystem.buoyDetectorOn() visionSystem.unbackground(True) forwardCamera.capturedImage(image) forwardCamera.background(0) # This stops the background thread visionSystem.update(0) visionSystem.buoyDetectorOff() # Check the event self.assert_(self.found) self.assert_(self.event)
def __init__(self, config, deps_): # Initialize super class with fake cameras cfg = core.ConfigNode.fromString(str(config)) deps = core.SubsystemList() for d in deps_: deps.append(d) cam1 = vision.Camera(640, 480) cam2 = vision.Camera(640, 480) vision.VisionSystem.__init__(self, cam1, cam2, cfg, deps) self._currentTime = 0 self._sendEvents = False if 1 == config.get('sendEvents', 1): self._sendEvents = True
def testRedLightDetector(self): # Create a vision system with two mock cameras and an EventHub cfg = {'testing': 1} cfg = core.ConfigNode.fromString(str(cfg)) forwardCamera = vision.Camera(640, 480) backwardCamera = vision.Camera(640, 480) eventHub = core.EventHub() deps = core.SubsystemList() deps.append(eventHub) visionSystem = vision.VisionSystem(forwardCamera, backwardCamera, cfg, deps) # Subscribe to our events about the red light eventHub.subscribeToType(vision.EventType.LIGHT_FOUND, self.redFoundHandler) eventHub.subscribeToType(vision.EventType.LIGHT_LOST, self.redLostHandler) # Load our test image (really upper right) image = vision.Image.loadFromFile( os.path.join(getConfigRoot(), 'red_light_upper_left.png')) # Start detector then unbackground it visionSystem.redLightDetectorOn() visionSystem.unbackground(True) forwardCamera.capturedImage(image) forwardCamera.background(0) # This stops the background thread visionSystem.update(0) visionSystem.redLightDetectorOff() # Check the event self.assert_(self.found) self.assert_(self.event) self.assertAlmostEqual(0.5 * 4.0 / 3.0, self.event.x, 2) self.assertAlmostEqual(0.5, self.event.y, 2) self.assertAlmostEqual(3, self.event.range, 1) self.assertAlmostEqual(-78.0 / 4, self.event.azimuth.valueDegrees(), 2) self.assertAlmostEqual(105.0 / 4, self.event.elevation.valueDegrees(), 0)
def test(self): cfg = { "type" : "CombineController", "TranslationalController" : {"type" : "OpenLoopTranslationalController"}, "DepthController" : {"type" : "PIDDepthController"}, "RotationalController" : {"type" : "NonlinearPDRotationalController"}, "holdCurrentDepth" : 0, "holdCurrentHeading" : 0, "holdCurrentPosition" : 0 } cfg = core.ConfigNode.fromString(str(cfg)) obj = core.SubsystemMaker.newObject(cfg, core.SubsystemList()) # Make sure we have the right methods on the controller object controller = obj self.assert_(hasattr(controller,'translate')) self.assert_(hasattr(controller,'yawVehicle')) self.assert_(hasattr(controller,'changeDepth'))
def test(self): cfg = { "type" : "BWPDController", "angularPGain" : 10, "angularDGain" : 1, "speedPGain" : 3, "depthPGain" : 20, "desiredSpeed" : 0, "desiredDepth" : 0.25, "desiredQuaternion" : [0, 0, 0, 1] } cfg = core.ConfigNode.fromString(str(cfg)) obj = core.SubsystemMaker.newObject(cfg, core.SubsystemList()) # Make sure we have the right methods on the controller object controller = obj self.assert_(hasattr(controller,'setSpeed')) self.assert_(hasattr(controller,'yawVehicle')) self.assert_(hasattr(controller,'setDepth'))
def _branchToState(self, nextState, branchingEvent = None): if self._branches.has_key(nextState): raise Exception("Already branched to this state") # Create new state machine deps = core.SubsystemList() for subsystem in self._subsystems.itervalues(): deps.append(subsystem) branchedMachine = Machine(self._config, deps) # Start it up with the proper state branchedMachine.start(nextState) # Set the previous state to avoid unwanted transitions caused by # the event that led us hear, triggering a transition in the newly # created state machine branchedMachine._previousEvent = branchingEvent # Store new state machine self._branches[nextState] = branchedMachine
def setUp(self): self.vehicle = MockVehicle() cfg = { 'name': 'TestController', 'type': 'BWPDController', 'angularPGain': 10, 'angularDGain': 1, 'desiredQuaternion': [0, 0, 0, 1] } cfg = core.ConfigNode.fromString(str(cfg)) self.eventHub = core.EventHub() deps = core.SubsystemList() deps.append(self.eventHub) deps.append(self.vehicle) self.actualDepth = 0 self.desiredDepth = 0 self.controller = core.SubsystemMaker.newObject(cfg, deps) self.qeventHub = core.QueuedEventHub(self.eventHub)
def testMaker(self): cfg = {'name': 'TestEstimator', 'type': 'MockStateEstimator'} cfg = core.ConfigNode.fromString(str(cfg)) estimator = core.SubsystemMaker.newObject(cfg, core.SubsystemList()) self.assert_(isinstance(estimator, estimation.IStateEstimator))