def _compute_system_state(ctx):
socket.setdefaulttimeout(3.0)
master = rosgraph.Master('/roswtf')
# store system state
try:
val = master.getSystemState()
except rosgraph.MasterException:
return
ctx.system_state = val
pubs, subs, srvs = val
# compute list of topics and services
topics = []
for t, _ in itertools.chain(pubs, subs):
topics.append(t)
services = []
service_providers = []
for s, l in srvs:
services.append(s)
service_providers.extend(l)
ctx.topics = topics
ctx.services = services
ctx.service_providers = service_providers
# compute list of nodes
nodes = []
for s in val:
for t, l in s:
nodes.extend(l)
ctx.nodes = list(set(nodes)) #uniq
# - compute reverse mapping of URI->nodename
count = 0
start = time.time()
for n in ctx.nodes:
count += 1
try:
val = master.lookupNode(n)
except socket.error:
ctx.errors.append(
WtfError("cannot contact ROS Master at %s" %
rosgraph.rosenv.get_master_uri()))
raise WtfException(
"roswtf lost connection to the ROS Master at %s" %
rosgraph.rosenv.get_master_uri())
ctx.uri_node_map[val] = n
end = time.time()
# - time thresholds currently very arbitrary
if count:
if ((end - start) / count) > 1.:
ctx.warnings.append(
WtfError(
"Communication with master is very slow (>1s average)"))
elif (end - start) / count > .5:
ctx.warnings.append(
WtfWarning(
"Communication with master is very slow (>0.5s average)"))
def run(self):
ctx = self.ctx
master = self.master
actual_edges = self.actual_edges
lock = self.lock
n = self.n
try:
socket.setdefaulttimeout(3.0)
with lock: #Apparently get_api_uri is not thread safe...
node_api = rosnode.get_api_uri(master, n)
if not node_api:
with lock:
ctx.errors.append(
WtfError(
"Master does not have lookup information for node [%s]"
% n))
return
node = ServerProxy(node_api)
start = time.time()
socket.setdefaulttimeout(3.0)
code, msg, bus_info = node.getBusInfo('/roswtf')
end = time.time()
with lock:
if (end - start) > 1.:
ctx.warnings.append(
WtfWarning(
"Communication with node [%s] is very slow" % n))
if code != 1:
ctx.warnings.append(
WtfWarning("Node [%s] would not return bus info" % n))
elif not bus_info:
if not n in ctx.service_providers:
ctx.warnings.append(
WtfWarning(
"Node [%s] is not connected to anything" % n))
else:
edges = _businfo(ctx, n, bus_info)
actual_edges.extend(edges)
except socket.error:
pass #ignore as we have rules to catch this
except Exception as e:
ctx.errors.append(
WtfError("Communication with [%s] raised an error: %s" %
(n, str(e))))
finally:
self.done = True
def probe_all_services(ctx):
master = rosgraph.Master('/roswtf')
errors = []
for service_name in ctx.services:
try:
service_uri = master.lookupService(service_name)
except:
ctx.errors.append(
WtfError("cannot contact ROS Master at %s" %
rosgraph.rosenv.get_master_uri()))
raise WtfException(
"roswtf lost connection to the ROS Master at %s" %
rosgraph.rosenv.get_master_uri())
try:
headers = rosservice.get_service_headers(service_name, service_uri)
if not headers:
errors.append("service [%s] did not return service headers" %
service_name)
except rosgraph.network.ROSHandshakeException as e:
errors.append("service [%s] appears to be malfunctioning" %
service_name)
except Exception as e:
errors.append("service [%s] appears to be malfunctioning: %s" %
(service_name, e))
return errors
class NodeInfoThread(threading.Thread):
def __init__(self, n, ctx, master, actual_edges, lock):
threading.Thread.__init__(self)
self.master = master
self.actual_edges = actual_edges
self.lock = lock
self.n = n
self.done = False
self.ctx = ctx
def run(self):
ctx = self.ctx
master = self.master
actual_edges = self.actual_edges
lock = self.lock
n = self.n
try:
socket.setdefaulttimeout(3.0)
node_api = rosnode.get_api_uri(master, n)
if not node_api:
ctx.errors.append(
WtfError(
"Master does not have lookup information for node [%s]"
% n))
return
node = xmlrpclib.ServerProxy(node_api)
start = time.time()
socket.setdefaulttimeout(3.0)
code, msg, bus_info = node.getBusInfo('/roswtf')
end = time.time()
with lock:
if (end - start) > 1.:
ctx.warnings.append(
WtfWarning(
"Communication with node [%s] is very slow" % n))
if code != 1:
ctx.warnings.append(
WtfWarning("Node [%s] would not return bus info" % n))
elif not bus_info:
if not n in ctx.service_providers:
ctx.warnings.append(
WtfWarning(
"Node [%s] is not connected to anything" % n))
else:
edges = _businfo(ctx, n, bus_info)
actual_edges.extend(edges)
except socket.error, e:
pass #ignore as we have rules to catch this
except Exception, e:
ctx.errors.append(
WtfError("Communication with [%s] raised an error: %s" %
(n, str(e))))
def simtime_check(ctx):
if ctx.use_sim_time:
master = rosgraph.Master('/roswtf')
try:
pubtopics = master.getPublishedTopics('/')
except rosgraph.MasterException:
ctx.errors.append(WtfError("Cannot talk to ROS master"))
raise WtfException(
"roswtf lost connection to the ROS Master at %s" %
rosgraph.rosenv.get_master_uri())
for topic, _ in pubtopics:
if topic in ['/time', '/clock']:
return
return True
def simtime_check(ctx):
if ctx.use_sim_time:
master = roslib.scriptutil.get_master()
try:
code, msg, pubtopics = master.getPublishedTopics('/roswtf', '/')
except:
ctx.errors.append(WtfError("Cannot talk to ROS master"))
raise WtfException("roswtf lost connection to the ROS Master at %s"%roslib.rosenv.get_master_uri())
if code != 1:
raise WtfException("cannot get published topics from master: %s"%msg)
for topic, _ in pubtopics:
if topic in ['/time', '/clock']:
return
return True
def probe_all_services(ctx):
master = roslib.scriptutil.get_master()
errors = []
for service_name in ctx.services:
try:
code, msg, service_uri = master.lookupService('/rosservice', service_name)
except:
ctx.errors.append(WtfError("cannot contact ROS Master at %s"%roslib.rosenv.get_master_uri()))
raise WtfException("roswtf lost connection to the ROS Master at %s"%roslib.rosenv.get_master_uri())
if code != 1:
ctx.warnings.append(WtfWarning("Unable to lookup service [%s]"%service_name))
else:
try:
headers = rosservice.get_service_headers(service_name, service_uri)
if not headers:
errors.append("service [%s] did not return service headers"%service_name)
except roslib.network.ROSHandshakeException, e:
errors.append("service [%s] appears to be malfunctioning"%service_name)
except Exception, e:
errors.append("service [%s] appears to be malfunctioning: %s"%(service_name, e))