From b9e233ca1923ed74e0767793a3412a52ed3b030d Mon Sep 17 00:00:00 2001 From: Stephen Date: Mon, 7 Jan 2019 12:55:59 -0800 Subject: [PATCH] Porting to ROS2 --- CMakeLists.txt | 22 --- package.xml | 24 +--- resource/rqt_graph | 0 scripts/rqt_graph | 2 +- setup.py | 42 ++++-- src/rqt_graph/dotcode.py | 60 +++----- src/rqt_graph/ros_graph.py | 56 ++++---- src/rqt_graph/rosgraph2_impl.py | 239 +++++++++++--------------------- test/dotcode_test.py | 33 +++-- 9 files changed, 194 insertions(+), 284 deletions(-) delete mode 100644 CMakeLists.txt create mode 100644 resource/rqt_graph diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index fefb859..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_graph) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - catkin_add_nosetests(test/dotcode_test.py) -endif() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_graph - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/package.xml b/package.xml index 8f6289b..00e9350 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,4 @@ - + rqt_graph 0.4.10 rqt_graph provides a GUI plugin for visualizing the ROS @@ -19,24 +19,14 @@ Dirk Thomas - catkin - - python_qt_binding - python-rospkg - qt_dotgraph - rosgraph - rosgraph_msgs - roslib - rosnode - rospy - rosservice - rostopic - rqt_gui - rqt_gui_py + ament_index_python + python_qt_binding + qt_dotgraph + rqt_gui + rqt_gui_py - + ament_python - diff --git a/resource/rqt_graph b/resource/rqt_graph new file mode 100644 index 0000000..e69de29 diff --git a/scripts/rqt_graph b/scripts/rqt_graph index cfb71a9..c91f606 100755 --- a/scripts/rqt_graph +++ b/scripts/rqt_graph @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import sys diff --git a/setup.py b/setup.py index de78f24..bc9a4ea 100644 --- a/setup.py +++ b/setup.py @@ -1,12 +1,36 @@ -#!/usr/bin/env python +from setuptools import setup -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_graph'], +package_name = 'rqt_graph' +setup( + name=package_name, + version='0.4.10', package_dir={'': 'src'}, - scripts=['scripts/rqt_graph'] + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/resource', ['resource/RosGraph.ui']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['plugin.xml']), + ('lib/' + package_name, ['scripts/rqt_graph']) + ], + install_requires=['setuptools'], + zip_safe=True, + author='Dirk Thomas', + maintainer='Dirk Thomas, Aaron Blasdell', + maintainer_email='dthomas@osrfoundation.org', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description=( + 'rqt_topic provides a GUI plugin for displaying debug information about ROS topics ' + 'including publishers, subscribers, publishing rate, and ROS Messages.' + ), + license='BSD', + tests_require=['pytest'], + scripts=['scripts/rqt_graph'], ) - -setup(**d) diff --git a/src/rqt_graph/dotcode.py b/src/rqt_graph/dotcode.py index 9eb21e0..86187b0 100644 --- a/src/rqt_graph/dotcode.py +++ b/src/rqt_graph/dotcode.py @@ -35,11 +35,9 @@ import re import copy -import rosgraph.impl.graph -import roslib +from rqt_graph import rosgraph2_impl import math -import rospy import pydot try: @@ -96,25 +94,8 @@ class RosGraphDotcodeGenerator: # ROS node name -> graph.node object nodes = dict([]) - def __init__(self): - try: - from rosgraph_msgs.msg import TopicStatistics - self.stats_sub = rospy.Subscriber( - '/statistics', TopicStatistics, self.statistics_callback) - except ImportError: - # not supported before Indigo - pass - - def statistics_callback(self, msg): - - # add connections (if new) - if msg.node_sub not in self.edges: - self.edges[msg.node_sub] = dict([]) - - if msg.topic not in self.edges[msg.node_sub]: - self.edges[msg.node_sub][msg.topic] = dict([]) - - self.edges[msg.node_sub][msg.topic][msg.node_pub] = msg + def __init__(self, node): + self._node = node def _get_max_traffic(self): traffic = 10000 # start at 10kb @@ -254,7 +235,7 @@ def _add_node(self, node, rosgraphinst, dotcode_factory, dotgraph, unreachable): if unreachable: return '' bn = rosgraphinst.bad_nodes[node] - if bn.type == rosgraph.impl.graph.BadNode.DEAD: + if bn.type == rosgraph2_impl.BadNode.DEAD: dotcode_factory.add_node_to_graph( dotgraph, nodename=_conv(node), @@ -262,7 +243,7 @@ def _add_node(self, node, rosgraphinst, dotcode_factory, dotgraph, unreachable): shape="ellipse", url=node + " (DEAD)", color="red") - elif bn.type == rosgraph.impl.graph.BadNode.WONKY: + elif bn.type == rosgraph2_impl.BadNode.WONKY: dotcode_factory.add_node_to_graph( dotgraph, nodename=_conv(node), @@ -287,7 +268,7 @@ def _add_node(self, node, rosgraphinst, dotcode_factory, dotgraph, unreachable): url=node) def _add_topic_node(self, node, dotcode_factory, dotgraph, quiet): - label = rosgraph.impl.graph.node_topic(node) + label = rosgraph2_impl.node_topic(node) dotcode_factory.add_node_to_graph( dotgraph, nodename=_conv(node), @@ -296,7 +277,7 @@ def _add_topic_node(self, node, dotcode_factory, dotgraph, quiet): url="topic:%s" % label) def _add_topic_node_group(self, node, dotcode_factory, dotgraph, quiet): - label = rosgraph.impl.graph.node_topic(node) + label = rosgraph2_impl.node_topic(node) dotcode_factory.add_node_to_graph( dotgraph, nodename=_conv(node), @@ -322,11 +303,12 @@ def generate_namespaces(self, graph, graph_mode, quiet=False): Determine the namespaces of the nodes being displayed """ namespaces = [] + nodes_and_namespaces = dict(self._node.get_node_names_and_namespace()) if graph_mode == NODE_NODE_GRAPH: nodes = graph.nn_nodes if quiet: nodes = [n for n in nodes if n not in QUIET_NAMES] - namespaces = list(set([roslib.names.namespace(n) for n in nodes])) + namespaces = list(set([nodes_and_namespaces[n] for n in nodes])) elif graph_mode == NODE_TOPIC_GRAPH or \ graph_mode == NODE_TOPIC_ALL_GRAPH: @@ -336,11 +318,11 @@ def generate_namespaces(self, graph, graph_mode, quiet=False): nn_nodes = [n for n in nn_nodes if n not in QUIET_NAMES] nt_nodes = [n for n in nt_nodes if n not in QUIET_NAMES] if nn_nodes or nt_nodes: - namespaces = [roslib.names.namespace(n) for n in nn_nodes] + namespaces = [nodes_and_namespaces[n] for n in nn_nodes] # an annoyance with the rosgraph library is that it # prepends a space to topic names as they have to have # different graph node namees from nodes. we have to strip here - namespaces.extend([roslib.names.namespace(n[1:]) for n in nt_nodes]) + namespaces.extend([nodes_and_namespaces[n[1:]] for n in nt_nodes]) return list(set(namespaces)) @@ -686,6 +668,7 @@ def generate_dotgraph( if quiet: nn_nodes = list(filter(self._quiet_filter, nn_nodes)) nt_nodes = list(filter(self._quiet_filter, nt_nodes)) + if graph_mode == NODE_NODE_GRAPH: edges = list(filter(self.quiet_filter_topic_edge, edges)) @@ -743,8 +726,8 @@ def generate_dotgraph( IMAGE_TOPICS_SUFFIX = '/image_topics' node_list = (nt_nodes or []) + \ - [action_prefix + ACTION_TOPICS_SUFFIX for (action_prefix, _) in action_nodes.items()] + \ - [image_prefix + IMAGE_TOPICS_SUFFIX for (image_prefix, _) in image_nodes.items()] + \ + [act_prefix + ACTION_TOPICS_SUFFIX for act_prefix, _ in action_nodes.items()] + \ + [img_prefix + IMAGE_TOPICS_SUFFIX for img_prefix, _ in image_nodes.items()] + \ nn_nodes if nn_nodes is not None else [] namespace_clusters = self._populate_node_graph( @@ -766,13 +749,14 @@ def generate_dotgraph( else: namespace = '/'.join(n.strip().split('/')[:cluster_namespaces_level + 1]) self._add_topic_node( - n, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], quiet=quiet) + n, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], + quiet=quiet) else: self._add_topic_node( n, dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet) - for n in [action_prefix + ACTION_TOPICS_SUFFIX for (action_prefix, _) in action_nodes.items()] + \ - [image_prefix + IMAGE_TOPICS_SUFFIX for (image_prefix, _) in image_nodes.items()]: + for n in [act_prefix + ACTION_TOPICS_SUFFIX for act_prefix, _ in action_nodes.items()] + \ + [img_prefix + IMAGE_TOPICS_SUFFIX for img_prefix, _ in image_nodes.items()]: # cluster topics with same namespace if cluster_namespaces_level > 0 and \ unicode(n).strip().count('/') > 1 and \ @@ -782,12 +766,13 @@ def generate_dotgraph( else: namespace = '/'.join(n.strip().split('/')[:cluster_namespaces_level + 1]) self._add_topic_node_group( - 'n' + n, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], quiet=quiet) + 'n' + n, dotcode_factory=dotcode_factory, + dotgraph=namespace_clusters[namespace], quiet=quiet) else: self._add_topic_node_group( 'n' + n, dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet) - if tf_connections != None: + if tf_connections is not None: # render tf nodes as a single node self._add_topic_node_group( 'n/tf', dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet) @@ -886,7 +871,8 @@ def generate_dotcode( @param hide_dead_end_topics: if true remove topics with publishers only @param cluster_namespaces_level: if > 0 places box around members of same namespace (TODO: multiple namespace layers) - @param accumulate_actions: if true each 5 action topic graph nodes are shown as single graph node + @param accumulate_actions: if true each 5 action topic graph nodes are shown as single + graph node @return: dotcode generated from graph singleton @rtype: str """ diff --git a/src/rqt_graph/ros_graph.py b/src/rqt_graph/ros_graph.py index 52a1ba2..26bb0e2 100644 --- a/src/rqt_graph/ros_graph.py +++ b/src/rqt_graph/ros_graph.py @@ -30,17 +30,15 @@ from __future__ import division import os -import rospkg +from ament_index_python import get_resource from python_qt_binding import loadUi from python_qt_binding.QtCore import QAbstractListModel, QFile, QIODevice, Qt, Signal from python_qt_binding.QtGui import QIcon, QImage, QPainter from python_qt_binding.QtWidgets import QCompleter, QFileDialog, QGraphicsScene, QWidget from python_qt_binding.QtSvg import QSvgGenerator -import rosgraph.impl.graph -import rosservice -import rostopic +from rqt_graph.rosgraph2_impl import Graph from qt_dotgraph.dot_to_qt import DotToQtGenerator # pydot requires some hacks @@ -49,7 +47,8 @@ # TODO: use pygraphviz instead, but non-deterministic layout will first be resolved in graphviz 2.30 # from qtgui_plugin.pygraphvizfactory import PygraphvizFactory -from .dotcode import RosGraphDotcodeGenerator, NODE_NODE_GRAPH, NODE_TOPIC_ALL_GRAPH, NODE_TOPIC_GRAPH +from .dotcode import \ + RosGraphDotcodeGenerator, NODE_NODE_GRAPH, NODE_TOPIC_ALL_GRAPH, NODE_TOPIC_GRAPH from .interactive_graphics_view import InteractiveGraphicsView try: @@ -109,6 +108,8 @@ class RosGraph(Plugin): def __init__(self, context): super(RosGraph, self).__init__(context) + self._node = context.node + self._logger = self._node.get_logger().get_child('rqt_graph.ros_graph.RosGraph') self.initialized = False self.setObjectName('RosGraph') @@ -121,12 +122,12 @@ def __init__(self, context): self.dotcode_factory = PydotFactory() # self.dotcode_factory = PygraphvizFactory() # generator builds rosgraph - self.dotcode_generator = RosGraphDotcodeGenerator() + self.dotcode_generator = RosGraphDotcodeGenerator(self._node) # dot_to_qt transforms into Qt elements using dot layout self.dot_to_qt = DotToQtGenerator() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_graph'), 'resource', 'RosGraph.ui') + _, package_path = get_resource('packages', 'rqt_graph') + ui_file = os.path.join(package_path, 'share', 'rqt_graph', 'resource', 'RosGraph.ui') loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) self._widget.setObjectName('RosGraphUi') if context.serial_number() > 1: @@ -216,7 +217,8 @@ def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value( 'auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked()) instance_settings.set_value( - 'highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked()) + 'highlight_connections_check_box_state', + self._widget.highlight_connections_check_box.isChecked()) instance_settings.set_value( 'group_tf_check_box_state', self._widget.group_tf_check_box.isChecked()) instance_settings.set_value( @@ -224,7 +226,8 @@ def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value( 'group_image_check_box_state', self._widget.group_image_check_box.isChecked()) instance_settings.set_value( - 'hide_dynamic_reconfigure_check_box_state', self._widget.hide_dynamic_reconfigure_check_box.isChecked()) + 'hide_dynamic_reconfigure_check_box_state', + self._widget.hide_dynamic_reconfigure_check_box.isChecked()) def restore_settings(self, plugin_settings, instance_settings): self._widget.graph_type_combo_box.setCurrentIndex( @@ -247,7 +250,8 @@ def restore_settings(self, plugin_settings, instance_settings): self._widget.auto_fit_graph_check_box.setChecked( instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true']) self._widget.highlight_connections_check_box.setChecked( - instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true']) + instance_settings.value('highlight_connections_check_box_state', True) in + [True, 'true']) self._widget.hide_tf_nodes_check_box.setChecked( instance_settings.value('hide_tf_nodes_check_box_state', False) in [True, 'true']) self._widget.group_tf_check_box.setChecked( @@ -255,7 +259,8 @@ def restore_settings(self, plugin_settings, instance_settings): self._widget.group_image_check_box.setChecked( instance_settings.value('group_image_check_box_state', True) in [True, 'true']) self._widget.hide_dynamic_reconfigure_check_box.setChecked( - instance_settings.value('hide_dynamic_reconfigure_check_box_state', True) in [True, 'true']) + instance_settings.value('hide_dynamic_reconfigure_check_box_state', True) in + [True, 'true']) self.initialized = True self._refresh_rosgraph() @@ -275,8 +280,7 @@ def _update_rosgraph(self): self._widget.group_image_check_box.setEnabled(True) self._widget.hide_dynamic_reconfigure_check_box.setEnabled(True) - self._graph = rosgraph.impl.graph.Graph() - self._graph.set_master_stale(5.0) + self._graph = Graph(self._node) self._graph.set_node_stale(5.0) self._graph.update() self.node_completionmodel.refresh(self._graph.nn_nodes) @@ -334,19 +338,17 @@ def _generate_tool_tip(self, url): item_type, item_path = url.split(':', 1) if item_type == 'node': tool_tip = 'Node:\n %s' % (item_path) - service_names = rosservice.get_service_list(node=item_path) - if service_names: + service_names_and_types = self._node.get_service_names_and_types() + if service_names_and_types: tool_tip += '\nServices:' - for service_name in service_names: - try: - service_type = rosservice.get_service_type(service_name) - tool_tip += '\n %s [%s]' % (service_name, service_type) - except rosservice.ROSServiceIOException as e: - tool_tip += '\n %s' % (e) + for service_name, service_type in service_names_and_types: + tool_tip += '\n %s [%s]' % (service_name, service_type) return tool_tip elif item_type == 'topic': - topic_type, topic_name, _ = rostopic.get_topic_type(item_path) - return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type) + for topic_name, topic_type in self._node.get_topic_names_and_types(): + if topic_name in item_path: + return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type) + return 'No topic with item path {}'.format(item_path) return url def _redraw_graph_view(self): @@ -416,7 +418,8 @@ def _save_dot(self): def _save_svg(self): file_name, _ = QFileDialog.getSaveFileName( - self._widget, self.tr('Save as SVG'), 'rosgraph.svg', self.tr('Scalable Vector Graphic (*.svg)')) + self._widget, self.tr('Save as SVG'), 'rosgraph.svg', + self.tr('Scalable Vector Graphic (*.svg)')) if file_name is None or file_name == '': return @@ -431,7 +434,8 @@ def _save_svg(self): def _save_image(self): file_name, _ = QFileDialog.getSaveFileName( - self._widget, self.tr('Save as image'), 'rosgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)')) + self._widget, self.tr('Save as image'), 'rosgraph.png', + self.tr('Image (*.bmp *.jpg *.png *.tiff)')) if file_name is None or file_name == '': return diff --git a/src/rqt_graph/rosgraph2_impl.py b/src/rqt_graph/rosgraph2_impl.py index 60f1ab6..9c03665 100644 --- a/src/rqt_graph/rosgraph2_impl.py +++ b/src/rqt_graph/rosgraph2_impl.py @@ -30,9 +30,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # -# Revision $Id$ - -from __future__ import print_function +# Brawner: Adopted from ros_comm for ROS2 """ Data structures and library for representing ROS Computation Graph state. @@ -44,18 +42,17 @@ import random import logging import traceback -try: - from xmlrpc.client import ServerProxy -except ImportError: - from xmlrpclib import ServerProxy import socket -import rosgraph.masterapi +from collections import defaultdict -logger = logging.getLogger('rosgraph.graph') +from python_qt_binding.QtCore import qDebug, qWarning +from rclpy import logging +logger = logging.get_logger('rqt_graph.rosgraph2_impl.graph') _ROS_NAME = '/rosviz' + def topic_node(topic): """ In order to prevent topic/node name aliasing, we have to remap @@ -64,6 +61,8 @@ def topic_node(topic): @return str: topic mapped to a graph node name. """ return ' ' + topic + + def node_topic(node): """ Inverse of topic_node @@ -71,14 +70,15 @@ def node_topic(node): """ return node[1:] + class BadNode(object): """ Data structure for storing info about a 'bad' node """ - ## no connectivity + # no connectivity DEAD = 0 - ## intermittent connectivity + # intermittent connectivity WONKY = 1 def __init__(self, name, type, reason): @@ -86,15 +86,17 @@ def __init__(self, name, type, reason): @param type: DEAD | WONKY @type type: int """ - self.name =name + 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 = {} @@ -112,8 +114,7 @@ def __contains__(self, edge): @rtype: bool """ key = edge.key - return key in self.edges_by_start and \ - edge in self.edges_by_start[key] + return key in self.edges_by_start and edge in self.edges_by_start[key] def add(self, edge): """ @@ -124,9 +125,9 @@ def add(self, 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) + edges = map[key] + if edge not in edges: + edges.append(edge) return True else: return False @@ -159,9 +160,9 @@ def add_edges(self, start, dest, direction, label=''): # doing an update updated = False if not start: - logger.warn("cannot add edge: cannot map start [%s] to a node name", start) + qWarning("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) + qWarning("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 @@ -193,27 +194,32 @@ def update_map(map, key, edge): 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) + self.key = "%s|%s" % (self.start, self.label) # reverse key, indexed from end - self.rkey = "%s|%s"%(self.end, self.label) + 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) + 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 @@ -235,9 +241,8 @@ class Graph(object): Not multi-thread-safe """ - def __init__(self, node_ns='/', topic_ns='/'): - self.master = rosgraph.masterapi.Master(_ROS_NAME) - + def __init__(self, node, node_ns='/', topic_ns='/'): + self._node = node self.node_ns = node_ns or '/' self.topic_ns = topic_ns or '/' @@ -261,28 +266,18 @@ def __init__(self, node_ns='/', topic_ns='/'): self.nt_all_edges = EdgeList() # node names to URI map - self.node_uri_map = {} # { node_name_str : uri_str } + self.node_uri_map = {} # { node_name_str : uri_str } # reverse map URIs to node names - self.uri_node_map = {} # { uri_str : node_name_str } + self.uri_node_map = {} # { uri_str : node_name_str } - # time we last contacted master - self.last_master_refresh = 0 + # time we last updated the graph + self.last_graph_refresh = 0 self.last_node_refresh = {} - # time we last communicated with master - # seconds until master data is considered stale - self.master_stale = 5.0 + self.graph_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 + self.node_stale = 5.0 # seconds def set_node_stale(self, stale_secs): """ @@ -291,24 +286,37 @@ def set_node_stale(self, stale_secs): """ self.node_stale = stale_secs - def _master_refresh(self): + def _graph_refresh(self): """ @return: True if nodes information was updated @rtype: bool """ - logger.debug("master refresh: starting") updated = False - try: - val = self.master.getSystemState() - except rosgraph.masterapi.MasterException as e: - print("Unable to contact master", str(e), file=sys.stderr) - logger.error("unable to contact master: %s", str(e)) - return False + publishers = defaultdict(list) + subscriptions = defaultdict(list) + servers = defaultdict(list) + + for name, namespace in self._node.get_node_names_and_namespaces(): + node_name = namespace + name + + for topic_name, topic_type in \ + self._node.get_publisher_names_and_types_by_node(name, namespace): + publishers[topic_name].append(node_name) - pubs, subs, srvs = val + for topic_name, topic_type in \ + self._node.get_subscriber_names_and_types_by_node(name, namespace): + subscriptions[topic_name].append(node_name) + + for service_name, service_type in \ + self._node.get_service_names_and_types_by_node(name, namespace): + servers[service_name].append(node_name) + + pubs = list(publishers.items()) + subs = list(subscriptions.items()) + srvs = list(servers.items()) nodes = [] - nt_all_edges = self.nt_all_edges + nt_all_edges = EdgeList() nt_nodes = self.nt_nodes for state, direction in ((pubs, 'o'), (subs, 'i')): for topic, l in state: @@ -318,7 +326,10 @@ def _master_refresh(self): for node in l: updated = nt_all_edges.add_edges( node, topic_node(topic), direction) or updated - + self.nt_nodes = nt_nodes + self.nt_all_edges = nt_all_edges + self.nt_edges = nt_all_edges + self.nn_edges = nt_all_edges nodes = set(nodes) srvs = set([s for s, _ in srvs]) @@ -332,14 +343,13 @@ def _master_refresh(self): updated = True if purge: - logger.debug("following nodes and related edges will be purged: %s", ','.join(purge)) + qDebug("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) + qDebug("Graph refresh: done, updated[%s]" % updated) return updated def _mark_bad_node(self, node, reason): @@ -365,7 +375,7 @@ def _unmark_bad_node(self, node, reason): finally: self.bad_nodes_lock.release() - def _node_refresh_businfo(self, node, api, bad_node=False): + def _node_refresh_businfo(self, node, bad_node=False): """ Retrieve bus info from the node and update nodes and edges as appropriate @param node: node name @@ -376,68 +386,9 @@ def _node_refresh_businfo(self, node, api, bad_node=False): 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 as 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: - # #3579 bad node, ignore - if len(info) < 5: - continue - - 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 + if bad_node: + self._mark_bad_node(node, 'Not found during discovery') + return True def _node_refresh(self, node, bad_node=False): """ @@ -453,34 +404,18 @@ def _node_refresh(self, node, bad_node=False): # getSystemState() instead to prevent the extra connection per node updated = False uri = self._node_uri_refresh(node) - try: - if uri: - api = ServerProxy(uri) - updated = self._node_refresh_businfo(node, api, bad_node) - except KeyError as e: - logger.warn('cannot contact node [%s] as it is not in the lookup table'%node) + + bad_node = (uri is None) + updated = self._node_refresh_businfo(node, bad_node) return updated def _node_uri_refresh(self, node): - try: - uri = self.master.lookupNode(node) - except: - msg = traceback.format_exc() - logger.warn("master reported error in node lookup: %s"%msg) + current_nodes = \ + {namespace + name for name, namespace in self._node.get_node_names_and_namespaces()} + if node not in current_nodes: + qWarning('Node "{}" does not exist'.format(node)) return None - # 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) + return node def bad_update(self): """ @@ -518,8 +453,7 @@ def bad_update(self): # 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)) + qDebug("ROS stats (bad nodes) update took %ss" % (end_time - start_time)) return updated def update(self): @@ -546,13 +480,9 @@ def update(self): # 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() + if time.time() > (self.last_graph_refresh + self.graph_stale): + updated = self._graph_refresh() + self.last_graph_refresh = time.time() # contact the nodes for stats else: # initialize update_queue based on most current nodes list @@ -570,13 +500,12 @@ def update(self): 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) + 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)) + qDebug("ROS stats update took %ss" % (end_time - start_time)) return updated - diff --git a/test/dotcode_test.py b/test/dotcode_test.py index cf10069..ab3e2b3 100644 --- a/test/dotcode_test.py +++ b/test/dotcode_test.py @@ -32,7 +32,6 @@ # POSSIBILITY OF SUCH DAMAGE. import unittest -import rospkg import os from rqt_graph.dotcode import RosGraphDotcodeGenerator @@ -40,10 +39,14 @@ PKG = 'rqt_graph' +class MockRclpyNode: + pass + + class DotcodeTest(unittest.TestCase): def test_split_filter_empty(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) inc, exc = gen._split_filter_string('') self.assertEqual(['.*'], inc) self.assertEqual(0, len(exc)) @@ -52,7 +55,7 @@ def test_split_filter_empty(self): self.assertEqual(0, len(exc)) def test_split_filter_includes(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) inc, exc = gen._split_filter_string('foo') self.assertEqual(['foo'], inc) self.assertEqual(0, len(exc)) @@ -61,7 +64,7 @@ def test_split_filter_includes(self): self.assertEqual(0, len(exc)) def test_split_filter_excludes(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) inc, exc = gen._split_filter_string('-foo') self.assertEqual(['.*'], inc) self.assertEqual(['foo'], exc) @@ -70,7 +73,7 @@ def test_split_filter_excludes(self): self.assertEqual(['foo', 'bar'], exc) def test_split_filter_both(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) inc, exc = gen._split_filter_string('-foo , bar ,baz, -bam') self.assertEqual(['bar', 'baz'], inc) self.assertEqual(['foo', 'bam'], exc) @@ -82,7 +85,7 @@ def __init__(self, start, end): self.end = end def test_get_node_edge_map(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) e1 = self.MockEdge('foo', 'bar') nmap = gen._get_node_edge_map([e1]) self.assertEqual(2, len(nmap)) @@ -94,7 +97,7 @@ def test_get_node_edge_map(self): self.assertEqual([], nmap['bar'].outgoing) def test_get_node_edge_map2(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) e1 = self.MockEdge('foo', 'bar') e2 = self.MockEdge('bar', 'foo') e3 = self.MockEdge('foo', 'pam') @@ -109,7 +112,7 @@ def test_get_node_edge_map2(self): self.assertEqual([e2], nmap['bar'].outgoing) def test_filter_leaf_topics_single_connection(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) topic_nodes = ['foo', 'bar', 'pam', 'boo'] e1 = self.MockEdge('n1', 'foo') e2 = self.MockEdge('n2', 'foo') @@ -134,7 +137,7 @@ def test_filter_leaf_topics_single_connection(self): self.assertEqual([e1, e2, e3, e4, e5, e6], edges) def test_filter_leaf_topics_dead_end(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) topic_nodes = ['foo', 'bar', 'pam', 'boo'] e1 = self.MockEdge('n1', 'foo') e2 = self.MockEdge('n2', 'foo') @@ -159,7 +162,7 @@ def test_filter_leaf_topics_dead_end(self): self.assertEqual([e1, e2, e3, e4, e5, e6], edges) def test_filter_leaf_topics_both(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) topic_nodes = ['foo', 'bar', 'pam', 'boo'] e1 = self.MockEdge('n1', 'foo') e2 = self.MockEdge('n2', 'foo') @@ -184,7 +187,7 @@ def test_filter_leaf_topics_both(self): self.assertEqual([e1, e2, e3, e4, e5, e6], edges) def test_accumulate_action_topics(self): - gen = RosGraphDotcodeGenerator() + gen = RosGraphDotcodeGenerator(MockRclpyNode()) topic_nodes = ['foo/feedback', 'foo/goal', 'foo/cancel', 'foo/result', 'foo/status', 'bar'] e1 = self.MockEdge('n1', 'foo/feedback') e2 = self.MockEdge('n1', 'foo/goal') @@ -200,10 +203,6 @@ def test_accumulate_action_topics(self): self.assertEqual(1, len(redges)) self.assertEqual(1, len(raction_nodes)) self.assertEqual( - ['foo/feedback', 'foo/goal', 'foo/cancel', 'foo/result', 'foo/status', 'bar'], topic_nodes) + ['foo/feedback', 'foo/goal', 'foo/cancel', 'foo/result', 'foo/status', 'bar'], + topic_nodes) self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - -if __name__ == '__main__': - import rosunit - rosunit.unitrun(PKG, 'dotcode_test', DotcodeTest)