diff --git a/src/rqt_py_trees/behaviour_tree.py b/src/rqt_py_trees/behaviour_tree.py index f60c6c2..76cd7af 100644 --- a/src/rqt_py_trees/behaviour_tree.py +++ b/src/rqt_py_trees/behaviour_tree.py @@ -45,6 +45,7 @@ import sys import termcolor import uuid_msgs.msg as uuid_msgs +import unique_id from . import visibility @@ -52,9 +53,10 @@ from .dynamic_timeline import DynamicTimeline from .dynamic_timeline_listener import DynamicTimelineListener from .timeline_listener import TimelineListener -from qt_dotgraph.dot_to_qt import DotToQtGenerator -from qt_dotgraph.pydotfactory import PydotFactory -from qt_dotgraph.pygraphvizfactory import PygraphvizFactory +from .qt_dotgraph.dot_to_qt import DotToQtGenerator +from .qt_dotgraph.pydotfactory import PydotFactory +from .qt_dotgraph.pygraphvizfactory import PygraphvizFactory +from visibility import items_with_hidden_children from rqt_bag.bag_timeline import BagTimeline # from rqt_bag.bag_widget import BagGraphicsView from rqt_graph.interactive_graphics_view import InteractiveGraphicsView @@ -68,8 +70,6 @@ except ImportError: # kinetic+ (pyqt5) from python_qt_binding.QtWidgets import QFileDialog, QGraphicsView, QGraphicsScene, QWidget, QShortcut -from . import qt_dotgraph - class RosBehaviourTree(QObject): @@ -78,6 +78,7 @@ class RosBehaviourTree(QObject): _refresh_combo = Signal() _message_changed = Signal() _message_cleared = Signal() + _node_item_click_event = Signal(str) _expected_type = py_trees_msgs.BehaviourTree()._type _empty_topic = "No valid topics available" _unselected_topic = "Not subscribing" @@ -267,6 +268,10 @@ def __init__(self, context): self._refresh_view.connect(self._refresh_tree_graph) self._force_refresh = False + self._force_redraw = False + + # click callback with a delayed response + self._node_item_click_event.connect(self.node_item_click_event, type=Qt.QueuedConnection) if self.live_update: context.add_widget(self._widget) @@ -288,6 +293,8 @@ def _update_visibility_level(self, visibility_level): We match the combobox index to the visibility levels defined in py_trees.common.VisibilityLevel. """ self.visibility_level = visibility.combo_to_py_trees[visibility_level] + self._force_refresh = True + self._force_redraw = True self._refresh_tree_graph() @staticmethod @@ -602,7 +609,8 @@ def _generate_dotcode(self, message): key = str(message.header.stamp) # stamps are unique if key in self._dotcode_cache: - return self._dotcode_cache[key] + if not self._force_refresh: + return self._dotcode_cache[key] force_refresh = self._force_refresh self._force_refresh = False @@ -615,8 +623,9 @@ def _generate_dotcode(self, message): timestamp=message.header.stamp, force_refresh=force_refresh ) + if key not in self._dotcode_cache: + self._dotcode_cache_keys.append(key) self._dotcode_cache[key] = dotcode - self._dotcode_cache_keys.append(key) if len(self._dotcode_cache) > self._dotcode_cache_capacity: oldest = self._dotcode_cache_keys[0] @@ -631,9 +640,18 @@ def _update_graph_view(self, dotcode): self._current_dotcode = dotcode self._redraw_graph_view() + def node_item_click_event(self, id): + if str(id) in items_with_hidden_children: + items_with_hidden_children.remove(str(id)) + else: + items_with_hidden_children.append(str(id)) + self._force_refresh = True + self._force_redraw = True + self._refresh_view.emit() + def _redraw_graph_view(self): key = str(self.get_current_message().header.stamp) - if key in self._scene_cache: + if key in self._scene_cache and not self._force_redraw: new_scene = self._scene_cache[key] else: # cache miss new_scene = QGraphicsScene() @@ -648,11 +666,12 @@ def _redraw_graph_view(self): # highlight_level) # this function is very expensive (nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, - highlight_level) + highlight_level, + click_signal=self._node_item_click_event) - for node_item in nodes.itervalues(): + for node_item in iter(nodes.values()): new_scene.addItem(node_item) - for edge_items in edges.itervalues(): + for edge_items in iter(edges.values()): for edge_item in edge_items: edge_item.add_to_scene(new_scene) @@ -660,13 +679,16 @@ def _redraw_graph_view(self): # put the scene in the cache self._scene_cache[key] = new_scene - self._scene_cache_keys.append(key) + if not self._force_redraw: + self._scene_cache_keys.append(key) if len(self._scene_cache) > self._scene_cache_capacity: oldest = self._scene_cache_keys[0] del self._scene_cache[oldest] self._scene_cache_keys.remove(oldest) + self._force_redraw = False + # after construction, set the scene and fit to the view self._scene = new_scene @@ -819,10 +841,10 @@ def _load_bag(self, file_name=None): rospy.loginfo("Reading bag from {0}".format(file_name)) bag = rosbag.Bag(file_name, 'r') # ugh... - topics = bag.get_type_and_topic_info()[1].keys() + topics = list(bag.get_type_and_topic_info()[1].keys()) types = [] for i in range(0, len(bag.get_type_and_topic_info()[1].values())): - types.append(bag.get_type_and_topic_info()[1].values()[i][0]) + types.append(list(bag.get_type_and_topic_info()[1].values())[i][0]) tree_topics = [] # only look at the first matching topic for ind, tp in enumerate(types): diff --git a/src/rqt_py_trees/qt_dotgraph/dot_to_qt.py b/src/rqt_py_trees/qt_dotgraph/dot_to_qt.py index 578d153..7f5b68d 100644 --- a/src/rqt_py_trees/qt_dotgraph/dot_to_qt.py +++ b/src/rqt_py_trees/qt_dotgraph/dot_to_qt.py @@ -112,7 +112,7 @@ def getNodeItemForSubgraph(self, subgraph, highlight_level): subgraph_nodeitem.set_hovershape(bounding_box) return subgraph_nodeitem - def getNodeItemForNode(self, node, highlight_level): + def getNodeItemForNode(self, node, highlight_level, click_signal=None): """ returns a pyqt NodeItem object, or None in case of error or invisible style """ @@ -164,9 +164,11 @@ def getNodeItemForNode(self, node, highlight_level): label=name, shape=node.attr.get('shape', 'ellipse'), color=color, - tooltip=node.attr.get('tooltip', None) + tooltip=node.attr.get('tooltip', None), # parent=None, # label_pos=None + uuid=node.name, + click_signal=click_signal ) # node_item.setToolTip(self._generate_tool_tip(node.attr.get('URL', None))) return node_item @@ -237,7 +239,7 @@ def addEdgeItem(self, edge, nodes, edges, highlight_level, same_label_siblings=F edges[label] = [] edges[label].append(edge_item) - def dotcode_to_qt_items(self, dotcode, highlight_level, same_label_siblings=False): + def dotcode_to_qt_items(self, dotcode, highlight_level, same_label_siblings=False, click_signal=None): """ takes dotcode, runs layout, and creates qt items based on the dot layout. returns two dicts, one mapping node names to Node_Item, one mapping edge names to lists of Edge_Item @@ -271,12 +273,12 @@ def dotcode_to_qt_items(self, dotcode, highlight_level, same_label_siblings=Fals # hack required by pydot if node.get_name() in ('graph', 'node', 'empty'): continue - nodes[node.get_name()] = self.getNodeItemForNode(node, highlight_level) + nodes[node.get_name()] = self.getNodeItemForNode(node, highlight_level, click_signal) for node in graph.nodes_iter(): # hack required by pydot if node.get_name() in ('graph', 'node', 'empty'): continue - nodes[node.get_name()] = self.getNodeItemForNode(node, highlight_level) + nodes[node.get_name()] = self.getNodeItemForNode(node, highlight_level, click_signal) edges = {} diff --git a/src/rqt_py_trees/qt_dotgraph/node_item.py b/src/rqt_py_trees/qt_dotgraph/node_item.py index 6ff1b88..396a9a1 100644 --- a/src/rqt_py_trees/qt_dotgraph/node_item.py +++ b/src/rqt_py_trees/qt_dotgraph/node_item.py @@ -35,7 +35,7 @@ class NodeItem(GraphItem): - def __init__(self, highlight_level, bounding_box, label, shape, color=None, parent=None, label_pos=None, tooltip=None): + def __init__(self, highlight_level, bounding_box, label, shape, color=None, parent=None, label_pos=None, tooltip=None, uuid=None, click_signal=None): super(NodeItem, self).__init__(highlight_level, parent) self._default_color = self._COLOR_BLACK if color is None else color @@ -128,6 +128,9 @@ def __init__(self, highlight_level, bounding_box, label, shape, color=None, pare self.hovershape = None + self._id = uuid + self._click_signal = click_signal + def set_hovershape(self, newhovershape): self.hovershape = newhovershape @@ -202,3 +205,7 @@ def hoverLeaveEvent(self, event): outgoing_edge.set_node_color() if self._highlight_level > 2 and outgoing_edge.to_node != self: outgoing_edge.to_node.set_node_color() + + def mouseDoubleClickEvent(self, event): + if self._click_signal is not None: + self._click_signal.emit(self._id) diff --git a/src/rqt_py_trees/visibility.py b/src/rqt_py_trees/visibility.py index d9e08ec..e94272c 100644 --- a/src/rqt_py_trees/visibility.py +++ b/src/rqt_py_trees/visibility.py @@ -50,14 +50,16 @@ py_trees_msgs.Behaviour.BLACKBOX_LEVEL_NOT_A_BLACKBOX: py_trees.common.BlackBoxLevel.NOT_A_BLACKBOX } +items_with_hidden_children = [] + def is_root(behaviour_id): """ Check the unique id to determine if it is the root (all zeros). - :param uuid.UUID behaviour_id: + :param str behaviour_id: """ - return behaviour_id == unique_id.fromMsg(uuid_msgs.UniqueID()) + return behaviour_id == str(uuid_msgs.UniqueID()) def get_branch_blackbox_level(behaviours, behaviour_id, current_level): @@ -66,25 +68,43 @@ def get_branch_blackbox_level(behaviours, behaviour_id, current_level): this behaviour. :param {id: py_trees_msgs.Behaviour} behaviours: (sub)tree of all behaviours, including this one - :param uuid.UUID behaviour_id: id of this behavour + :param str behaviour_id: id of this behavour :param py_trees.common.BlackBoxLevel current_level """ if is_root(behaviour_id): return current_level - parent_id = unique_id.fromMsg(behaviours[behaviour_id].parent_id) + parent_id = str(behaviours[behaviour_id].parent_id) new_level = min(behaviours[behaviour_id].blackbox_level, current_level) return get_branch_blackbox_level(behaviours, parent_id, new_level) +def is_parent_visible(behaviours, behaviour_id): + """ + :param {id: py_trees_msgs.Behaviour} behaviours: + :param str behaviour_id: + """ + parent_id = str(behaviours[behaviour_id].parent_id) + for i in items_with_hidden_children: + if i == str(parent_id): + return False + + if parent_id in behaviours: + return is_parent_visible(behaviours, parent_id) + + return True def is_visible(behaviours, behaviour_id, visibility_level): """ :param {id: py_trees_msgs.Behaviour} behaviours: - :param uuid.UUID behaviour_id: + :param str behaviour_id: :param py_trees.common.VisibilityLevel visibility_level """ + # check if the parent is visible + if not is_parent_visible(behaviours, behaviour_id): + return False + branch_blackbox_level = get_branch_blackbox_level( behaviours, - unique_id.fromMsg(behaviours[behaviour_id].parent_id), + str(behaviours[behaviour_id].parent_id), py_trees.common.BlackBoxLevel.NOT_A_BLACKBOX ) # see also py_trees.display.generate_pydot_graph @@ -99,9 +119,9 @@ def filter_behaviours_by_visibility_level(behaviours, visibility_level): :param py_trees_msgs.msg.Behaviour[] behaviours :returns: py_trees_msgs.msg.Behaviour[] """ - behaviours_by_id = {unique_id.fromMsg(b.own_id): b for b in behaviours} + behaviours_by_id = {str(b.own_id): b for b in behaviours} visible_behaviours = [b for b in behaviours if is_visible(behaviours_by_id, - unique_id.fromMsg(b.own_id), + str(b.own_id), visibility_level) ] return visible_behaviours