[backend] ensure tree modifications are published with an updated tim…

…estamps (#100)

CHANGELOG.rst

@@ -4,6 +4,7 @@ Changelog
 
 Forthcoming
 -----------
+* [backend] ensure tree modifications are published with an updated timestamp, `#100 <https://github.com/splintered-reality/py_trees_ros/pull/100>`_
 
 1.1.2 (2019-08-10)
 ------------------

py_trees_ros/trees.py

@@ -165,7 +165,7 @@ def _setup_publishers(self):
         # set a handler to publish future modifications whenever the tree is modified
         # (e.g. pruned). The tree_update_handler method is in the base class, set this
         # to the callback function here.
-        self.tree_update_handler = self._publish_serialised_tree
+        self.tree_update_handler = self._on_tree_update_handler
 
     def tick_tock(
             self,
@@ -239,6 +239,17 @@ def _tick_tock_timer_callback(
         else:
             self.timer.cancel()
 
+    def _on_tree_update_handler(self):
+        """
+        Whenever there has been a modification to the tree (insertion/pruning), publish
+        the snapshot.
+        """
+        # only worth notifying once we've actually commenced
+        if self.statistics is not None:
+            rclpy_start_time = rclpy.clock.Clock().now()
+            self.statistics.stamp = rclpy_start_time.to_msg()
+            self._publish_serialised_tree()
+
     def _statistics_pre_tick_handler(self, tree: py_trees.trees.BehaviourTree):
         """
         Pre-tick handler that resets the statistics and starts the clock.