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