class NaoQiModule(object):
VERBOSE = True # mainly here so I can shut it down when using twisted
def __init__(self, con, modname):
self._con = con
self._modname = modname
# async? nah, don't bother right now. (everything here
# should be twisted.. ok, it can be asynced some other way)
self.initDeferred = Deferred() # hook for inheriting classes, will be called when init is done
self.post = ModulePosts()
self.methods = MethodHolder()
self.getMethods().addCallback(self.onMethodsAvailable)
def onMethodsAvailable(self, methods):
self.method_names = methods
for meth in self.method_names:
methobj = NaoQiMethod(self, meth)
setattr(self, meth, methobj)
setattr(self.methods, meth, methobj)
setattr(self.post, meth, methobj.post)
# this actually uses one of the methods above!
self._has_docs = False
self.__doc__ = """ Call con.%s.makeHelp() to generate help for this module and it's methods """ % self._modname
self.initDeferred.callback(None)
def makeHelp(self):
if self.VERBOSE:
print "going to get help..",
self._getDoc()
for meth in self.methods.__dict__.values():
if hasattr(meth, 'makeHelp'):
meth.makeHelp()
if self.VERBOSE:
print "done"
self._has_docs = True
def _getDoc(self):
""" self annihilating method """
self.moduleHelp().addCallback(self._onGetDocReturn)
def _onGetDocReturn(self, result):
self.__doc__ = result[0]
def getName(self):
return self._modname
def getMethods(self):
""" returns the method names in unicode for the given module
"""
return self._con._sendRequest(callNaoQiObject(self._modname,
'getMethodList')).addCallback(self._getMethods_onResponse)
def _getMethods_onResponse(self, soapbody):
return [e.firstChild.wholeText for e in soapbody.firstChild.firstChild.firstChild.childNodes]
def justModuleHelp(self):
return self._con._sendRequest(callNaoQiObject(self._mod._modname, 'moduleHelp'))
def __init__(self, varlist):
""" construct a SharedMemoryReader, given a variable list (names
from ALMemory). Since this is done in async style, there is a deferred
for completion of the init called self.openDeferred which
can be used, i.e. self.openDeferred.addCallback(on_shm_inited)
"""
if not self.isMMapAvailable():
raise Exception("Don't initialize me if there is no MMAP!")
self._shm_proxy = shm_proxy = burst.getBurstMemProxy(deferred=True)
self._var_names = varlist
self._fd = None
self._buf = None
self._unpack = 'f' * len(self._var_names)
start_offset = BURST_SHARED_MEMORY_VARIABLES_START_OFFSET
self._unpack_start = start_offset
self._unpack_end = start_offset + struct.calcsize(self._unpack)
self._sonar_unpack = 'f'*US_ELEMENTS_NUM
self._sonar_start = 0
self._sonar_end = struct.calcsize(self._sonar_unpack) + self._sonar_start
self.vars = dict((k, 0.0) for k in self._var_names)
# TODO - ugly (next year)
self.vars[US_DISTANCES_VARNAME] = [0.0] * US_ELEMENTS_NUM
print "SharedMemory: asked burstmem to map %s variables" % len(self._var_names)
# set all the names of the variables we want mapped - we don't block
# or anything since we expect the first few frames to be eventless,
# but this is definitely a TODO
self.openDeferred = Deferred()
shm_proxy.clearMappedVariables().addCallback(self._complete_init).addErrback(log.err)
def __init__(self, url="http://localhost:9559/", verbose = True, options=None):
self.verbose = verbose
self.options = options # the parsed command line options, convenient place to store them
self._url = url
self._message_maker = MessageMaker(url)
self._is_webots = self._message_maker._port == 9560
self.s = None # socket to connect to broker. reusing - will it work?
self._myip = getip()
self._myport = 12345 # bogus - we are acting as a broker - this needs to be a seperate class
self._error_accessing_host = False
self.modulesDeferred = Deferred()
self._modules_to_wait_for_init_of = None
if self.options.twisted:
# if we have twisted, use that implementation
try:
import twisted.internet.reactor
except:
pass
else:
print "twisted found, using self._twistedSendRequest"
self._sendRequest = self._twistedSendRequest
from .pynaoqi_twisted import SoapConnectionManager
self.connection_manager = SoapConnectionManager(self)
NaoQiModule.VERBOSE = False
self._initModules()
def __init__(self, con, modname):
self._con = con
self._modname = modname
# async? nah, don't bother right now. (everything here
# should be twisted.. ok, it can be asynced some other way)
self.initDeferred = Deferred() # hook for inheriting classes, will be called when init is done
self.post = ModulePosts()
self.methods = MethodHolder()
self.getMethods().addCallback(self.onMethodsAvailable)
class SharedMemoryReader(object):
""" read from memory mapped file and not from ALMemory proxy, which
is painfully slow (but only if the file is there - for example, we
don't want to break running controllers not on the robot)
"""
verbose = False
def __init__(self, varlist):
""" construct a SharedMemoryReader, given a variable list (names
from ALMemory). Since this is done in async style, there is a deferred
for completion of the init called self.openDeferred which
can be used, i.e. self.openDeferred.addCallback(on_shm_inited)
"""
if not self.isMMapAvailable():
raise Exception("Don't initialize me if there is no MMAP!")
self._shm_proxy = shm_proxy = burst.getBurstMemProxy(deferred=True)
self._var_names = varlist
self._fd = None
self._buf = None
self._unpack = 'f' * len(self._var_names)
start_offset = BURST_SHARED_MEMORY_VARIABLES_START_OFFSET
self._unpack_start = start_offset
self._unpack_end = start_offset + struct.calcsize(self._unpack)
self._sonar_unpack = 'f'*US_ELEMENTS_NUM
self._sonar_start = 0
self._sonar_end = struct.calcsize(self._sonar_unpack) + self._sonar_start
self.vars = dict((k, 0.0) for k in self._var_names)
# TODO - ugly (next year)
self.vars[US_DISTANCES_VARNAME] = [0.0] * US_ELEMENTS_NUM
print "SharedMemory: asked burstmem to map %s variables" % len(self._var_names)
# set all the names of the variables we want mapped - we don't block
# or anything since we expect the first few frames to be eventless,
# but this is definitely a TODO
self.openDeferred = Deferred()
shm_proxy.clearMappedVariables().addCallback(self._complete_init).addErrback(log.err)
def _complete_init(self, _):
# TODO - this is slow but only done on init
map_d = chainDeferreds([lambda result, i=i, varname=varname: self._shm_proxy.addMappedVariable(i, varname)
for i, varname in enumerate(self._var_names)])
map_d.addCallback(lambda _: self._shm_proxy.getNumberOfVariables().addCallback(
self._reportNumberOfVariablesInBurstmem))
map_d.addCallback(lambda _:
self._shm_proxy.isMemoryMapRunning().addCallback(self._completeOpen))
map_d.addErrback(log.err)
def _reportNumberOfVariablesInBurstmem(self, num):
print "SharedMemory: burstmem says it has %s variables" % num
def _completeOpen(self, mmap_running):
""" callback for shm_proxy.isMemoryMapRunning called by open """
print "SharedMemory: _completeOpen: memory mapped = %s (if True then previous session didn't close it)" % mmap_running
def assertMMAPRunning(is_running):
assert(is_running)
self._shm_proxy.startMemoryMap().addCallback(
lambda _: self._shm_proxy.isMemoryMapRunning().addCallback(assertMMAPRunning)).addErrback(log.err)
if self._fd is not None: return
data = open(MMAP_FILENAME, 'r')
self._fd = fd = data.fileno()
self._buf = mmap.mmap(fd, MMAP_LENGTH, mmap.MAP_SHARED | mmap.ACCESS_READ, mmap.PROT_READ)
print "world: shared memory opened successfully"
self.openDeferred.callback(None)
def close(self):
if self._fd is None: return
# no mmap.munmap??
self._fd.close()
self._fd = None
self._buf = None
def update(self):
# TODO - this is slow
# TODO - instead of updating the dict I could just update the
# values and only make the dict if someone explicitly wants it,
# and give values using cached indices created in constructor.
if not self._buf: return
values = struct.unpack(self._unpack, self._buf[self._unpack_start:self._unpack_end])
# TODO - would a single dict.update be faster?
for k, v in zip(self._var_names, values):
self.vars[k] = v
# update sonar variables differently - they are stored at the beginning
# of the shared memory region
self.vars[US_DISTANCES_VARNAME] = struct.unpack(self._sonar_unpack,
self._buf[self._sonar_start:self._sonar_end])
if self.verbose:
print self.vars
@classmethod
def isMMapAvailable(cls):
return (os.path.exists(MMAP_FILENAME))
@classmethod
def tryToInitMMap(cls):
""" This is just the mmap setup file hidden in a class variable here.
It is perfectly safe to run on a regular computer since it won't
do anything if it finds it isn't on a nao, and only creates a file
if there is none there right now.
"""
if not os.path.exists(MMAP_FILENAME):
fd = open(MMAP_FILENAME, "w+")
fd.write(MMAP_LENGTH*"\00")
fd.close()
assert(os.path.exists(MMAP_FILENAME))
assert(os.stat(MMAP_FILENAME)[stat.ST_SIZE] == MMAP_LENGTH)
class BaseNaoQiConnection(object):
def __init__(self, url="http://localhost:9559/", verbose = True, options=None):
self.verbose = verbose
self.options = options # the parsed command line options, convenient place to store them
self._url = url
self._message_maker = MessageMaker(url)
self._is_webots = self._message_maker._port == 9560
self.s = None # socket to connect to broker. reusing - will it work?
self._myip = getip()
self._myport = 12345 # bogus - we are acting as a broker - this needs to be a seperate class
self._error_accessing_host = False
self.modulesDeferred = Deferred()
self._modules_to_wait_for_init_of = None
if self.options.twisted:
# if we have twisted, use that implementation
try:
import twisted.internet.reactor
except:
pass
else:
print "twisted found, using self._twistedSendRequest"
self._sendRequest = self._twistedSendRequest
from .pynaoqi_twisted import SoapConnectionManager
self.connection_manager = SoapConnectionManager(self)
NaoQiModule.VERBOSE = False
self._initModules()
def getHost(self):
return self._message_maker._host
host = property(getHost)
def _initModules(self):
""" internal method. Initializes the list of modules and their list of methods """
self._modules = []
modules = []
try:
modules = self.getModules()
except urllib2.URLError:
if not self._error_accessing_host:
print "!!! connection refused, will retry 1 second after reactor.start"
self._error_accessing_host = True
import twisted.internet.reactor as reactor
reactor.callLater(1.0, self._initModules)
for i, modname in enumerate(modules):
if self.verbose:
print "(%s) %s.." % (i + 1, modname),
sys.stdout.flush()
mod = self.getModule(modname)
self._modules.append(mod)
if len(modules) > 0 and self.verbose:
print
self.modules = ModulesHolder()
for m in self._modules:
self.__dict__[m.getName()] = m
self.modules.__dict__[m.getName()] = m
if len(self._modules) > 0:
# assume this is the fixed number of modules.
# this doesn't hold if we start in the middle of
# a loading naoqi - live with it.
self._modules_to_wait_for_init_of = len(self._modules)
for m in self._modules:
m.initDeferred.addErrback(lambda failure, m=m: self._moduleInitDoneFailure(failure, m))
m.initDeferred.addCallback(self._moduleInitDone)
def _moduleInitDoneFailure(self, failure, module):
print "%s init failed" % module.getName()
failure.printTraceback()
self._moduleInitDone()
def _moduleInitDone(self, _):
self._modules_to_wait_for_init_of -= 1
if self._modules_to_wait_for_init_of <= 0:
self.modulesDeferred.callback(None)
def contentLengthFromHeaders(self, headers):
return int(re.search('Content-Length: ([0-9]+)', headers).groups()[0])
def closeSocketFromHeaders(self, headers):
return re.search('Connection: (.*)', headers).groups()[0].strip() == 'close'
def _sendRequest(self, o):
""" sends a request, returns a Deferred, which you can do addCallback on.
If this is in fact synchronous, your callback will be called immediately,
otherwise it will be called upon availability of data from socket.
"""
if self.s is None:
self.s = socket.socket()
self.s.connect((self._message_maker._host, self._message_maker._port))
s = self.s
tosend = self._message_maker.make(o, keepalive=True)
if DEBUG:
print "*** Sending: ***\n%s" % tosend
s.send(tosend)
# get headers, read size, read the rest
h = []
c = None
if DEBUG:
print "receiving:"
while c != '<':
c = s.recv(1)
if DEBUG:
sys.stdout.write(c)
sys.stdout.flush()
h.append(c)
if DEBUG:
print
headers = ''.join(h[:-1])
content_length = self.contentLengthFromHeaders(headers)
# Connection: close
close_socket = self.closeSocketFromHeaders(headers)
if close_socket and DEBUG_CLOSE:
print "Will close socket"
if DEBUG:
print "expecting %s" % content_length
# this loop is required for getting large stuff, like
# getRemoteImage (1200000~ bytes for 640x480 RGB)
rest = []
left = content_length - 1
while left > 0:
rest.append(s.recv(content_length-1))
left -= len(rest[-1])
if DEBUG:
print "memory = %s" % memory.memory()
body = h[-1] + ''.join(rest)
if DEBUG:
print "*** Got: ***\n%s" % compresstoprint(
headers + body, 1000, 1000)
print "*************************"
if close_socket:
self.s.close()
self.s = None
xml = minidom.parseString(body)
soapbody = xml.documentElement.firstChild
return succeed(soapbody)
def _twistedSendRequest(self, o):
""" send a request for object o, return deferred to be called with
result as soapbody instance
"""
tosend = self._message_maker.make(o, keepalive=True)
return self.connection_manager.sendPacket(tosend)
# Reflection api - getMethods, getModules
def getModules(self):
""" get the modules list by parsing the http page for the broker -
probably there is another way, but who cares?! 8)
Changes:
* Supports naoqi 1.6.0 sdk - still using soap, but format changed. Page
now includes a little more data. Instead of the nextSibling strategy
using [-1] which should be a little more future proof as long as
modules is the last entry.
"""
x = minidom.parse(urllib2.urlopen(self._url))
page=x.getElementsByTagName('page')[0]
content=page.getElementsByTagName('content')[0]
broker=content.getElementsByTagName('broker')[0]
modulesroot = broker.getElementsByTagName('modules')[0]
modules = modulesroot.getElementsByTagName('module')
module_names = [m.getElementsByTagName('name')[0].firstChild.nodeValue for m in modules]
return module_names
def getModule(self, modname):
if modname == 'ALMotion':
# specializations
return ALMotionExtended(self)
return NaoQiModule(self, modname)
def makeHelp(self):
for m in self._modules:
m.makeHelp()
# Helpers
def call(self, modname, meth, *args):
""" debugging helper call method. In general better to use
self.module_name.method_name(*args)
"""
return self._sendRequest(callNaoQiObject(modname, meth, *args))
