[trees] snapshot streams (#135)

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

@@ -17,6 +17,7 @@ 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_subscribers.py=utf-8
 encoding//tests/test_transforms.py=utf-8
 encoding/setup.py=utf-8

py_trees_ros/blackboard.py

@@ -218,10 +218,10 @@ class Exchange(object):
         * **~/get_variables** (:class:`py_trees_msgs.srv.GetBlackboardVariables`)
 
           * list all the blackboard variable names (not values)
-        * **~/open_watcher** (:class:`py_trees_msgs.srv.OpenBlackboardWatcher`)
+        * **~/open** (:class:`py_trees_msgs.srv.OpenBlackboardStream`)
 
           * request a publisher to stream a part of the blackboard contents
-        * **~/close_watcher** (:class:`py_trees_msgs.srv.CloseBlackboardWatcher`)
+        * **~/close** (:class:`py_trees_msgs.srv.CloseBlackboardStream`)
 
           * close a previously opened watcher
 
@@ -245,8 +245,7 @@ def __init__(self):
         self.node = None
         self.views = []
         self.services = {}
-        # might want to revisit this if it proves to be suboptimal
-        py_trees.blackboard.Blackboard.enable_activity_stream()
+        self.activity_stream_clients = 0
 
     def setup(self, node: rclpy.node.Node):
         """
@@ -280,12 +279,12 @@ def setup(self, timeout):
         self.node = node
         for service_name, service_type in [
             ("get_variables", py_trees_srvs.GetBlackboardVariables),
-            ("open_watcher", py_trees_srvs.OpenBlackboardWatcher),
-            ("close_watcher", py_trees_srvs.CloseBlackboardWatcher)
+            ("open", py_trees_srvs.OpenBlackboardStream),
+            ("close", py_trees_srvs.CloseBlackboardStream)
         ]:
             self.services[service_name] = self.node.create_service(
                 srv_type=service_type,
-                srv_name='~/blackboard/' + service_name,
+                srv_name='~/blackboard_streams/' + service_name,
                 callback=getattr(self, "_{}_service".format(service_name)),
                 qos_profile=rclpy.qos.qos_profile_services_default
             )
@@ -331,42 +330,50 @@ def post_tick_handler(self, visited_client_ids: typing.List[uuid.UUID]=None):
                             msg.data = console.green + "Blackboard Data\n" + console.reset
                             msg.data += "{}\n".format(view.sub_blackboard)
                             msg.data += py_trees.display.unicode_blackboard_activity_stream(
-                                activity_stream=view.sub_activity_stream
+                                activity_stream=view.sub_activity_stream.data
                             )
                         else:
                             msg.data = "{}".format(view.sub_blackboard)
                         view.publisher.publish(msg)
 
-        # clear the activity stream
+        # manage the activity stream
         if py_trees.blackboard.Blackboard.activity_stream is not None:
-            py_trees.blackboard.Blackboard.activity_stream.clear()
+            if self.activity_stream_clients == 0:
+                py_trees.blackboard.Blackboard.disable_activity_stream()
+            else:
+                py_trees.blackboard.Blackboard.activity_stream.clear()
+        elif self.activity_stream_clients > 0:
+            py_trees.blackboard.Blackboard.enable_activity_stream()
+
+    def register_activity_stream_client(self):
+        self.activity_stream_clients += 1
+
+    def unregister_activity_stream_client(self):
+        self.activity_stream_clients -= 1
 
-    def _close_watcher_service(self, request, response):
+    def _close_service(self, request, response):
         response.result = False
         for view in self.views:
-            # print("   DJS: close watcher? [%s][%s]" % (view.topic_name, request.topic_name))
             if view.topic_name == request.topic_name:
                 view.shutdown()  # that node.destroy_publisher call makes havoc
                 response.result = True
                 break
         self.views[:] = [view for view in self.views if view.topic_name != request.topic_name]
-        # print("DJS: close result: %s" % response.result)
-        if any([view.with_activity_stream for view in self.views]):
-            py_trees.blackboard.Blackboard.enable_activity_stream()
-        else:
-            py_trees.blackboard.Blackboard.disable_activity_stream()
+        self.unregister_activity_stream_client()
         return response
 
     def _get_variables_service(self, unused_request, response):
         response.variables = self._get_nested_keys()
         return response
 
-    def _open_watcher_service(self, request, response):
+    def _open_service(self, request, response):
         response.topic = rclpy.expand_topic_name.expand_topic_name(
-            topic_name="~/blackboard/_watcher_" + str(Exchange._counter),
+            topic_name="~/blackboard_streams/_watcher_" + str(Exchange._counter),
             node_name=self.node.get_name(),
             node_namespace=self.node.get_namespace())
         Exchange._counter += 1
+        if request.with_activity_stream:
+            self.register_activity_stream_client()
         view = BlackboardView(
             node=self.node,
             topic_name=response.topic,
@@ -406,13 +413,13 @@ def __init__(self, namespace_hint: str=None):
         }
         self.service_type_strings = {
             'list': 'py_trees_ros_interfaces/srv/GetBlackboardVariables',
-            'open': 'py_trees_ros_interfaces/srv/OpenBlackboardWatcher',
-            'close': 'py_trees_ros_interfaces/srv/CloseBlackboardWatcher'
+            'open': 'py_trees_ros_interfaces/srv/OpenBlackboardStream',
+            'close': 'py_trees_ros_interfaces/srv/CloseBlackboardStream'
         }
         self.service_types = {
             'list': py_trees_srvs.GetBlackboardVariables,
-            'open': py_trees_srvs.OpenBlackboardWatcher,
-            'close': py_trees_srvs.CloseBlackboardWatcher
+            'open': py_trees_srvs.OpenBlackboardStream,
+            'close': py_trees_srvs.CloseBlackboardStream
         }
 
     def setup(self, timeout_sec: float):
@@ -474,7 +481,6 @@ def echo_blackboard_contents(self, msg: std_msgs.String):
         Args:
             msg (:class:`std_msgs.String`): incoming blackboard message as a string.
         """
-        # print("DJS: echo_blackboard_contents")
         print("{}".format(msg.data))
 
     def shutdown(self):

py_trees_ros/conversions.py

@@ -20,16 +20,36 @@
 import py_trees
 import py_trees_ros_interfaces.msg  # noqa
 import rclpy
+import typing
 import unique_identifier_msgs.msg
 import uuid
 
-from typing import Any
 
 ##############################################################################
 # <etjpds
 ##############################################################################
 
 
+def activity_stream_to_msgs() -> typing.List[py_trees_ros_interfaces.msg.ActivityItem]:
+    """
+    Convert the blackboard activity stream to a message.
+
+    Returns:
+        A list of activity item messages.
+    """
+    activity_stream = py_trees.blackboard.Blackboard.activity_stream
+    activity_stream_msgs = []
+    for item in activity_stream.data:
+        msg = py_trees_ros_interfaces.msg.ActivityItem()
+        msg.key = item.key
+        msg.client_name = item.client_name
+        msg.activity_type = item.activity_type
+        msg.previous_value = str(item.previous_value)
+        msg.current_value = str(item.current_value)
+        activity_stream_msgs.append(msg)
+    return activity_stream_msgs
+
+
 def behaviour_type_to_msg_constant(behaviour: py_trees.behaviour.Behaviour):
     """
     Convert a behaviour class type to a message constant.
@@ -58,7 +78,7 @@ def behaviour_type_to_msg_constant(behaviour: py_trees.behaviour.Behaviour):
         return py_trees_ros_interfaces.msg.Behaviour.UNKNOWN_TYPE
 
 
-def msg_constant_to_behaviour_type(value: int) -> Any:
+def msg_constant_to_behaviour_type(value: int) -> typing.Any:
     """
     Convert one of the behaviour type constants in a
     :class:`py_trees_ros_interfaces.msg.Behaviour` message to

py_trees_ros/programs/tree_watcher.py

@@ -15,9 +15,8 @@
    :prog: py-trees-tree-watcher
 
 Command line utility to interact with a running
-:class:`~py_trees_ros.trees.BehaviourTree` instance. Print the tree structure
-or a live snapshot of the tree state as unicode art on your console,
-view tick statistics as a stream or display the tree as a dot graph.
+:class:`~py_trees_ros.trees.BehaviourTree` instance. Stream snapshots
+of the tree as unicode art on your console, or display the tree as a dot graph.
 
 .. image:: images/tree-watcher.gif
 
@@ -28,6 +27,7 @@
 ##############################################################################
 
 import argparse
+import py_trees
 import py_trees.console as console
 import py_trees_ros
 import rclpy
@@ -40,12 +40,12 @@
 
 def description(formatted_for_sphinx):
     short = "Open up a window onto the behaviour tree!\n"
-    long = ("\nPrint a single snapshot, or stream the tree state as unicode art on your console\n"
-            "or render the tree as a dot graph (does not include behaviour's status flags).\n"
+    long = ("\nStream the tree state as unicode art on your console\n"
+            "or render the tree as a dot graph.\n"
             "Use the namespace argument to select from trees when there are multiple available.\n"
             )
     examples = [
-        "", "--stream", "--snapshot", "--dot-graph", "--namespace=foo --stream"
+        "", "--stream", "--dot-graph", "--namespace=foo --stream"
     ]
     script_name = "py-trees-tree-watcher"
 
@@ -91,27 +91,22 @@ def command_line_argument_parser(formatted_for_sphinx=True):
                                      formatter_class=argparse.RawDescriptionHelpFormatter,
                                      )
     # common arguments
-    parser.add_argument('-n', '--namespace', nargs='?', default=None, help='namespace of pytree communications (if there should be more than one tree active)')
-    parser.add_argument('-a', '--stream-blackboard-activity', action='store_true', help="show logged activity stream (streaming mode only)")
-    parser.add_argument('-b', '--stream-blackboard-variables', action='store_true', help="show visited path variables (streaming mode only)")
-    parser.add_argument('-s', '--stream-statistics', action='store_true', help="show tick timing statistics (streaming mode only)")
-    # TODO : break these out into different subcommands
+    parser.add_argument('topic_name', nargs='?', default=None, help='snapshot stream to connect to, will create a temporary stream if none specified')
+    parser.add_argument('-n', '--namespace-hint', nargs='?', const=None, default=None, help='namespace hint snapshot stream services (if there should be more than one tree)')
+    parser.add_argument('-a', '--blackboard-activity', action='store_true', help="show logged activity stream (streaming mode only)")
+    parser.add_argument('-b', '--blackboard-data', action='store_true', help="show visited path variables (streaming mode only)")
+    parser.add_argument('-s', '--statistics', action='store_true', help="show tick timing statistics (streaming mode only)")
+    # don't use 'required=True' here since it forces the user to expclitly type out one option
     group = parser.add_mutually_exclusive_group()
     group.add_argument(
-        '--stream',
-        dest='viewing_mode',
+        '--snapshots',
+        dest='mode',
         action='store_const',
-        const=py_trees_ros.trees.WatcherMode.STREAM,
-        help='stream the tree state as unicode art on your console')
-    group.add_argument(
-        '--snapshot',
-        dest='viewing_mode',
-        action='store_const',
-        const=py_trees_ros.trees.WatcherMode.SNAPSHOT,
-        help='print a single snapshot as unicode art on your console')
+        const=py_trees_ros.trees.WatcherMode.SNAPSHOTS,
+        help='render ascii/unicode snapshots from a snapshot stream')
     group.add_argument(
         '--dot-graph',
-        dest='viewing_mode',
+        dest='mode',
         action='store_const',
         const=py_trees_ros.trees.WatcherMode.DOT_GRAPH,
         help='render the tree as a dot graph')
@@ -143,10 +138,6 @@ def echo_blackboard_contents(contents):
     """
     print("{}".format(contents))
 
-##############################################################################
-# Tree Watcher
-##############################################################################
-
 ##############################################################################
 # Main
 ##############################################################################
@@ -159,37 +150,49 @@ def main():
     ####################
     # Arg Parsing
     ####################
+
     # command_line_args = rclpy.utilities.remove_ros_args(command_line_args)[1:]
     command_line_args = None
     parser = command_line_argument_parser(formatted_for_sphinx=False)
     args = parser.parse_args(command_line_args)
-    if not args.viewing_mode:
-        args.viewing_mode = py_trees_ros.trees.WatcherMode.STREAM
+
+    # mode is None if the user didn't specify any option in the exclusive group
+    if args.mode is None:
+        args.mode = py_trees_ros.trees.WatcherMode.SNAPSHOTS
+    args.snapshot_period = 2.0 if (args.statistics or args.blackboard_data or args.blackboard_activity) else py_trees.common.Duration.INFINITE.value
+    tree_watcher = py_trees_ros.trees.Watcher(
+        namespace_hint=args.namespace_hint,
+        topic_name=args.topic_name,
+        parameters=py_trees_ros.trees.SnapshotStream.Parameters(
+            blackboard_data=args.blackboard_data,
+            blackboard_activity=args.blackboard_activity,
+            snapshot_period=args.snapshot_period
+        ),
+        mode=args.mode,
+        statistics=args.statistics,
+    )
 
     ####################
     # Setup
     ####################
-    tree_watcher = py_trees_ros.trees.Watcher(
-        namespace_hint=args.namespace,
-        mode=args.viewing_mode,
-        display_statistics=args.stream_statistics,
-        display_blackboard_variables=args.stream_blackboard_variables,
-        display_blackboard_activity=args.stream_blackboard_activity,
-    )
     rclpy.init(args=None)
     try:
-        tree_watcher.setup()
+        tree_watcher.setup(timeout_sec=1.0)
+    # setup discovery fails
     except py_trees_ros.exceptions.NotFoundError as e:
         print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
         sys.exit(1)
+    # setup discovery finds duplicates
     except py_trees_ros.exceptions.MultipleFoundError as e:
         print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
         if args.namespace is None:
             print(console.red + "\nERROR: select one with the --namespace argument\n" + console.reset)
-            sys.exit(1)
         else:
-            print(console.red + "\nERROR: but none matching the requested '%s'\n" % args.namespace + console.reset)
-            sys.exit(1)
+            print(console.red + "\nERROR: but none matching the requested '{}'\n".format(args.namespace) + console.reset)
+        sys.exit(1)
+    except py_trees_ros.exceptions.TimedOutError as e:
+        print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
+        sys.exit(1)
 
     ####################
     # Execute
@@ -200,14 +203,17 @@ def main():
                 break
             if tree_watcher.done:
                 if tree_watcher.xdot_process is None:
+                    # no xdot found on the system, just break out and finish
                     break
                 elif tree_watcher.xdot_process.poll() is not None:
+                    # xdot running, wait for it to terminate
                     break
             rclpy.spin_once(tree_watcher.node, timeout_sec=0.1)
     except KeyboardInterrupt:
         pass
-    if tree_watcher.xdot_process is not None:
-        if tree_watcher.xdot_process.poll() is not None:
-            tree_watcher.xdot_process.terminate()
-    tree_watcher.node.destroy_node()
-    rclpy.shutdown()
+    finally:
+        if tree_watcher.xdot_process is not None:
+            if tree_watcher.xdot_process.poll() is not None:
+                tree_watcher.xdot_process.terminate()
+        tree_watcher.shutdown()
+        rclpy.shutdown()