def __call__(self, req):
serviceUrl = yield self._node_handle._master_proxy.lookupService(self._name)
protocol, rest = serviceUrl.split('://', 1)
host, port_str = rest.rsplit(':', 1)
port = int(port_str)
assert protocol == 'rosrpc'
conn = yield endpoints.TCP4ClientEndpoint(reactor, host, port).connect(util.AutoServerFactory(lambda addr: tcpros.Protocol()))
try:
conn.sendString(tcpros.serialize_dict(dict(
callerid=self._node_handle._name,
service=self._name,
md5sum=self._type._md5sum,
type=self._type._type,
)))
header = tcpros.deserialize_dict((yield conn.receiveString()))
# XXX do something with header
# request could be sent before header is received to reduce latency...
x = StringIO.StringIO()
self._type._request_class.serialize(req, x)
data = x.getvalue()
conn.sendString(data)
result = ord((yield conn.receiveByte()))
data = yield conn.receiveString()
if result: # success
defer.returnValue(self._type._response_class().deserialize(data))
else:
raise ServiceError(data)
finally:
conn.transport.loseConnection()
def _publisher_thread(self, url):
while True:
try:
proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(url),
self._node_handle._name)
value = yield proxy.requestTopic(self._name, [['TCPROS']])
protocol, host, port = value
conn = yield endpoints.TCP4ClientEndpoint(
reactor, host, port).connect(
util.AutoServerFactory(lambda addr: tcpros.Protocol()))
try:
conn.sendString(
tcpros.serialize_dict(
dict(
message_definition=self._type._full_text,
callerid=self._node_handle._name,
topic=self._name,
md5sum=self._type._md5sum,
type=self._type._type,
)))
header = tcpros.deserialize_dict((yield
conn.receiveString()))
self._connections[conn] = header.get('callerid', None)
try:
while True:
data = yield conn.receiveString()
msg = self._type().deserialize(data)
try:
self._callback(msg)
except:
traceback.print_exc()
self._last_message = msg
self._last_message_time = self._node_handle.get_time(
)
old, self._message_dfs = self._message_dfs, []
for df in old:
df.callback(msg)
finally:
del self._connections[conn]
finally:
conn.transport.loseConnection()
except (error.ConnectionDone, error.ConnectionLost,
error.ConnectionRefusedError):
pass
except Exception:
traceback.print_exc()
yield util.wall_sleep(
1
) # pause so that we don't repeatedly reconnect immediately on failure
def __new__(cls, ns, name, addr, master_uri, remappings):
# constraints: anything blocking here should print something if it's
# taking a long time in order to avoid confusion
self = object.__new__(cls)
if ns:
assert ns[0] == '/'
assert not ns.endswith('/')
self._ns = ns # valid values: '', '/a', '/a/b'
assert '/' not in name
self._name = self._ns + '/' + name
self._shutdown_callbacks = set()
reactor.addSystemEventTrigger('before', 'shutdown', self.shutdown)
self._addr = addr
self._master_uri = master_uri
self._remappings = remappings
self._master_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(self._master_uri),
self._name)
self._is_running = True
self._xmlrpc_handlers = {}
self._xmlrpc_server = reactor.listenTCP(
0, server.Site(_XMLRPCSlave(self)))
self._shutdown_callbacks.add(self._xmlrpc_server.loseConnection)
self._xmlrpc_server_uri = 'http://%s:%i/' % (
self._addr, self._xmlrpc_server.getHost().port)
self._tcpros_handlers = {}
@util.cancellableInlineCallbacks
def _handle_tcpros_conn(conn):
try:
header = tcpros.deserialize_dict((yield conn.receiveString()))
def default(header, conn):
conn.sendString(
tcpros.serialize_dict(
dict(error='unhandled connection')))
conn.transport.loseConnection()
if 'service' in header:
self._tcpros_handlers.get(('service', header['service']),
default)(header, conn)
elif 'topic' in header:
self._tcpros_handlers.get(('topic', header['topic']),
default)(header, conn)
else:
conn.sendString(
tcpros.serialize_dict(
dict(error='no topic or service name detected')))
conn.transport.loseConnection()
except:
conn.transport.loseConnection()
raise
def _make_tcpros_protocol(addr):
conn = tcpros.Protocol()
_handle_tcpros_conn(conn)
return conn
self._tcpros_server = reactor.listenTCP(
0, util.AutoServerFactory(_make_tcpros_protocol))
self._shutdown_callbacks.add(self._tcpros_server.loseConnection)
self._tcpros_server_uri = 'rosrpc://%s:%i' % (
self._addr, self._tcpros_server.getHost().port)
self._tcpros_server_addr = self._addr, self._tcpros_server.getHost(
).port
while True:
try:
other_node_uri = yield self._master_proxy.lookupNode(
self._name)
except rosxmlrpc.Error:
break # assume that error means unknown node
except Exception:
traceback.print_exc()
yield util.wall_sleep(1) # pause so we don't retry immediately
else:
other_node_proxy = rosxmlrpc.Proxy(
xmlrpc.Proxy(other_node_uri), self._name)
try:
yield util.wrap_timeout(
other_node_proxy.shutdown(
'new node registered with same name'), 3)
except error.ConnectionRefusedError:
pass
except Exception:
traceback.print_exc()
break
try:
self._use_sim_time = yield self.get_param('/use_sim_time')
except rosxmlrpc.Error: # assume that error means not found
self._use_sim_time = False
if self._use_sim_time:
def got_clock(msg):
self._sim_time = msg.clock
self._clock_sub = self.subscribe('/clock', Clock, got_clock)
# make sure self._sim_time gets set before we continue
yield util.wrap_time_notice(self._clock_sub.get_next_message(), 1,
'getting simulated time from /clock')
for k, v in self._remappings.iteritems():
if k.startswith('_') and not k.startswith('__'):
yield self.set_param(self.resolve_name('~' + k[1:]),
yaml.load(v))
self.advertise_service('~get_loggers', GetLoggers,
lambda req: GetLoggersResponse())
self.advertise_service('~set_logger_level', SetLoggerLevel,
lambda req: SetLoggerLevelResponse())
defer.returnValue(self)