p36850791
/
kinetic-devel
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

to ros_comm

This commit is contained in:
Tully Foote 2010-10-26 21:31:01 +00:00
parent 30c9d18c47
commit f4ca3d1372
11 changed files with 0 additions and 1370 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
tools/rosgraph/CMakeLists.txt
View File

@ -1,30 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})

1
tools/rosgraph/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

28
tools/rosgraph/mainpage.dox
View File

@ -1,28 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b rosgraph contains command-line and graphical tools for visualizing the ROS Computation Graph.
\section commandline Command-line tools
\subsection rxgraph rxgraph
\b rxgraph is a graphical tool that visualizes the connections between ROS nodes and topics.
\verbatim
$ rxgraph
\endverbatim
\subsection rosgraph rosgraph
The \b rosgraph command-line tool that displays information about connectivity between ROS nodes.
\subsubsection Usage
\verbatim
$ rosgraph
\endverbatim
*/

20
tools/rosgraph/manifest.xml
View File

@ -1,20 +0,0 @@
<package>
<description brief="rosgraph command-line tool">
<p>
rosgraph contains the rosgraph command-line tool, which prints information about the ROS Computation Graph. It also provides an internal library that is used by the graphical version of this tool, <tt>rxgraph</tt>.
</p>
</description>
<author>Ken Conley</author>
<license>BSD</license>
<review status="Doc reviewed" notes="2009/12/30"/>
<url>http://ros.org/wiki/rosgraph</url>
<depend package="roslib"/>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
<platform os="macports" version="macports"/>
</package>

39
tools/rosgraph/nodes/rosgraph
View File

@ -1,39 +0,0 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: rosgraph 3804 2009-02-11 02:16:00Z rob_wheeler $
import roslib; roslib.load_manifest('rosgraph')
import rosgraph.rosgraph_main
rosgraph.rosgraph_main.rosgraph_main()

38
tools/rosgraph/nodes/rxgraph
View File

@ -1,38 +0,0 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: rxgraph 3804 2009-02-11 02:16:00Z rob_wheeler $
import roslib; roslib.load_manifest('rosgraph')
import rosgraph.rxgraph
rosgraph.rxgraph.rxgraph_main()

34
tools/rosgraph/src/rosgraph/__init__.py
View File

@ -1,34 +0,0 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$

34
tools/rosgraph/src/rosgraph/impl/__init__.py
View File

@ -1,34 +0,0 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: __init__.py 5735 2009-08-20 21:31:27Z sfkwc $

580
tools/rosgraph/src/rosgraph/impl/graph.py
View File

@ -1,580 +0,0 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$
"""
Data structures and library for representing ROS Computation Graph state.
"""
import sys
import time
import itertools
import random
import logging
import traceback
import xmlrpclib
import socket
import roslib.scriptutil
logger = logging.getLogger('rosgraph.graph')
_ROS_NAME = '/rosviz'
def topic_node(topic):
"""
In order to prevent topic/node name aliasing, we have to remap
topic node names. Currently we just prepend a space, which is
an illegal ROS name and thus not aliased.
@return str: topic mapped to a graph node name.
"""
return ' ' + topic
def node_topic(node):
"""
Inverse of topic_node
@return str: undo topic_node() operation
"""
return node[1:]
class BadNode(object):
"""
Data structure for storing info about a 'bad' node
"""
## no connectivity
DEAD = 0
## intermittent connectivity
WONKY = 1
def __init__(self, name, type, reason):
"""
@param type: DEAD | WONKY
@type type: int
"""
self.name =name
self.reason = reason
self.type = type
class EdgeList(object):
"""
Data structure for storing Edge instances
"""
__slots__ = ['edges_by_start', 'edges_by_end']
def __init__(self):
# in order to make it easy to purge edges, we double-index them
self.edges_by_start = {}
self.edges_by_end = {}
def __iter__(self):
return itertools.chain(*[v for v in self.edges_by_start.itervalues()])
def has(self, edge):
"""
@return: True if edge is in edge list
@rtype: bool
"""
key = edge.key
return key in self.edges_by_start and \
edge in self.edges_by_start[key]
def add(self, edge):
"""
Add an edge to our internal representation. not multi-thread safe
@param edge: edge to add
@type edge: Edge
"""
# see note in __init__
def update_map(map, key, edge):
if key in map:
l = map[key]
if not edge in l:
l.append(edge)
return True
else:
return False
else:
map[key] = [edge]
return True
updated = update_map(self.edges_by_start, edge.key, edge)
updated = update_map(self.edges_by_end, edge.rkey, edge) or updated
return updated
def add_edges(self, start, dest, direction, label=''):
"""
Create Edge instances for args and add resulting edges to edge
list. Convenience method to avoid repetitve logging, etc...
@param edge_list: data structure to add edge to
@type edge_list: EdgeList
@param start: name of start node. If None, warning will be logged and add fails
@type start: str
@param dest: name of start node. If None, warning will be logged and add fails
@type dest: str
@param direction: direction string (i/o/b)
@type direction: str
@return: True if update occured
@rtype: bool
"""
# the warnings should generally be temporary, occuring of the
# master/node information becomes stale while we are still
# doing an update
updated = False
if not start:
logger.warn("cannot add edge: cannot map start [%s] to a node name", start)
elif not dest:
logger.warn("cannot add edge: cannot map dest [%s] to a node name", dest)
else:
for args in edge_args(start, dest, direction, label):
updated = self.add(Edge(*args)) or updated
return updated
def delete_all(self, node):
"""
Delete all edges that start or end at node
@param node: name of node
@type node: str
"""
def matching(map, pref):
return [map[k] for k in map.iterkeys() if k.startswith(pref)]
pref = node+"|"
edge_lists = matching(self.edges_by_start, pref) + matching(self.edges_by_start, pref)
for el in edge_lists:
for e in el:
self.delete(e)
def delete(self, edge):
# see note in __init__
def update_map(map, key, edge):
if key in map:
edges = map[key]
if edge in edges:
edges.remove(edge)
return True
update_map(self.edges_by_start, edge.key, edge)
update_map(self.edges_by_end, edge.rkey, edge)
class Edge(object):
"""
Data structure for representing ROS node graph edge
"""
__slots__ = ['start', 'end', 'label', 'key', 'rkey']
def __init__(self, start, end, label=''):
self.start = start
self.end = end
self.label = label
self.key = "%s|%s"%(self.start, self.label)
# reverse key, indexed from end
self.rkey = "%s|%s"%(self.end, self.label)
def __ne__(self, other):
return self.start != other.start or self.end != other.end
def __str__(self):
return "%s->%s"%(self.start, self.end)
def __eq__(self, other):
return self.start == other.start and self.end == other.end
def edge_args(start, dest, direction, label):
"""
compute argument ordering for Edge constructor based on direction flag
@param direction str: 'i', 'o', or 'b' (in/out/bidir) relative to \a start
@param start str: name of starting node
@param start dest: name of destination node
"""
edge_args = []
if direction in ['o', 'b']:
edge_args.append((start, dest, label))
if direction in ['i', 'b']:
edge_args.append((dest, start, label))
return edge_args
class Graph(object):
"""
Utility class for polling ROS statistics from running ROS graph.
Not multi-thread-safe
"""
def __init__(self, node_ns='/', topic_ns='/'):
self.master = roslib.scriptutil.get_master()
self.node_ns = node_ns or '/'
self.topic_ns = topic_ns or '/'
# ROS nodes
self.nn_nodes = set([])
# ROS topic nodes
self.nt_nodes = set([])
# ROS nodes that aren't responding quickly
self.bad_nodes = {}
import threading
self.bad_nodes_lock = threading.Lock()
# ROS services
self.srvs = set([])
# ROS node->node transport connections
self.nn_edges = EdgeList()
# ROS node->topic connections
self.nt_edges = EdgeList()
# ROS all node->topic connections, including empty
self.nt_all_edges = EdgeList()
# node names to URI map
self.node_uri_map = {} # { node_name_str : uri_str }
# reverse map URIs to node names
self.uri_node_map = {} # { uri_str : node_name_str }
# time we last contacted master
self.last_master_refresh = 0
self.last_node_refresh = {}
# time we last communicated with master
# seconds until master data is considered stale
self.master_stale = 5.0
# time we last communicated with node
# seconds until node data is considered stale
self.node_stale = 5.0 #seconds
def set_master_stale(self, stale_secs):
"""
@param stale_secs: seconds that data is considered fresh
@type stale_secs: double
"""
self.master_stale = stale_secs
def set_node_stale(self, stale_secs):
"""
@param stale_secs: seconds that data is considered fresh
@type stale_secs: double
"""
self.node_stale = stale_secs
def _master_refresh(self):
"""
@return: True if nodes information was updated
@rtype: bool
"""
logger.debug("master refresh: starting")
updated = False
#TODO: getSystemState probably needs to return URIs instead
try:
code, msg, val = self.master.getSystemState(_ROS_NAME)
except socket.error, msg:
print >> sys.stderr, "Unable to contact master", msg
logger.error("unable to contact master: %s", msg)
return False
if code != 1:
print >> sys.stderr, "Unable to contact master: %s"%msg
logger.error("unable to contact master: %s", msg)
updated = False
else:
pubs, subs, srvs = val
nodes = []
nt_all_edges = self.nt_all_edges
nt_nodes = self.nt_nodes
for state, direction in ((pubs, 'o'), (subs, 'i')):
for topic, l in state:
if topic.startswith(self.topic_ns):
nodes.extend([n for n in l if n.startswith(self.node_ns)])
nt_nodes.add(topic_node(topic))
for node in l:
updated = nt_all_edges.add_edges(
node, topic_node(topic), direction) or updated
nodes = set(nodes)
srvs = set([s for s, _ in srvs])
purge = None
if nodes ^ self.nn_nodes:
purge = self.nn_nodes - nodes
self.nn_nodes = nodes
updated = True
if srvs ^ self.srvs:
self.srvs = srvs
updated = True
if purge:
logger.debug("following nodes and related edges will be purged: %s", ','.join(purge))
for p in purge:
logger.debug('purging edges for node %s', p)
self.nn_edges.delete_all(p)
self.nt_edges.delete_all(p)
self.nt_all_edges.delete_all(p)
logger.debug("master refresh: done, updated[%s]", updated)
return updated
def _mark_bad_node(self, node, reason):
try:
# bad nodes are updated in a separate thread, so lock
self.bad_nodes_lock.acquire()
if node in self.bad_nodes:
self.bad_nodes[node].type = BadNode.DEAD
else:
self.bad_nodes[node] = (BadNode(node, BadNode.DEAD, reason))
finally:
self.bad_nodes_lock.release()
def _unmark_bad_node(self, node, reason):
"""
Promotes bad node to 'wonky' status.
"""
try:
# bad nodes are updated in a separate thread, so lock
self.bad_nodes_lock.acquire()
bad = self.bad_nodes[node]
bad.type = BadNode.WONKY
finally:
self.bad_nodes_lock.release()
def _node_refresh_businfo(self, node, api, bad_node=False):
"""
Retrieve bus info from the node and update nodes and edges as appropriate
@param node: node name
@type node: str
@param api: XML-RPC proxy
@type api: ServerProxy
@param bad_node: If True, node has connectivity issues and
should be treated differently
@type bad_node: bool
"""
try:
logger.debug("businfo: contacting node [%s] for bus info", node)
# unmark bad node, though it stays on the bad list
if bad_node:
self._unmark_bad_node(node)
# Lower the socket timeout as we cannot abide by slow HTTP timeouts.
# If a node cannot meet this timeout, it goes on the bad list
# TODO: override transport instead.
old_timeout = socket.getdefaulttimeout()
if bad_node:
#even stricter timeout for bad_nodes right now
socket.setdefaulttimeout(0.2)
else:
socket.setdefaulttimeout(1.0)
code, msg, bus_info = api.getBusInfo(_ROS_NAME)
socket.setdefaulttimeout(old_timeout)
except Exception, e:
# node is (still) bad
self._mark_bad_node(node, str(e))
code = -1
msg = traceback.format_exc()
updated = False
if code != 1:
logger.error("cannot get stats info from node [%s]: %s", node, msg)
else:
# [[connectionId1, destinationId1, direction1, transport1, ...]... ]
for info in bus_info:
connection_id = info[0]
dest_id = info[1]
direction = info[2]
transport = info[3]
topic = info[4]
if len(info) > 5:
connected = info[5]
else:
connected = True #backwards compatibility
if connected and topic.startswith(self.topic_ns):
# blindly add as we will be able to catch state change via edges.
# this currently means we don't cleanup topics
self.nt_nodes.add(topic_node(topic))
# update node->topic->node graph edges
updated = self.nt_edges.add_edges(node, topic_node(topic), direction) or updated
# update node->node graph edges
if dest_id.startswith('http://'):
#print "FOUND URI", dest_id
dest_name = self.uri_node_map.get(dest_id, None)
updated = self.nn_edges.add_edges(node, dest_name, direction, topic) or updated
else:
#TODO: anyting to do here?
pass
return updated
def _node_refresh(self, node, bad_node=False):
"""
Contact node for stats/connectivity information
@param node: name of node to contact
@type node: str
@param bad_node: if True, node has connectivity issues
@type bad_node: bool
@return: True if node was successfully contacted
@rtype bool
"""
# TODO: I'd like for master to provide this information in
# getSystemState() instead to prevent the extra connection per node
updated = False
uri = self._node_uri_refresh(node)
try:
if uri:
api = xmlrpclib.ServerProxy(uri)
updated = self._node_refresh_businfo(node, api, bad_node)
except KeyError, e:
logger.warn('cannot contact node [%s] as it is not in the lookup table'%node)
return updated
def _node_uri_refresh(self, node):
try:
code, msg, uri = self.master.lookupNode(_ROS_NAME, node)
except:
code = -1
msg = traceback.format_exc()
if code != 1:
#TODO:REMOVE
print >> sys.stderr, "Unknown node reported: %s"%msg
logger.warn("master reported error in node lookup: %s"%msg)
return None
else:
# update maps
self.node_uri_map[node] = uri
self.uri_node_map[uri] = node
return uri
def _node_uri_refresh_all(self):
"""
Build self.node_uri_map and self.uri_node_map using master as a
lookup service. This will make N requests to the master for N
nodes, so this should only be used sparingly
"""
for node in self.nn_nodes:
self._node_uri_refresh(node)
def bad_update(self):
"""
Update loop for nodes with bad connectivity. We box them separately
so that we can maintain the good performance of the normal update loop.
Once a node is on the bad list it stays there.
"""
last_node_refresh = self.last_node_refresh
# nodes left to check
try:
self.bad_nodes_lock.acquire()
# make copy due to multithreading
update_queue = self.bad_nodes.values()[:]
finally:
self.bad_nodes_lock.release()
# return value. True if new data differs from old
updated = False
# number of nodes we checked
num_nodes = 0
start_time = time.time()
while update_queue:
# figure out the next node to contact
next = update_queue.pop()
# rate limit talking to any particular node
if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale):
updated = self._node_refresh(next.name, True) or updated
# include in random offset (max 1/5th normal update interval)
# to help spread out updates
last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0)
num_nodes += 1
# small yield to keep from torquing the processor
time.sleep(0.01)
end_time = time.time()
#print "Update (bad nodes) took %ss for %s nodes"%((end_time-start_time), num_nodes)
logger.debug("ROS stats (bad nodes) update took %ss"%(end_time-start_time))
return updated
def update(self):
"""
Update all the stats. This method may take awhile to complete as it will
communicate with all nodes + master.
"""
last_node_refresh = self.last_node_refresh
# nodes left to check
update_queue = None
# True if there are still more stats to fetch this cycle
work_to_do = True
# return value. True if new data differs from old
updated = False
# number of nodes we checked
num_nodes = 0
start_time = time.time()
while work_to_do:
# each time through the loop try to talk to either the master
# or a node. stop when we have talked to everybody.
# get a new node list from the master
if time.time() > (self.last_master_refresh + self.master_stale):
updated = self._master_refresh()
if self.last_master_refresh == 0:
# first time we contact the master, also refresh our full lookup tables
self._node_uri_refresh_all()
self.last_master_refresh = time.time()
# contact the nodes for stats
else:
# initialize update_queue based on most current nodes list
if update_queue is None:
update_queue = list(self.nn_nodes)
# no nodes left to contact
elif not update_queue:
work_to_do = False
# contact next node
else:
# figure out the next node to contact
next = update_queue.pop()
# rate limit talking to any particular node
if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale):
updated = self._node_refresh(next) or updated
# include in random offset (max 1/5th normal update interval)
# to help spread out updates
last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0)
num_nodes += 1
# small yield to keep from torquing the processor
time.sleep(0.01)
end_time = time.time()
#print "Update took %ss for %s nodes"%((end_time-start_time), num_nodes)
logger.debug("ROS stats update took %ss"%(end_time-start_time))
return updated

467
tools/rosgraph/src/rosgraph/masterapi.py
View File

@ -1,467 +0,0 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: masterapi.py 9672 2010-05-11 21:57:40Z kwc $
"""
Python adapter for calling ROS Master API. While it is trivial to call the
Master directly using XML-RPC, this API provides a safer abstraction in the event
the Master API is changed.
"""
import os
import sys
import xmlrpclib
import roslib.exceptions
import roslib.names
class ROSMasterException(roslib.exceptions.ROSLibException):
"""
Base class of ROS-master related errors.
"""
pass
class Failure(ROSMasterException):
"""
Call to Master failed. This generally indicates an internal error
in the Master and that the Master may be in an inconsistent state.
"""
pass
class Error(ROSMasterException):
"""
Master returned an error code, which indicates an error in the
arguments passed to the Master.
"""
pass
def is_online(master_uri=None):
"""
@param master_uri: (optional) override environment's ROS_MASTER_URI
@type master_uri: str
@return: True if Master is available
"""
return Master('roslib', master_uri=master_uri).is_online()
class Master(object):
"""
API for interacting with the ROS master. Although the Master is
relatively simple to interact with using the XMLRPC API, this
abstraction layer provides protection against future updates. It
also provides a streamlined API with builtin return code checking
and caller_id passing.
"""
def __init__(self, caller_id, master_uri=None):
"""
@param caller_id: name of node to use in calls to master
@type caller_id: str
@param master_uri: (optional) override environment's ROS_MASTER_URI
@type master_uri: str
"""
if master_uri is None:
master_uri = roslib.rosenv.get_master_uri()
self._reinit(master_uri)
self.caller_id = roslib.names.make_caller_id(caller_id) #resolve
if self.caller_id[-1] == '/':
self.caller_id = self.caller_id[:-1]
def _reinit(self, master_uri):
"""
Internal API for reinitializing this handle to be a new master
"""
self.master_uri = master_uri
self.handle = xmlrpclib.ServerProxy(self.master_uri)
def is_online(self):
"""
Check if Master is online.
NOTE: this is not part of the actual Master API. This is a convenience function.
@param master_uri: (optional) override environment's ROS_MASTER_URI
@type master_uri: str
@return: True if Master is available
"""
try:
self.getPid()
return True
except:
return False
def _succeed(self, args):
"""
Check master return code and return the value field.
@param args: master return value
@type args: (int, str, XMLRPCLegalValue)
@return: value field of args (master return value)
@rtype: XMLRPCLegalValue
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
code, msg, val = args
if code == 1:
return val
elif code == -1:
raise Error(msg)
else:
raise Failure(msg)
################################################################################
# PARAM SERVER
def deleteParam(self, key):
"""
Parameter Server: delete parameter
@param key: parameter name
@type key: str
@return: 0
@rtype: int
"""
return self._succeed(self.handle.deleteParam(self.caller_id, key))
def setParam(self, key, value):
"""
Parameter Server: set parameter. NOTE: if value is a
dictionary it will be treated as a parameter tree, where key
is the parameter namespace. For example:::
{'x':1,'y':2,'sub':{'z':3}}
will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it
will replace all existing parameters in the key parameter
namespace with the parameters in value. You must set
parameters individually if you wish to perform a union update.
@param key: parameter name
@type key: str
@param value: parameter value.
@type value: XMLRPCLegalValue
@return: 0
@rtype: int
"""
return self._succeed(self.handle.setParam(self.caller_id, key, value))
def getParam(self, key):
"""
Retrieve parameter value from server.
@param key: parameter to lookup. If key is a namespace,
getParam() will return a parameter tree.
@type key: str
getParam() will return a parameter tree.
@return: parameterValue. If key is a namespace,
the return value will be a dictionary, where each key is a
parameter in that namespace. Sub-namespaces are also
represented as dictionaries.
@rtype: XMLRPCLegalValue
"""
return self._succeed(self.handle.getParam(self.caller_id, key))
def searchParam(self, key):
"""
Search for parameter key on parameter server. Search starts in caller's namespace and proceeds
upwards through parent namespaces until Parameter Server finds a matching key.
searchParam's behavior is to search for the first partial match.
For example, imagine that there are two 'robot_description' parameters::
/robot_description
/robot_description/arm
/robot_description/base
/pr2/robot_description
/pr2/robot_description/base
If I start in the namespace /pr2/foo and search for
'robot_description', searchParam will match
/pr2/robot_description. If I search for 'robot_description/arm'
it will return /pr2/robot_description/arm, even though that
parameter does not exist (yet).
@param key: parameter key to search for.
@type key: str
@return: foundKey
@rtype: str
"""
return self._succeed(self.handle.searchParam(self.caller_id, key))
def subscribeParam(self, caller_api, key):
"""
Retrieve parameter value from server and subscribe to updates to that param. See
paramUpdate() in the Node API.
@param key: parameter to lookup.
@type key: str
@param caller_api: API URI for paramUpdate callbacks.
@type caller_api: str
@return: parameterValue. parameterValue is an empty dictionary if the parameter has not been set yet.
@rtype: XMLRPCLegalValue
"""
return self._succeed(self.handle.subscribeParam(self.caller_id, caller_api, key))
def unsubscribeParam(self, caller_api, key):
"""
Retrieve parameter value from server and subscribe to updates to that param. See
paramUpdate() in the Node API.
@param key: parameter to lookup.
@type key: str
@param caller_api: API URI for paramUpdate callbacks.
@type caller_api: str
@return: numUnsubscribed. If numUnsubscribed is zero it means that the caller was not subscribed to the parameter.
@rtype: int
"""
return self._succeed(self.handle.unsubscribeParam(self.caller_id, caller_api, key))
def hasParam(self, key):
"""
Check if parameter is stored on server.
@param key: parameter to check
@type key: str
@return: [code, statusMessage, hasParam]
@rtype: [int, str, bool]
"""
return self._succeed(self.handle.hasParam(self.caller_id, key))
def getParamNames(self):
"""
Get list of all parameter names stored on this server.
This does not adjust parameter names for caller's scope.
@return: [code, statusMessage, parameterNameList]
@rtype: [int, str, [str]]
"""
return self._succeed(self.handle.getParamNames(self.caller_id))
################################################################################
def getPid(self):
"""
Get the PID of this server
@return: serverProcessPID
@rtype: int
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.getPid(self.caller_id))
def getUri(self):
"""
Get the URI of this Master
@return: masterUri
@rtype: str
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.getUri(self.caller_id))
def registerService(self, service, service_api, caller_api):
"""
Register the caller as a provider of the specified service.
@param service str: Fully-qualified name of service
@param service_api str: Service URI
@param caller_api str: XML-RPC URI of caller node
@return: ignore
@rtype: int
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.registerService(self.caller_id, service, service_api, caller_api))
def lookupService(self, service):
"""
Lookup all provider of a particular service.
@param service: fully-qualified name of service to lookup.
@type: service: str
@return (int, str, str): (code, message, serviceUrl). service URL is provides
and address and port of the service. Fails if there is no provider.
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.lookupService(self.caller_id, service))
def unregisterService(self, service, service_api):
"""
Unregister the caller as a provider of the specified service.
@param service: Fully-qualified name of service
@type service: str
@param service_api: API URI of service to unregister. Unregistration will only occur if current
registration matches.
@type service_api: str
@return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
If this is zero it means that the caller was not registered as a service provider.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.unregisterService(self.caller_id, service, service_api))
def registerSubscriber(self, topic, topic_type, caller_api):
"""
Subscribe the caller to the specified topic. In addition to receiving
a list of current publishers, the subscriber will also receive notifications
of new publishers via the publisherUpdate API.
@param topic str: Fully-qualified name of topic to subscribe to.
@param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name.
@type topic_type: str
@param caller_api: XML-RPC URI of caller node for new publisher notifications
@type caller_api: str
@return: (code, message, publishers). Publishers is a list of XMLRPC API URIs
for nodes currently publishing the specified topic.
@rtype: (int, str, list(str))
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.registerSubscriber(self.caller_id, topic, topic_type, caller_api))
def unregisterSubscriber(self, topic, caller_api):
"""
Unregister the caller as a publisher of the topic.
@param topic: Fully-qualified name of topic to unregister.
@type topic: str
@param caller_api: API URI of service to unregister. Unregistration will only occur if current
@type caller_api: str
registration matches.
@return: (code, statusMessage, numUnsubscribed).
If numUnsubscribed is zero it means that the caller was not registered as a subscriber.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.unregisterSubscriber(self.caller_id, topic, caller_api))
def registerPublisher(self, topic, topic_type, caller_api):
"""
Register the caller as a publisher the topic.
@param topic: Fully-qualified name of topic to register.
@type topic: str
@param topic_type: Datatype for topic. Must be a
package-resource name, i.e. the .msg name.
@type topic_type: str
@param caller_api str: ROS caller XML-RPC API URI
@type caller_api: str
@return: subscriberApis.
List of current subscribers of topic in the form of XMLRPC URIs.
@rtype: [str]
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.registerPublisher(self.caller_id, topic, topic_type, caller_api))
def unregisterPublisher(self, topic, caller_api):
"""
Unregister the caller as a publisher of the topic.
@param topic: Fully-qualified name of topic to unregister.
@type topic: str
@param caller_api str: API URI of service to
unregister. Unregistration will only occur if current
registration matches.
@type caller_api: str
@return: numUnregistered.
If numUnregistered is zero it means that the caller was not registered as a publisher.
The call still succeeds as the intended final state is reached.
@rtype: int
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.unregisterPublisher(self.caller_id, topic, caller_api))
def lookupNode(self, node_name):
"""
Get the XML-RPC URI of the node with the associated
name/caller_id. This API is for looking information about
publishers and subscribers. Use lookupService instead to lookup
ROS-RPC URIs.
@param node: name of node to lookup
@type node: str
@return: URI
@rtype: str
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.lookupNode(self.caller_id, node_name))
def getPublishedTopics(self, subgraph):
"""
Get list of topics that can be subscribed to. This does not return topics that have no publishers.
See L{getSystemState()} to get more comprehensive list.
@param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace
is resolved relative to the caller's namespace. Use '' to specify all names.
@type subgraph: str
@return: [[topic1, type1]...[topicN, typeN]]
@rtype: [[str, str],]
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.getPublishedTopics(self.caller_id, subgraph))
def getTopicTypes(self):
"""
Retrieve list topic names and their types.
New in ROS 1.2.
@param caller_id: ROS caller id
@type caller_id: str
@rtype: (int, str, [[str,str]] )
@return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs.
"""
return self._succeed(self.handle.getTopicTypes(self.caller_id))
def getSystemState(self):
"""
Retrieve list representation of system state (i.e. publishers, subscribers, and services).
@rtype: [[str,[str]], [str,[str]], [str,[str]]]
@return: systemState
System state is in list representation::
[publishers, subscribers, services].
publishers is of the form::
[ [topic1, [topic1Publisher1...topic1PublisherN]] ... ]
subscribers is of the form::
[ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
services is of the form::
[ [service1, [service1Provider1...service1ProviderN]] ... ]
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.getSystemState(self.caller_id))

99
tools/rosgraph/src/rosgraph/rosgraph_main.py
View File

@ -1,99 +0,0 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$
import sys
import time
import roslib.roslogging
import roslib.scriptutil
import rosgraph.impl.graph
import rosgraph.masterapi
def fullusage():
print """rosgraph is a command-line tool for debugging the ROS Computation Graph.
Usage:
\trosgraph
"""
def rosgraph_main():
if len(sys.argv) == 1:
pass
elif len(sys.argv) == 2 and sys.argv[1] == '-h':
fullusage()
return
else:
fullusage()
sys.exit(-1)
roslib.roslogging.configure_logging('rosgraph')
# make sure master is available
master = rosgraph.masterapi.Master('rosgraph')
try:
master.getPid()
except:
print >> sys.stderr, "ERROR: Unable to communicate with master!"
return
g = rosgraph.impl.graph.Graph()
try:
while 1:
g.update()
if not g.nn_nodes and not g.srvs:
print "empty"
else:
print '\n'
if g.nn_nodes:
print 'Nodes:'
for n in g.nn_nodes:
print ' ' + n + ' :'
print ' Inbound:'
if n in g.nn_edges.edges_by_end:
for c in g.nt_all_edges.edges_by_end[n]:
print ' ' + c.start
print ' Outbound:'
if n in g.nn_edges.edges_by_start:
for c in g.nt_all_edges.edges_by_start[n]:
print ' ' + c.end
if g.srvs:
print 'Services:'
for s in g.srvs:
print ' ' + s
time.sleep(1.0)
except KeyboardInterrupt:
pass