Publish snapshot changed & key access info

CHANGELOG.rst

@@ -4,7 +4,7 @@ Changelog
 
 Forthcoming
 -----------
-* ...
+* [trees] snapshot publishing changed, key access info, `#124 <https://github.com/splintered-reality/py_trees_ros/pull/124>`_,
 
 2.0.0 (2019-11-20)
 ------------------

py_trees_ros/conversions.py

@@ -240,7 +240,26 @@ def behaviour_to_msg(behaviour: py_trees.behaviour.Behaviour) -> py_trees_ros_in
     msg.blackbox_level = blackbox_enum_to_msg_constant(behaviour.blackbox_level)
     msg.status = status_enum_to_msg_constant(behaviour.status)
     msg.message = behaviour.feedback_message
-
+    msg.blackboard_access = []
+    for blackboard in behaviour.blackboards:
+        for key in blackboard.read:
+            access_info = py_trees_ros_interfaces.msg.KeyValue(
+                key=key,
+                value=py_trees_ros_interfaces.msg.Behaviour.BLACKBOARD_ACCESS_READ  # noqa
+            )
+            msg.blackboard_access.append(access_info)
+        for key in blackboard.write:
+            access_info = py_trees_ros_interfaces.msg.KeyValue(
+                key=key,
+                value=py_trees_ros_interfaces.msg.Behaviour.BLACKBOARD_ACCESS_WRITE  # noqa
+            )
+            msg.blackboard_access.append(access_info)
+        for key in blackboard.exclusive:
+            access_info = py_trees_ros_interfaces.msg.KeyValue(
+                key=key,
+                value=py_trees_ros_interfaces.msg.Behaviour.BLACKBOARD_ACCESS_EXCLUSIVE_WRITE  # noqa
+            )
+            msg.blackboard_access.append(access_info)
     return msg
 
 

py_trees_ros/trees.py

@@ -190,7 +190,7 @@ def _setup_publishers(self):
         )
 
         # publish current state
-        self._publish_serialised_tree()
+        self._publish_serialised_tree(changed=True)
 
         # 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
@@ -279,7 +279,7 @@ def _on_tree_update_handler(self):
         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()
+            self._publish_serialised_tree(changed=True)
 
     def _statistics_pre_tick_handler(self, tree: py_trees.trees.BehaviourTree):
         """
@@ -353,7 +353,7 @@ def _on_change_post_tick_handler(self, tree: py_trees.trees.BehaviourTree):
 
         # if tree state changed, publish
         if self.snapshot_visitor.changed:
-            self._publish_serialised_tree()
+            self._publish_serialised_tree(changed=self.snapshot_visitor.changed)
             # with self.lock:
             #     if not self._bag_closed:
             #         # self.bag.write(self.publishers.log_tree.name, self.logging_visitor.tree)
@@ -364,15 +364,16 @@ def _on_change_post_tick_handler(self, tree: py_trees.trees.BehaviourTree):
             visited_client_ids=self.snapshot_visitor.visited_blackboard_client_ids  # .keys()
         )
 
-    def _publish_serialised_tree(self):
+    def _publish_serialised_tree(self, changed: bool=False):
         """"
         Args:
-            tree (:class:`~py_trees.trees_ros.BehaviourTree`): the behaviour tree that has just been ticked
+            changed: whether the tree status / graph changed or not
         """
         # Don't fuss over lazy publishing, tree changes should not occur with high
         # frequency and more importantly, it needs to be latched with the latest
         # snapshot in the case of it not changing for quite some time to come...
         tree_message = py_trees_msgs.BehaviourTree()
+        tree_message.changed = changed
         # tree
         for behaviour in self.root.iterate():
             msg = conversions.behaviour_to_msg(behaviour)