00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 import re
00036 import copy
00037
00038 import rosgraph.impl.graph
00039 import roslib
00040
00041
00042 NODE_NODE_GRAPH = 'node_node'
00043
00044 NODE_TOPIC_GRAPH = 'node_topic'
00045
00046 NODE_TOPIC_ALL_GRAPH = 'node_topic_all'
00047
00048 QUIET_NAMES = ['/diag_agg', '/runtime_logger', '/pr2_dashboard', '/rviz', '/rosout', '/cpu_monitor', '/monitor', '/hd_monitor', '/rxloggerlevel', '/clock', '/rqt']
00049
00050
00051 def matches_any(name, patternlist):
00052 if patternlist is None or len(patternlist) == 0:
00053 return False
00054 for pattern in patternlist:
00055 if str(name).strip() == pattern:
00056 return True
00057 if re.match("^[a-zA-Z0-9_/]+$", pattern) is None:
00058 if re.match(str(pattern), name.strip()) is not None:
00059 return True
00060 return False
00061
00062
00063 class NodeConnections:
00064 def __init__(self, incoming=None, outgoing=None):
00065 self.incoming = incoming or []
00066 self.outgoing = outgoing or []
00067
00068
00069 class RosGraphDotcodeGenerator:
00070
00071 def __init__(self):
00072 pass
00073
00074 def _add_edge(self, edge, dotcode_factory, dotgraph, is_topic=False):
00075 if is_topic:
00076 dotcode_factory.add_edge_to_graph(dotgraph, edge.start, edge.end, label=edge.label, url='topic:%s' % edge.label)
00077 else:
00078 dotcode_factory.add_edge_to_graph(dotgraph, edge.start, edge.end, label=edge.label)
00079
00080 def _add_node(self, node, rosgraphinst, dotcode_factory, dotgraph, quiet):
00081 if node in rosgraphinst.bad_nodes:
00082 if quiet:
00083 return ''
00084 bn = rosgraphinst.bad_nodes[node]
00085 if bn.type == rosgraph.impl.graph.BadNode.DEAD:
00086 dotcode_factory.add_node_to_graph(dotgraph,
00087 nodename=node,
00088 shape="doublecircle",
00089 url=node,
00090 color="red")
00091 else:
00092 dotcode_factory.add_node_to_graph(dotgraph,
00093 nodename=node,
00094 shape="doublecircle",
00095 url=node,
00096 color="orange")
00097 else:
00098 dotcode_factory.add_node_to_graph(dotgraph,
00099 nodename=node,
00100 shape='ellipse',
00101 url=node)
00102
00103 def _add_topic_node(self, node, dotcode_factory, dotgraph, quiet):
00104 label = rosgraph.impl.graph.node_topic(node)
00105 dotcode_factory.add_node_to_graph(dotgraph,
00106 nodename=label,
00107 nodelabel=label,
00108 shape='box',
00109 url="topic:%s" % label)
00110
00111 def _quiet_filter(self, name):
00112
00113 for n in QUIET_NAMES:
00114 if n in name:
00115 return False
00116 return True
00117
00118 def quiet_filter_topic_edge(self, edge):
00119 for quiet_label in ['/time', '/clock', '/rosout']:
00120 if quiet_label == edge.label:
00121 return False
00122 return self._quiet_filter(edge.start) and self._quiet_filter(edge.end)
00123
00124 def generate_namespaces(self, graph, graph_mode, quiet=False):
00125 """
00126 Determine the namespaces of the nodes being displayed
00127 """
00128 namespaces = []
00129 if graph_mode == NODE_NODE_GRAPH:
00130 nodes = graph.nn_nodes
00131 if quiet:
00132 nodes = [n for n in nodes if not n in QUIET_NAMES]
00133 namespaces = list(set([roslib.names.namespace(n) for n in nodes]))
00134
00135 elif graph_mode == NODE_TOPIC_GRAPH or \
00136 graph_mode == NODE_TOPIC_ALL_GRAPH:
00137 nn_nodes = graph.nn_nodes
00138 nt_nodes = graph.nt_nodes
00139 if quiet:
00140 nn_nodes = [n for n in nn_nodes if not n in QUIET_NAMES]
00141 nt_nodes = [n for n in nt_nodes if not n in QUIET_NAMES]
00142 if nn_nodes or nt_nodes:
00143 namespaces = [roslib.names.namespace(n) for n in nn_nodes]
00144
00145
00146
00147 namespaces.extend([roslib.names.namespace(n[1:]) for n in nt_nodes])
00148
00149 return list(set(namespaces))
00150
00151 def _filter_orphaned_edges(self, edges, nodes):
00152 nodenames = [str(n).strip() for n in nodes]
00153
00154 return [e for e in edges if e.start.strip() in nodenames and e.end.strip() in nodenames]
00155
00156 def _filter_orphaned_topics(self, nt_nodes, edges):
00157 '''remove topic graphnodes without connected ROS nodes'''
00158 removal_nodes = []
00159 for n in nt_nodes:
00160 keep = False
00161 for e in edges:
00162 if (e.start.strip() == str(n).strip() or e.end.strip() == str(n).strip()):
00163 keep = True
00164 break
00165 if not keep:
00166 removal_nodes.append(n)
00167 for n in removal_nodes:
00168 nt_nodes.remove(n)
00169 return nt_nodes
00170
00171 def _split_filter_string(self, ns_filter):
00172 '''splits a string after each comma, and treats tokens with leading dash as exclusions.
00173 Adds .* as inclusion if no other inclusion option was given'''
00174 includes = []
00175 excludes = []
00176 for name in ns_filter.split(','):
00177 if name.strip().startswith('-'):
00178 excludes.append(name.strip()[1:])
00179 else:
00180 includes.append(name.strip())
00181 if includes == [] or includes == ['/'] or includes == ['']:
00182 includes = ['.*']
00183 return includes, excludes
00184
00185 def _get_node_edge_map(self, edges):
00186 '''returns a dict mapping node name to edge objects partitioned in incoming and outgoing edges'''
00187 node_connections = {}
00188 for edge in edges:
00189 if not edge.start in node_connections:
00190 node_connections[edge.start] = NodeConnections()
00191 if not edge.end in node_connections:
00192 node_connections[edge.end] = NodeConnections()
00193 node_connections[edge.start].outgoing.append(edge)
00194 node_connections[edge.end].incoming.append(edge)
00195 return node_connections
00196
00197 def _filter_leaf_topics(self,
00198 nodes_in,
00199 edges_in,
00200 node_connections,
00201 hide_single_connection_topics,
00202 hide_dead_end_topics):
00203 '''
00204 removes certain ending topic nodes and their edges from list of nodes and edges
00205
00206 @param hide_single_connection_topics: if true removes topics that are only published/subscribed by one node
00207 @param hide_dead_end_topics: if true removes topics having only publishers
00208 '''
00209 if not hide_dead_end_topics and not hide_single_connection_topics:
00210 return nodes_in, edges_in
00211
00212 nodes = copy.copy(nodes_in)
00213 edges = copy.copy(edges_in)
00214 removal_nodes = []
00215 for n in nodes:
00216 if n in node_connections:
00217 node_edges = []
00218 has_out_edges = False
00219 node_edges.extend(node_connections[n].outgoing)
00220 if len(node_connections[n].outgoing) > 0:
00221 has_out_edges = True
00222 node_edges.extend(node_connections[n].incoming)
00223 if ((hide_single_connection_topics and len(node_edges) < 2) or
00224 (hide_dead_end_topics and not has_out_edges)):
00225 removal_nodes.append(n)
00226 for e in node_edges:
00227 if e in edges:
00228 edges.remove(e)
00229 for n in removal_nodes:
00230 nodes.remove(n)
00231 return nodes, edges
00232
00233 def _accumulate_action_topics(self, nodes_in, edges_in, node_connections):
00234 '''takes topic nodes, edges and node connections.
00235 Returns topic nodes where action topics have been removed,
00236 edges where the edges to action topics have been removed, and
00237 a map with the connection to each virtual action topic node'''
00238 removal_nodes = []
00239 action_nodes = {}
00240
00241 nodes = copy.copy(nodes_in)
00242 edges = copy.copy(edges_in)
00243 for n in nodes:
00244 if str(n).endswith('/feedback'):
00245 prefix = str(n)[:-len('/feedback')].strip()
00246 action_topic_nodes = []
00247 action_topic_edges_out = set()
00248 action_topic_edges_in = set()
00249 for suffix in ['/status', '/result', '/goal', '/cancel', '/feedback']:
00250 for n2 in nodes:
00251 if str(n2).strip() == prefix + suffix:
00252 action_topic_nodes.append(n2)
00253 if n2 in node_connections:
00254 action_topic_edges_out.update(node_connections[n2].outgoing)
00255 action_topic_edges_in.update(node_connections[n2].incoming)
00256 if len(action_topic_nodes) == 5:
00257
00258 removal_nodes.extend(action_topic_nodes)
00259 for e in action_topic_edges_out:
00260 if e in edges:
00261 edges.remove(e)
00262 for e in action_topic_edges_in:
00263 if e in edges:
00264 edges.remove(e)
00265 action_nodes[prefix] = {'topics': action_topic_nodes,
00266 'outgoing': action_topic_edges_out,
00267 'incoming': action_topic_edges_in}
00268 for n in removal_nodes:
00269 nodes.remove(n)
00270 return nodes, edges, action_nodes
00271
00272 def generate_dotgraph(self,
00273 rosgraphinst,
00274 ns_filter,
00275 topic_filter,
00276 graph_mode,
00277 dotcode_factory,
00278 hide_single_connection_topics=False,
00279 hide_dead_end_topics=False,
00280 cluster_namespaces_level=0,
00281 accumulate_actions=True,
00282 orientation='LR',
00283 rank='same',
00284 ranksep=0.2,
00285 rankdir='TB',
00286 simplify=True,
00287 quiet=False):
00288 """
00289 See generate_dotcode
00290 """
00291 includes, excludes = self._split_filter_string(ns_filter)
00292 topic_includes, topic_excludes = self._split_filter_string(topic_filter)
00293
00294 nn_nodes = []
00295 nt_nodes = []
00296
00297 if graph_mode == NODE_NODE_GRAPH:
00298 nn_nodes = rosgraphinst.nn_nodes
00299 nn_nodes = [n for n in nn_nodes if matches_any(n, includes) and not matches_any(n, excludes)]
00300 edges = rosgraphinst.nn_edges
00301 edges = [e for e in edges if matches_any(e.label, topic_includes) and not matches_any(e.label, topic_excludes)]
00302
00303 elif graph_mode == NODE_TOPIC_GRAPH or \
00304 graph_mode == NODE_TOPIC_ALL_GRAPH:
00305 nn_nodes = rosgraphinst.nn_nodes
00306 nt_nodes = rosgraphinst.nt_nodes
00307 nn_nodes = [n for n in nn_nodes if matches_any(n, includes) and not matches_any(n, excludes)]
00308 nt_nodes = [n for n in nt_nodes if matches_any(n, topic_includes) and not matches_any(n, topic_excludes)]
00309
00310
00311 if graph_mode == NODE_TOPIC_GRAPH:
00312 edges = [e for e in rosgraphinst.nt_edges]
00313 else:
00314 edges = [e for e in rosgraphinst.nt_all_edges]
00315
00316 if quiet:
00317 nn_nodes = filter(self._quiet_filter, nn_nodes)
00318 nt_nodes = filter(self._quiet_filter, nt_nodes)
00319 if graph_mode == NODE_NODE_GRAPH:
00320 edges = filter(self.quiet_filter_topic_edge, edges)
00321
00322
00323 action_nodes = {}
00324
00325 if graph_mode != NODE_NODE_GRAPH and (hide_single_connection_topics or hide_dead_end_topics or accumulate_actions):
00326
00327 node_connections = self._get_node_edge_map(edges)
00328
00329 nt_nodes, edges = self._filter_leaf_topics(nt_nodes,
00330 edges,
00331 node_connections,
00332 hide_single_connection_topics,
00333 hide_dead_end_topics)
00334
00335 if accumulate_actions:
00336 nt_nodes, edges, action_nodes = self._accumulate_action_topics(nt_nodes, edges, node_connections)
00337
00338 edges = self._filter_orphaned_edges(edges, list(nn_nodes) + list(nt_nodes))
00339 nt_nodes = self._filter_orphaned_topics(nt_nodes, edges)
00340
00341
00342
00343 dotgraph = dotcode_factory.get_graph(rank=rank,
00344 ranksep=ranksep,
00345 simplify=simplify,
00346 rankdir=orientation)
00347
00348 ACTION_TOPICS_SUFFIX = '/action_topics'
00349 namespace_clusters = {}
00350 for n in (nt_nodes or []) + [action_prefix + ACTION_TOPICS_SUFFIX for (action_prefix, _) in action_nodes.items()]:
00351
00352 if (cluster_namespaces_level > 0 and
00353 str(n).count('/') > 1 and
00354 len(str(n).split('/')[1]) > 0):
00355 namespace = str(n).split('/')[1]
00356 if namespace not in namespace_clusters:
00357 namespace_clusters[namespace] = dotcode_factory.add_subgraph_to_graph(dotgraph, namespace, rank=rank, rankdir=orientation, simplify=simplify)
00358 self._add_topic_node(n, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], quiet=quiet)
00359 else:
00360 self._add_topic_node(n, dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet)
00361
00362
00363
00364 if nn_nodes is not None:
00365 for n in nn_nodes:
00366 if (cluster_namespaces_level > 0 and
00367 str(n).count('/') >= 1 and
00368 len(str(n).split('/')[1]) > 0 and
00369 str(n).split('/')[1] in namespace_clusters):
00370 namespace = str(n).split('/')[1]
00371 self._add_node(n, rosgraphinst=rosgraphinst, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], quiet=quiet)
00372 else:
00373 self._add_node(n, rosgraphinst=rosgraphinst, dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet)
00374
00375 for e in edges:
00376 self._add_edge(e, dotcode_factory, dotgraph=dotgraph, is_topic=(graph_mode == NODE_NODE_GRAPH))
00377
00378 for (action_prefix, node_connections) in action_nodes.items():
00379 for out_edge in node_connections.get('outgoing', []):
00380 dotcode_factory.add_edge_to_graph(dotgraph, action_prefix[1:] + ACTION_TOPICS_SUFFIX, out_edge.end)
00381 for in_edge in node_connections.get('incoming', []):
00382 dotcode_factory.add_edge_to_graph(dotgraph, in_edge.start, action_prefix[1:] + ACTION_TOPICS_SUFFIX)
00383 return dotgraph
00384
00385 def generate_dotcode(self,
00386 rosgraphinst,
00387 ns_filter,
00388 topic_filter,
00389 graph_mode,
00390 dotcode_factory,
00391 hide_single_connection_topics=False,
00392 hide_dead_end_topics=False,
00393 cluster_namespaces_level=0,
00394 accumulate_actions=True,
00395 orientation='LR',
00396 rank='same',
00397 ranksep=0.2,
00398 rankdir='TB',
00399 simplify=True,
00400 quiet=False):
00401 """
00402 @param rosgraphinst: RosGraph instance
00403 @param ns_filter: nodename filter
00404 @type ns_filter: string
00405 @param topic_filter: topicname filter
00406 @type ns_filter: string
00407 @param graph_mode str: NODE_NODE_GRAPH | NODE_TOPIC_GRAPH | NODE_TOPIC_ALL_GRAPH
00408 @type graph_mode: str
00409 @param orientation: rankdir value (see ORIENTATIONS dict)
00410 @type dotcode_factory: object
00411 @param dotcode_factory: abstract factory manipulating dot language objects
00412 @param hide_single_connection_topics: if true remove topics with just one connection
00413 @param hide_dead_end_topics: if true remove topics with publishers only
00414 @param cluster_namespaces_level: if > 0 places box around members of same namespace (TODO: multiple namespace layers)
00415 @param accumulate_actions: if true each 5 action topic graph nodes are shown as single graph node
00416 @return: dotcode generated from graph singleton
00417 @rtype: str
00418 """
00419 dotgraph = self.generate_dotgraph(rosgraphinst=rosgraphinst,
00420 ns_filter=ns_filter,
00421 topic_filter=topic_filter,
00422 graph_mode=graph_mode,
00423 dotcode_factory=dotcode_factory,
00424 hide_single_connection_topics=hide_single_connection_topics,
00425 hide_dead_end_topics=hide_dead_end_topics,
00426 cluster_namespaces_level=cluster_namespaces_level,
00427 accumulate_actions=accumulate_actions,
00428 orientation=orientation,
00429 rank=rank,
00430 ranksep=ranksep,
00431 rankdir=rankdir,
00432 simplify=simplify,
00433 quiet=quiet)
00434 dotcode = dotcode_factory.create_dot(dotgraph)
00435 return dotcode
