[behaviours] FromBlackboard publisher (#146)

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

@@ -11,13 +11,15 @@ encoding//py_trees_ros/programs/blackboard_watcher.py=utf-8
 encoding//py_trees_ros/programs/echo.py=utf-8
 encoding//py_trees_ros/programs/multi_talker.py=utf-8
 encoding//py_trees_ros/programs/tree_watcher.py=utf-8
+encoding//py_trees_ros/publishers.py=utf-8
 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_expand_topic_name.py=utf-8
+encoding//tests/test_publishers.py=utf-8
 encoding//tests/test_subscribers.py=utf-8
 encoding//tests/test_transforms.py=utf-8
 encoding/setup.py=utf-8

doc/conf.py

@@ -62,7 +62,7 @@
 
 intersphinx_mapping = {
     'python': ('https://docs.python.org/3', None),
-    'py_trees': ('https://py-trees.readthedocs.io/en/release-1.2.x', None),
+    'py_trees': ('https://py-trees.readthedocs.io/en/release-2.0.x', None),
     'py_trees_ros_tutorials': ('https://py-trees-ros-tutorials.readthedocs.io/en/release-1.0.x', None),
     'rclpy': ('http://docs.ros2.org/crystal/api/rclpy/', None),
 }

doc/features.rst

@@ -8,6 +8,7 @@ Behaviours
 
    py_trees_ros.actions.ActionClient
    py_trees_ros.battery.ToBlackboard
+   py_trees_ros.publishers.FromBlackboard
    py_trees_ros.subscribers.CheckData
    py_trees_ros.subscribers.EventToBlackboard
    py_trees_ros.subscribers.ToBlackboard

doc/modules.rst

@@ -49,6 +49,14 @@ py_trees_ros.exceptions
     :show-inheritance:
     :synopsis: custom exceptions for py_trees_ros
 
+py_trees_ros.publishers
+-----------------------
+
+.. automodule:: py_trees_ros.publishers
+    :members:
+    :show-inheritance:
+    :synopsis: publish data to the ROS network
+
 py_trees_ros.subscribers
 ------------------------
 

py_trees_ros/__init__.py

@@ -21,6 +21,7 @@
 from . import exceptions
 from . import mock
 from . import programs
+from . import publishers
 from . import subscribers
 from . import transforms
 from . import trees

py_trees_ros/publishers.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# License: BSD
+#   https://raw.githubusercontent.com/splintered-reality/py_trees_ros/devel/LICENSE
+#
+##############################################################################
+# Description
+##############################################################################
+
+"""
+Convenience behaviours for publishing ROS messages.
+"""
+
+##############################################################################
+# Imports
+##############################################################################
+
+import typing
+
+import py_trees
+import rclpy.qos
+
+##############################################################################
+# Behaviours
+##############################################################################
+
+
+class FromBlackboard(py_trees.behaviour.Behaviour):
+    """
+    This behaviour looks up the blackboard for content to publish ...
+    and publishes it.
+
+    This is a non-blocking behaviour - if there is no data yet on
+    the blackboard it will tick with :data:`~py_trees.common.Status.FAILURE`,
+    otherwise :data:`~py_trees.common.Status.SUCCESS`.
+
+    To convert it to a blocking behaviour, simply use with the
+    :class:`py_trees.behaviours.WaitForBlackboardVariable`. e.g.
+
+    .. code-block:: python
+
+        sequence = py_trees.composites.Sequence(name="Sequence")
+        wait_for_data = py_trees.behaviours.WaitForBlackboardVariable(
+            name="WaitForData",
+            variable_name="/my_message"
+        )
+        publisher = py_trees_ros.publishers.FromBlackboard(
+            topic_name="/foo",
+            topic_type=std_msgs.msg.Empty
+            qos_profile=my_qos_profile,
+            blackboard_variable="/my_message"
+        )
+        sequence.add_children([wait_for_data, publisher])
+
+    The various set/unset blackboard variable behaviours can also be useful for
+    setting and unsetting the message to be published (typically elsewhere
+    in the tree).
+
+    Args:
+        topic_name: name of the topic to connect to
+        topic_type: class of the message type (e.g. :obj:`std_msgs.msg.String`)
+        qos_profile: qos profile for the subscriber
+        name: name of the behaviour
+        blackboared_variable: name of the variable on the blackboard (can be nested)
+    """
+    def __init__(self,
+                 topic_name: str,
+                 topic_type: typing.Any,
+                 qos_profile: rclpy.qos.QoSProfile,
+                 blackboard_variable: str,
+                 name: str=py_trees.common.Name.AUTO_GENERATED,
+                 ):
+        super().__init__(name=name)
+        self.topic_name = topic_name
+        self.topic_type = topic_type
+        self.blackboard = self.attach_blackboard_client(name=self.name)
+        self.blackboard_variable = blackboard_variable
+        self.key = blackboard_variable.split('.')[0]  # in case it is nested
+        self.blackboard.register_key(
+            key=self.key,
+            access=py_trees.common.Access.READ
+        )
+        self.publisher = None
+        self.qos_profile = qos_profile
+        self.node = None
+
+    def setup(self, **kwargs):
+        """
+        Initialises the publisher.
+
+        Args:
+            **kwargs (:obj:`dict`): distribute arguments to this
+               behaviour and in turn, all of it's children
+
+        Raises:
+            KeyError: if a ros2 node isn't passed under the key 'node' in kwargs
+        """
+        try:
+            self.node = kwargs['node']
+        except KeyError as e:
+            error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(self.name, self.__class__.__name__)
+            raise KeyError(error_message) from e  # 'direct cause' traceability
+        self.publisher = self.node.create_publisher(
+            msg_type=self.topic_type,
+            topic=self.topic_name,
+            qos_profile=self.qos_profile
+        )
+
+    def update(self):
+        """
+        Publish the specified variable from the blackboard.
+
+        Raises:
+            TypeError if the blackboard variable is not of the required type
+
+        Returns:
+            :data:`~py_trees.common.Status.FAILURE` (variable does not exist on the blackboard) or :data:`~py_trees.common.Status.SUCCESS` (published)
+        """
+        self.logger.debug("%s.initialise()" % self.__class__.__name__)
+        try:
+            if isinstance(self.blackboard.get(self.blackboard_variable), self.topic_type):
+                self.publisher.publish(self.blackboard.get(self.blackboard_variable))
+            else:
+                raise TypeError("{} is not the required type [{}][{}]".format(
+                    self.blackboard_variable,
+                    self.topic_type,
+                    type(self.blackboard.get(self.blackboard_variable)))
+                )
+            self.feedback_message = "published"
+            return py_trees.common.Status.SUCCESS
+        except KeyError:
+            self.feedback_message = "nothing to publish"
+            return py_trees.common.Status.FAILURE

tests/test_publishers.py

@@ -0,0 +1,182 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# License: BSD
+#   https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE
+#
+
+##############################################################################
+# Imports
+##############################################################################
+
+import py_trees
+import py_trees.console as console
+import py_trees_ros
+import pytest
+import rclpy
+import rclpy.executors
+import std_msgs.msg as std_msgs
+import time
+
+##############################################################################
+# Helpers
+##############################################################################
+
+
+def assert_banner():
+    print(console.green + "----- Asserts -----" + console.reset)
+
+
+def assert_details(text, expected, result):
+    print(console.green + text +
+          "." * (40 - len(text)) +
+          console.cyan + "{}".format(expected) +
+          console.yellow + " [{}]".format(result) +
+          console.reset)
+
+
+def setup_module(module):
+    console.banner("ROS Init")
+    rclpy.init()
+
+
+def teardown_module(module):
+    console.banner("ROS Shutdown")
+    rclpy.shutdown()
+
+
+def topic_name():
+    return "/empty"
+
+
+def blackboard_key():
+    return "/empty"
+
+
+def timeout():
+    return 0.3
+
+
+def number_of_iterations():
+    return 3
+
+
+def qos_profile():
+    return py_trees_ros.utilities.qos_profile_unlatched()
+
+
+class Subscriber(object):
+    def __init__(self, node_name, topic_name, topic_type, qos_profile):
+
+        self.node = rclpy.create_node(node_name)
+        self.subscriber = self.node.create_subscription(
+            msg_type=topic_type,
+            topic=topic_name,
+            callback=self.callback,
+            qos_profile=qos_profile
+        )
+        self.count = 0
+
+    def callback(self, msg):
+        self.count += 1
+
+    def shutdown(self):
+        self.subscriber.destroy()
+        self.node.destroy_node()
+
+
+def create_all_the_things():
+    subscriber = Subscriber(
+        node_name="catcher_of_nothing",
+        topic_name=topic_name(),
+        topic_type=std_msgs.Empty,
+        qos_profile=qos_profile()
+    )
+    root = py_trees_ros.publishers.FromBlackboard(
+        topic_name=topic_name(),
+        topic_type=std_msgs.Empty,
+        qos_profile=qos_profile(),
+        blackboard_variable=blackboard_key()
+    )
+    return subscriber, root
+
+##############################################################################
+# Tests
+##############################################################################
+
+
+def test_publish_with_existing_data():
+    console.banner("Publish Existing Data")
+
+    py_trees.blackboard.Blackboard.set(
+        variable_name=blackboard_key(),
+        value=std_msgs.Empty()
+    )
+
+    subscriber, root = create_all_the_things()
+    tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False)
+    tree.setup()
+    executor = rclpy.executors.SingleThreadedExecutor()
+    executor.add_node(subscriber.node)
+    executor.add_node(tree.node)
+
+    assert_banner()
+
+    start_time = time.monotonic()
+    while ((time.monotonic() - start_time) < timeout()) and subscriber.count == 0:
+        tree.tick()
+        executor.spin_once(timeout_sec=0.05)
+
+    assert_details("root.status", "SUCCESS", root.status)
+    assert(root.status == py_trees.common.Status.SUCCESS)
+
+    py_trees.blackboard.Blackboard.clear()
+    tree.shutdown()
+    subscriber.shutdown()
+    executor.shutdown()
+
+
+def test_fail_with_no_data():
+    console.banner("Fail with No Data")
+
+    subscriber, root = create_all_the_things()
+    tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False)
+    tree.setup()
+    executor = rclpy.executors.SingleThreadedExecutor()
+    executor.add_node(subscriber.node)
+    executor.add_node(tree.node)
+    assert_banner()
+
+    tree.tick()
+    executor.spin_once(timeout_sec=0.05)
+
+    assert_details("root.status", "FAILURE", root.status)
+    assert(root.status == py_trees.common.Status.FAILURE)
+
+    tree.shutdown()
+    subscriber.shutdown()
+    executor.shutdown()
+
+
+def test_exception_with_wrong_data():
+    console.banner("Exception with Wrong Data")
+
+    py_trees.blackboard.Blackboard.set(
+        variable_name=blackboard_key(),
+        value=5.0
+    )
+
+    subscriber, root = create_all_the_things()
+    tree = py_trees_ros.trees.BehaviourTree(root=root, unicode_tree_debug=False)
+    tree.setup()
+    executor = rclpy.executors.SingleThreadedExecutor()
+    executor.add_node(subscriber.node)
+    executor.add_node(tree.node)
+    assert_banner()
+
+    with pytest.raises(TypeError) as unused_e_info:  # e_info survives outside this block
+        tree.tick()
+
+    tree.shutdown()
+    subscriber.shutdown()
+    executor.shutdown()