Better tree setup handling (#129)

* [trees] bugfix non-infinite timeout arg getting ignored
* [trees] permit setup visitors, provide a default

.settings/org.eclipse.core.resources.prefs

@@ -15,6 +15,7 @@ encoding//py_trees_ros/subscribers.py=utf-8
 encoding//py_trees_ros/transforms.py=utf-8
 encoding//py_trees_ros/trees.py=utf-8
 encoding//py_trees_ros/utilities.py=utf-8
+encoding//py_trees_ros/visitors.py=utf-8
 encoding//tests/test_action_client.py=utf-8
 encoding//tests/test_subscribers.py=utf-8
 encoding//tests/test_transforms.py=utf-8

CHANGELOG.rst

@@ -4,14 +4,16 @@ Changelog
 
 Forthcoming
 -----------
-* [trees] snapshot publishing changed, key access info, `#124 <https://github.com/splintered-reality/py_trees_ros/pull/124>`_,
+* [trees] permit setup visitors, provide a default with timings, `#129 <https://github.com/splintered-reality/py_trees_ros/pull/129>`_,
+* [trees] bugfix non-infinite timeout arg getting ignored, `#129 <https://github.com/splintered-reality/py_trees_ros/pull/129>`_,
+* [trees] snapshot now publishing a) tree changed, b) key access info, `#128 <https://github.com/splintered-reality/py_trees_ros/pull/128>`_,
 
 2.0.0 (2019-11-20)
 ------------------
 * [blackboards] updated pretty printing to differentiate namespace vs attribute access, `#123 <https://github.com/splintered-reality/py_trees_ros/pull/123>`_
 * [blackboards] api updates for namespaced clients, `#122 <https://github.com/splintered-reality/py_trees_ros/pull/122>`_,
 * [tests] migrated tests from unittest to pytest
-* [transforms] From and To behaviours added, `#121 <https://github.com/splintered-reality/py_trees_ros/pull/121>`_
+* [transforms] behaviours for writing to and broadcasting from the blackboard, `#121 <https://github.com/splintered-reality/py_trees_ros/pull/121>`_
 * [transforms] add missing mocks and update to latest blackboard api, `#125 <https://github.com/splintered-reality/py_trees_ros/pull/125>`_
 
 1.2.1 (2019-10-08)

py_trees_ros/trees.py

@@ -46,6 +46,7 @@
 from . import conversions
 from . import exceptions
 from . import utilities
+from . import visitors
 
 ##############################################################################
 # ROS Trees
@@ -120,13 +121,18 @@ def __init__(self,
         # _cleanup must come last as it assumes the existence of the bag
         # TODO: rospy.on_shutdown(self._cleanup)
 
-    def setup(self, timeout: float=py_trees.common.Duration.INFINITE):
+    def setup(
+            self,
+            timeout: float=py_trees.common.Duration.INFINITE,
+            visitor: py_trees.visitors.VisitorBase=None
+    ):
         """
         Setup the publishers, exechange and add ros-relevant pre/post tick handlers to the tree.
         Ultimately relays this call down to all the behaviours in the tree.
 
         Args:
             timeout: time (s) to wait (use common.Duration.INFINITE to block indefinitely)
+            visitor: runnable entities on each node after it's setup
 
         ROS Params:
             timeout: time (s) to wait (use common.Duration.INFINITE (math.inf) to block indefinitely)
@@ -140,12 +146,14 @@ def setup(self, timeout: float=py_trees.common.Duration.INFINITE):
         # node creation - can raise rclpy.exceptions.NotInitializedException
         default_node_name = "tree"
         self.node = rclpy.create_node(node_name=default_node_name)
+        if visitor is None:
+            visitor = visitors.SetupLogger(node=self.node)
         # timeout parameter:
         #   if not initialised from, e.g. launch, then
         #   use the arg provided timeout
         self.node.declare_parameter(
             name='setup_timeout_sec',
-            value=timeout if not py_trees.common.Duration.INFINITE else py_trees.common.Duration.INFINITE.value,
+            value=timeout if timeout != py_trees.common.Duration.INFINITE else py_trees.common.Duration.INFINITE.value,
             descriptor=rcl_interfaces_msgs.ParameterDescriptor(
                 name="setup_timeout_sec",
                 type=rcl_interfaces_msgs.ParameterType.PARAMETER_DOUBLE,  # noqa
@@ -173,7 +181,11 @@ def setup(self, timeout: float=py_trees.common.Duration.INFINITE):
 
         # share the tree's node with it's behaviours
         try:
-            super().setup(setup_timeout_sec, node=self.node)
+            super().setup(
+                timeout=setup_timeout_sec,
+                visitor=visitor,
+                node=self.node
+            )
         except RuntimeError as e:
             if str(e) == "tree setup interrupted or timed out":
                 raise exceptions.TimedOutError(str(e))

py_trees_ros/visitors.py

@@ -27,6 +27,8 @@
 
 import py_trees.visitors
 import py_trees_ros_interfaces.msg as py_trees_msgs
+import rclpy
+import time
 
 from . import conversions
 
@@ -35,6 +37,40 @@
 ##############################################################################
 
 
+class SetupLogger(py_trees.visitors.VisitorBase):
+    """
+    Use as a visitor to :meth:`py_trees_ros.trees.TreeManager.setup`
+    to log the name and timings of each behaviours' setup
+    to the ROS debug channel.
+
+    Args:
+        node: an rclpy node that will provide debug logger
+    """
+    def __init__(self, node: rclpy.node.Node):
+        super().__init__(full=True)
+        self.node = node
+
+    def initialise(self):
+        """
+        Initialise the timestamping chain.
+        """
+        self.start_time = time.monotonic()
+        self.last_time = self.start_time
+
+    def run(self, behaviour):
+        current_time = time.monotonic()
+        self.node.get_logger().debug(
+            "'{}'.setup: {:.4f}s".format(behaviour.name, current_time - self.last_time)
+        )
+        self.last_time = current_time
+
+    def finalise(self):
+        current_time = time.monotonic()
+        self.node.get_logger().debug(
+            "Total tree setup time: {:.4f}s".format(current_time - self.start_time)
+        )
+
+
 class TreeToMsgVisitor(py_trees.visitors.VisitorBase):
     """
     Visits the entire tree and gathers all behaviours as