Merge pull request #47 from tangkong/enh_pytrees_behaviors

ENH: Add py_trees.behaviours Behaviours, build a functional `beams run` command

beams/bin/main.py

@@ -7,7 +7,7 @@
 import logging
 
 import beams
-from beams.logging import setup_logging
+from beams.logging import configure_log_directory, setup_logging
 
 DESCRIPTION = __doc__
 
@@ -70,6 +70,12 @@ def main():
         type=str,
         help='Python logging level (e.g. DEBUG, INFO, WARNING)'
     )
+    top_parser.add_argument(
+        "--log-dir", dest="log_dir",
+        type=str,
+        default="",
+        help="directory to create log files.  If not provided, do not log to file."
+    )
 
     subparsers = top_parser.add_subparsers(help='Possible subcommands')
     for command_name, (build_func, main) in COMMANDS.items():
@@ -81,6 +87,9 @@ def main():
     kwargs = vars(args)
     log_level = kwargs.pop('log_level')
 
+    log_dir = kwargs.pop("log_dir")
+    if log_dir:
+        configure_log_directory(log_dir)
     setup_logging(log_level)
     logger = logging.getLogger("beams")
 

beams/bin/run.py

@@ -25,6 +25,37 @@ def build_arg_parser(argparser=None):
         type=str,
         help="Behavior Tree configuration filepath"
     )
+    argparser.add_argument(
+        "-i", "--interactive",
+        action="store_true", default=False,
+        help="pause and wait for keypress at each tick",
+    )
+    argparser.add_argument(
+        "-t", "--tick-count",
+        dest="tick_count", default=1, type=int,
+        help="How many times to tick the tree. Values <=0 mean continuous ticking "
+             "(Ctrl+C to terminate tree)"
+    )
+    argparser.add_argument(
+        "-d", "--tick-delay",
+        dest="tick_delay", default=0.5, type=float,
+        help="Delay time (s) between each tick.  Ignored if interactive mode is enabled"
+    )
+    argparser.add_argument(
+        "--hide-node-status",
+        action="store_false", dest="show_node_status", default=True,
+        help="Hide individual node status output"
+    )
+    argparser.add_argument(
+        "--hide-tree",
+        action="store_false", dest="show_tree", default=True,
+        help="Hide tree status summary after each tree tick"
+    )
+    argparser.add_argument(
+        "--show-blackboard",
+        action="store_true", dest="show_blackboard", default=False,
+        help="Show blackboard status after each tree tick"
+    )
 
 
 def main(*args, **kwargs):

beams/bin/run_main.py

@@ -3,24 +3,94 @@
 """
 
 import logging
+import time
+from functools import partial
 from pathlib import Path
 
+from py_trees.console import read_single_keypress
+from py_trees.display import unicode_blackboard, unicode_tree
+from py_trees.trees import BehaviourTree
+from py_trees.visitors import SnapshotVisitor
+
+from beams.logging import LoggingVisitor
 from beams.tree_config import get_tree_from_path
 
 logger = logging.getLogger(__name__)
 
 
-def main(filepath: str):
+def snapshot_post_tick_handler(
+    snapshot_visitor: SnapshotVisitor,
+    show_tree: bool,
+    show_blackboard: bool,
+    behaviour_tree: BehaviourTree,
+) -> None:
+    """
+    Print data about the part of the tree visited.
+    Does not log, to allow use of color codes.
+    ``snapshot_visitor`` keeps track of visited nodes and their status
+
+    Args:
+        snapshot_handler: gather data about the part of the tree visited
+        behaviour_tree: tree to gather data from
+    """
+    if show_tree:
+        print(
+            "\n"
+            + unicode_tree(
+                root=behaviour_tree.root,
+                visited=snapshot_visitor.visited,
+                previously_visited=snapshot_visitor.previously_visited,
+                show_status=True
+            )
+        )
+
+    if show_blackboard:
+        print(unicode_blackboard())
+
+
+def tick_tree(tree: BehaviourTree, interactive: bool, tick_delay: float):
+    tree.tick()
+    if interactive:
+        read_single_keypress()
+    else:
+        time.sleep(tick_delay)
+
+
+def main(
+    filepath: str,
+    tick_count: int,
+    tick_delay: float,
+    interactive: bool,
+    show_node_status: bool,
+    show_tree: bool,
+    show_blackboard: bool,
+):
     logger.info(f"Running behavior tree at {filepath}")
     # grab config
     fp = Path(filepath).resolve()
     if not fp.is_file():
         raise ValueError("Provided filepath is not a file")
 
     tree = get_tree_from_path(fp)
-    print(tree)
-    # TODO: the rest of whatever we determine the "run" process to be
-    # run external server?
-    # setup tree?
-    # tick?
-    # settings for ticking? (continuous tick?, as separate process?)
+    tree.visitors.append(LoggingVisitor(print_status=show_node_status))
+
+    snapshot_visitor = SnapshotVisitor()
+    tree.add_post_tick_handler(
+        partial(snapshot_post_tick_handler,
+                snapshot_visitor,
+                show_tree,
+                show_blackboard)
+    )
+    tree.setup()
+    if tick_count <= 0:
+        while True:
+            try:
+                tick_tree(tree, interactive, tick_delay)
+            except KeyboardInterrupt:
+                break
+    else:
+        for _ in range(tick_count):
+            try:
+                tick_tree(tree, interactive, tick_delay)
+            except KeyboardInterrupt:
+                break

beams/logging.py

@@ -14,6 +14,8 @@
 from typing import Optional, Union
 
 import yaml
+from py_trees.behaviour import Behaviour
+from py_trees.visitors import VisitorBase
 
 LOGGER_QUEUE = mp.Queue(-1)
 LOGGER_THREAD: Optional[threading.Thread] = None
@@ -159,3 +161,34 @@ def setup_logging(level: int = logging.INFO):
         target=logger_thread, args=(LOGGER_QUEUE,), daemon=True
     )
     LOGGER_THREAD.start()
+
+
+class LoggingVisitor(VisitorBase):
+    """
+    logs feedback messages and behaviour status
+
+    Uses the beams logger rather than the py_trees logger
+    """
+
+    def __init__(self, print_status: bool = False):
+        self.print_status = print_status
+        super().__init__(full=False)
+        stream_handler = [h for h in logging.getLogger("beams").handlers
+                          if h.name == "console"][0]
+        self.stream_handler_level = stream_handler.level or logging.DEBUG
+
+    def run(self, behaviour: Behaviour) -> None:
+        """
+        Write node status to logging stream.
+        If print_status is requested and the console logger won't display, also
+        print to console
+        """
+        out_msg = f"{behaviour.__class__.__name__}.run() [{behaviour.status}]"
+        if behaviour.feedback_message:
+            logger.debug(out_msg + f": [{behaviour.feedback_message}]")
+            if self.print_status and (self.stream_handler_level > logging.DEBUG):
+                print(out_msg + f": [{behaviour.feedback_message}]")
+        else:
+            logger.debug(out_msg)
+            if self.print_status and (self.stream_handler_level > logging.DEBUG):
+                print(out_msg)

beams/tests/artifacts/eggs.json

@@ -11,19 +11,17 @@
         "operator": "ge"
       },
       "do": {
-        "IncPVActionItem": {
-          "name": "self_test_do",
+        "name": "self_test_do",
+        "description": "",
+        "pv": "PERC:COMP",
+        "increment": 10,
+        "loop_period_sec": 0.01,
+        "termination_check": {
+          "name": "",
           "description": "",
-          "loop_period_sec": 0.01,
           "pv": "PERC:COMP",
-          "increment": 10,
-          "termination_check": {
-            "name": "",
-            "description": "",
-            "pv": "PERC:COMP",
-            "value": 100,
-            "operator": "ge"
-          }
+          "value": 100,
+          "operator": "ge"
         }
       }
     }

beams/tests/artifacts/eggs2.json

@@ -17,19 +17,17 @@
               "operator": "ge"
             },
             "do": {
-              "SetPVActionItem": {
-                "name": "ret_find_do",
+              "name": "ret_find_do",
+              "description": "",
+              "pv": "RET:FOUND",
+              "value": 1,
+              "loop_period_sec": 0.01,
+              "termination_check": {
+                "name": "",
                 "description": "",
-                "loop_period_sec": 0.01,
                 "pv": "RET:FOUND",
                 "value": 1,
-                "termination_check": {
-                  "name": "",
-                  "description": "",
-                  "pv": "RET:FOUND",
-                  "value": 1,
-                  "operator": "ge"
-                }
+                "operator": "ge"
               }
             }
           }
@@ -46,19 +44,17 @@
               "operator": "ge"
             },
             "do": {
-              "SetPVActionItem": {
+              "name": "",
+              "description": "",
+              "pv": "RET:INSERT",
+              "value": 1,
+              "loop_period_sec": 1.0,
+              "termination_check": {
                 "name": "",
                 "description": "",
-                "loop_period_sec": 1.0,
                 "pv": "RET:INSERT",
                 "value": 1,
-                "termination_check": {
-                  "name": "",
-                  "description": "",
-                  "pv": "RET:INSERT",
-                  "value": 1,
-                  "operator": "ge"
-                }
+                "operator": "ge"
               }
             }
           }

beams/tests/artifacts/eternal_guard.json

@@ -0,0 +1,56 @@
+{
+  "root": {
+    "SequenceItem": {
+      "name": "Eternal Guard",
+      "description": "",
+      "memory": false,
+      "children": [
+        {
+          "StatusQueueItem": {
+            "name": "Condition 1",
+            "description": "",
+            "queue": [
+              "SUCCESS",
+              "FAILURE",
+              "SUCCESS"
+            ],
+            "eventually": "SUCCESS"
+          }
+        },
+        {
+          "StatusQueueItem": {
+            "name": "Condition 2",
+            "description": "",
+            "queue": [
+              "SUCCESS",
+              "SUCCESS",
+              "FAILURE"
+            ],
+            "eventually": "SUCCESS"
+          }
+        },
+        {
+          "SequenceItem": {
+            "name": "Task Sequence",
+            "description": "",
+            "memory": true,
+            "children": [
+              {
+                "SuccessItem": {
+                  "name": "Worker 1",
+                  "description": ""
+                }
+              },
+              {
+                "RunningItem": {
+                  "name": "Worker 2",
+                  "description": ""
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  }
+}

beams/tests/conftest.py

@@ -1,4 +1,7 @@
 import logging
+import sys
+from contextlib import contextmanager
+from copy import copy
 
 import py_trees.logging
 import pytest
@@ -16,3 +19,26 @@ def central_logging_setup(caplog):
     setup_logging(logging.DEBUG)
     # Set py_trees logging level (not our library)
     py_trees.logging.level = py_trees.logging.Level.DEBUG
+
+
+@contextmanager
+def cli_args(args):
+    """
+    Context manager for running a block of code with a specific set of
+    command-line arguments.
+    """
+    prev_args = sys.argv
+    sys.argv = args
+    yield
+    sys.argv = prev_args
+
+
+@contextmanager
+def restore_logging():
+    """
+    Context manager for reverting our logging config after testing a function
+    that configures the logging.
+    """
+    prev_handlers = copy(logging.root.handlers)
+    yield
+    logging.root.handlers = prev_handlers