Skip to content

Commit

Permalink
Porting to ROS2
Browse files Browse the repository at this point in the history
  • Loading branch information
brawner committed Jan 8, 2019
1 parent 7945bb8 commit cdc466a
Show file tree
Hide file tree
Showing 9 changed files with 199 additions and 289 deletions.
22 changes: 0 additions & 22 deletions CMakeLists.txt

This file was deleted.

24 changes: 7 additions & 17 deletions package.xml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<package>
<package format="2">
<name>rqt_graph</name>
<version>0.4.10</version>
<description>rqt_graph provides a GUI plugin for visualizing the ROS
Expand All @@ -19,24 +19,14 @@

<author>Dirk Thomas</author>

<buildtool_depend>catkin</buildtool_depend>

<run_depend version_gte="0.2.19">python_qt_binding</run_depend>
<run_depend>python-rospkg</run_depend>
<run_depend>qt_dotgraph</run_depend>
<run_depend>rosgraph</run_depend>
<run_depend>rosgraph_msgs</run_depend>
<run_depend>roslib</run_depend>
<run_depend>rosnode</run_depend>
<run_depend>rospy</run_depend>
<run_depend>rosservice</run_depend>
<run_depend>rostopic</run_depend>
<run_depend>rqt_gui</run_depend>
<run_depend>rqt_gui_py</run_depend>
<exec_depend>ament_index_python</exec_depend>
<exec_depend version_gte="0.2.19">python_qt_binding</exec_depend>
<exec_depend>qt_dotgraph</exec_depend>
<exec_depend>rqt_gui</exec_depend>
<exec_depend>rqt_gui_py</exec_depend>

<export>
<architecture_independent/>
<build_type>ament_python</build_type>
<rqt_gui plugin="${prefix}/plugin.xml"/>
</export>
</package>

Empty file added resource/rqt_graph
Empty file.
2 changes: 1 addition & 1 deletion scripts/rqt_graph
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python3

import sys

Expand Down
42 changes: 33 additions & 9 deletions setup.py
Original file line number Diff line number Diff line change
@@ -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)
60 changes: 23 additions & 37 deletions src/rqt_graph/dotcode.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -254,15 +235,15 @@ 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),
nodelabel=node,
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),
Expand All @@ -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),
Expand All @@ -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),
Expand All @@ -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:
Expand All @@ -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))

Expand Down Expand Up @@ -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))

Expand Down Expand Up @@ -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(
Expand All @@ -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 \
Expand All @@ -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)
Expand Down Expand Up @@ -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
"""
Expand Down
Loading

0 comments on commit cdc466a

Please sign in to comment.