[watcher] de-serialise the tip (#91)

CHANGELOG.rst

@@ -7,6 +7,7 @@ Forthcoming
 * [actions] bugfix action client, don't cancel if not RUNNING
 * [conversions] bugfix msg_to_behaviour for decorators
 * [watchers] bugfix tree-watchers dot-graph to string functionality
+* [watchers] bugfix missing tip in deserialised tree-watcher tree
 
 1.0.0 (2019-04-28)
 ------------------

py_trees_ros/conversions.py

@@ -229,11 +229,13 @@ def behaviour_to_msg(behaviour: py_trees.behaviour.Behaviour) -> py_trees_ros_in
     msg.child_ids = [uuid4_to_msg(child.id) for child in behaviour.iterate(direct_descendants=True) if not child.id == behaviour.id]
 
     tip = behaviour.tip()
-    # tip_id is empty if the behaviour is invalid or if it is a valid
-    # leaf
+    # tip_id is empty if the behaviour is invalid or if it is a valid leaf
     if tip is not None and tip != behaviour:
         msg.tip_id = uuid4_to_msg(tip.id)
-
+    # else it gets the 'zero' uuid
+    if isinstance(behaviour, py_trees.composites.Composite):
+        if behaviour.current_child is not None:
+            msg.current_child_id = uuid4_to_msg(behaviour.current_child.id)
     msg.type = behaviour_type_to_msg_constant(behaviour)
     msg.blackbox_level = blackbox_enum_to_msg_constant(behaviour.blackbox_level)
     msg.status = status_enum_to_msg_constant(behaviour.status)

py_trees_ros/trees.py

@@ -7,7 +7,6 @@
 ##############################################################################
 # Documentation
 ##############################################################################
-
 """
 The :class:`py_trees_ros.trees.BehaviourTree` class
 extends the core :class:`py_trees.trees.BehaviourTree` class
@@ -38,6 +37,7 @@
 import tempfile
 import time
 import unique_identifier_msgs.msg as unique_identifier_msgs
+import uuid
 
 from . import blackboard
 from . import conversions
@@ -446,8 +446,29 @@ def deserialise_tree_recursively(msg):
                 child = deserialise_tree_recursively(
                     serialised_behaviours[child_id]
                 )
+                # invasive hack to revert the dummy child we added in msg_to_behaviour
+                if isinstance(behaviour, py_trees.decorators.Decorator):
+                    behaviour.children = [child]
+                    behaviour.decorated = behaviour.children[0]
+                else:
+                    behaviour.children.append(child)
                 child.parent = behaviour
-                behaviour.children.append(child)
+            # set the current child so tip() works properly everywhere
+            if behaviour.children:
+                if msg.current_child_id != unique_identifier_msgs.UUID():
+                    current_child_id = conversions.msg_to_uuid4(msg.current_child_id)
+                    for index, child in enumerate(behaviour.children):
+                        if child.id == current_child_id:
+                            # somewhat ugly not having a consistent api here
+                            if isinstance(behaviour, py_trees.composites.Selector):
+                                behaviour.current_child = child
+                            elif isinstance(behaviour, py_trees.composites.Chooser):
+                                behaviour.current_child = child
+                            elif isinstance(behaviour, py_trees.composites.Sequence):
+                                behaviour.current_index = index
+                            # else Parallel, nothing to do since it infers
+                            # the current child from children's status on the fly
+                            break
             if msg.is_active:
                 self.snapshot_visitor.visited[behaviour.id] = behaviour.status
             return behaviour