Unified cleanup using scoped behaviors

ada_feeding/CMakeLists.txt

@@ -62,13 +62,14 @@ install(DIRECTORY
 if(BUILD_TESTING)
   find_package(ament_cmake_pytest REQUIRED)
   set(_pytest_tests
+    tests/test_eventually_swiss.py
     # Add other test files here
   )
   foreach(_test_path ${_pytest_tests})
     get_filename_component(_test_name ${_test_path} NAME_WE)
     ament_add_pytest_test(${_test_name} ${_test_path}
       APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
-      TIMEOUT 60
+      TIMEOUT 10
       WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
     )
   endforeach()

ada_feeding/ada_feeding/behaviors/move_to.py

@@ -395,7 +395,7 @@ def terminate(self, new_status: py_trees.common.Status) -> None:
         # A termination request has not succeeded until the MoveIt2 action server is IDLE
         with self.moveit2_lock:
             while self.moveit2.query_state() != MoveIt2State.IDLE:
-                self.node.logger.info(
+                self.logger.info(
                     f"MoveTo Update MoveIt2State not Idle {time.time()} {terminate_requested_time} "
                     f"{self.terminate_timeout_s}"
                 )

ada_feeding/ada_feeding/decorators/__init__.py

@@ -22,3 +22,9 @@
 from .set_joint_path_constraint import SetJointPathConstraint
 from .set_position_path_constraint import SetPositionPathConstraint
 from .set_orientation_path_constraint import SetOrientationPathConstraint
+
+# Force Status
+from .force_status import ForceStatus
+
+# On Preempt
+from .on_preempt import OnPreempt

ada_feeding/ada_feeding/decorators/force_status.py

@@ -0,0 +1,68 @@
+"""
+This module defines the ForceStatus decorator, which ignores the child's
+current status and always returns the status specified by the user.
+"""
+
+# Standard imports
+import typing
+
+# Third-party imports
+from py_trees import behaviour, common
+from py_trees.decorators import Decorator
+
+# Local imports
+
+
+class ForceStatus(Decorator):
+    """
+    This decorator ignores the child's current status and always returns
+    the status specified by the user. This can be useful e.g., to force a
+    composite without memory to move on to the next child while still ticking
+    the child of this decorator.
+
+    While this decorator's behavior can be achieved with a chain of "X is Y"
+    style decorators, it is often conceptually easier to reason about a decorator
+    always returning one status, as opposed to a chain of decorators that each
+    flip their child's status.
+    """
+
+    def __init__(self, name: str, child: behaviour.Behaviour, status: common.Status):
+        """
+        Initialise the decorator.
+
+        Args:
+            name: the decorator name
+            child: the child to be decorated
+            status: the status to return on :meth:`update`
+        """
+        super().__init__(name=name, child=child)
+        self.const_status = status
+
+    def tick(self) -> typing.Iterator[behaviour.Behaviour]:
+        """
+        Don't stop the child even if this decorator's status is non-RUNNING.
+
+        Yields:
+            a reference to itself
+        """
+        self.logger.debug(f"{self.__class__.__name__}.tick()")
+        # initialise just like other behaviours/composites
+        if self.status != common.Status.RUNNING:
+            self.initialise()
+        # interrupt proceedings and process the child node
+        # (including any children it may have as well)
+        for node in self.decorated.tick():
+            yield node
+        # resume normal proceedings for a Behaviour's tick
+        self.status = self.update()
+        # do not stop even if this decorator's status is non-RUNNING
+        yield self
+
+    def update(self) -> common.Status:
+        """
+        Return the status specified when creating this decorator.
+
+        Returns:
+            the behaviour's new status :class:`~py_trees.common.Status`
+        """
+        return self.const_status

ada_feeding/ada_feeding/decorators/on_preempt.py

@@ -0,0 +1,109 @@
+"""
+NOTE: This is a multi-tick version of the decorator discussed in
+https://github.com/splintered-reality/py_trees/pull/427 . Once a
+multi-tick version of that decorator is merged into py_trees, this
+decorator should be removed in favor of the main py_trees one.
+"""
+
+import time
+import typing
+
+from py_trees import behaviour, common
+from py_trees.decorators import Decorator
+
+
+class OnPreempt(Decorator):
+    """
+    Trigger the child on preemption, e.g., when :meth:`terminate` is
+    called with status :data:`~py_trees.common.Status.INVALID`. The
+    child can either be triggered: (a) for a single tick; or (b) until it
+    reaches a status other than :data:`~py_trees.common.Status.RUNNING` or
+    times out.
+
+    Always return `update_status` and on :meth:`terminate` (with new_status
+    :data:`~py_trees.common.Status.INVALID`), call the child's
+    :meth:`~py_trees.behaviour.Behaviour.update` method.
+
+    This is useful to cleanup, restore a context switch or to
+    implement a finally-like behaviour.
+
+    .. seealso:: :meth:`py_trees.idioms.eventually`, :meth:`py_trees.idioms.eventually_swiss`
+    """
+
+    # pylint: disable=too-many-arguments
+    # This is acceptable, to give users maximum control over how this decorator
+    # behaves.
+    def __init__(
+        self,
+        name: str,
+        child: behaviour.Behaviour,
+        update_status: common.Status = common.Status.RUNNING,
+        single_tick: bool = True,
+        period_ms: int = 0,
+        timeout: typing.Optional[float] = None,
+    ):
+        """
+        Initialise with the standard decorator arguments.
+
+        Args:
+            name: the decorator name
+            child: the child to be decorated
+            update_status: the status to return on :meth:`update`
+            single_tick: if True, tick the child once on preemption. Else,
+                tick the child until it reaches a status other than
+                :data:`~py_trees.common.Status.RUNNING`.
+            period_ms: how long to sleep between ticks (in milliseconds)
+                if `single_tick` is False. If 0, then do not sleep.
+            timeout: how long (sec) to wait for the child to reach a status
+                other than :data:`~py_trees.common.Status.RUNNING` if
+                `single_tick` is False. If None, then do not timeout.
+        """
+        super().__init__(name=name, child=child)
+        self.update_status = update_status
+        self.single_tick = single_tick
+        self.period_ms = period_ms
+        self.timeout = timeout
+
+    def tick(self) -> typing.Iterator[behaviour.Behaviour]:
+        """
+        Bypass the child when ticking.
+
+        Yields:
+            a reference to itself
+        """
+        self.logger.debug(f"{self.__class__.__name__}.tick()")
+        self.status = self.update()
+        yield self
+
+    def update(self) -> common.Status:
+        """
+        Return the constant status specified in the constructor.
+
+        Returns:
+            the behaviour's new status :class:`~py_trees.common.Status`
+        """
+        return self.update_status
+
+    def terminate(self, new_status: common.Status) -> None:
+        """Tick the child behaviour once."""
+        self.logger.debug(
+            f"{self.__class__.__name__}.terminate({self.status}->{new_status})"
+            if self.status != new_status
+            else f"{new_status}",
+        )
+        if new_status == common.Status.INVALID:
+            terminate_start_s = time.time()
+            # Tick the child once
+            self.decorated.tick_once()
+            # If specified, tick until the child reaches a non-RUNNING status
+            if not self.single_tick:
+                while self.decorated.status == common.Status.RUNNING and (
+                    self.timeout is None
+                    or time.time() - terminate_start_s < self.timeout
+                ):
+                    if self.period_ms > 0:
+                        time.sleep(self.period_ms / 1000.0)
+                    self.decorated.tick_once()
+            # Do not need to stop the child here - this method
+            # is only called by Decorator.stop() which will handle
+            # that responsibility immediately after this method returns.

ada_feeding/ada_feeding/idioms/__init__.py

@@ -3,5 +3,7 @@
 project.
 """
 from .add_pose_path_constraints import add_pose_path_constraints
+from .eventually_swiss import eventually_swiss
 from .pre_moveto_config import pre_moveto_config
 from .retry_call_ros_service import retry_call_ros_service
+from .scoped_behavior import scoped_behavior

ada_feeding/ada_feeding/idioms/eventually_swiss.py

@@ -0,0 +1,166 @@
+"""
+NOTE: This is a preempt-handling version of the idiom discussed in
+https://github.com/splintered-reality/py_trees/pull/427 . Once a
+preempt-handling version of that idiom is merged into py_trees, this
+idiom should be removed in favor of the main py_trees one.
+"""
+
+import typing
+
+from py_trees import behaviour, behaviours, common, composites
+from py_trees.decorators import Inverter, StatusToBlackboard, SuccessIsFailure
+
+from ada_feeding.decorators import ForceStatus, OnPreempt
+
+
+def eventually_swiss(
+    name: str,
+    workers: typing.List[behaviour.Behaviour],
+    on_failure: behaviour.Behaviour,
+    on_success: behaviour.Behaviour,
+    on_preempt: behaviour.Behaviour,
+    on_preempt_single_tick: bool = True,
+    on_preempt_period_ms: int = 0,
+    on_preempt_timeout: typing.Optional[float] = None,
+    status_blackboard_key: typing.Optional[str] = None,
+) -> behaviour.Behaviour:
+    """
+    Implement a multi-tick, general purpose 'try-except-else'-like pattern.
+
+    This is a swiss knife version of the eventually idiom
+    that facilitates a multi-tick response for specialised
+    handling work sequence's completion status. Specifically, this idiom
+    guarentees the following:
+    1. The on_success behaviour is ticked only if the workers all return SUCCESS.
+    2. The on_failure behaviour is ticked only if at least one worker returns FAILURE.
+    3. The on_preempt behaviour is ticked only if `stop(INVALID)` is called on the
+       root behavior returned from this idiom.
+
+    .. graphviz:: dot/eventually-swiss.dot
+
+    Args:
+        name: the name to use for the idiom root
+        workers: the worker behaviours or subtrees
+        on_success: the behaviour or subtree to tick on work success
+        on_failure: the behaviour or subtree to tick on work failure
+        on_preempt: the behaviour or subtree to tick on work preemption
+        on_preempt_single_tick: if True, tick the on_preempt behaviour once
+            on preemption. Else, tick the on_preempt behaviour until it
+            reaches a status other than :data:`~py_trees.common.Status.RUNNING`.
+        on_preempt_period_ms: how long to sleep between ticks (in milliseconds)
+            if `on_preempt_single_tick` is False. If 0, then do not sleep.
+        on_preempt_timeout: how long (sec) to wait for the on_preempt behaviour
+            to reach a status other than :data:`~py_trees.common.Status.RUNNING`
+            if `on_preempt_single_tick` is False. If None, then do not timeout.
+        status_blackboard_key: the key to use for the status blackboard variable.
+            If None, use "/{name}/eventually_swiss_status".
+
+    Returns:
+        :class:`~py_trees.behaviour.Behaviour`: the root of the oneshot subtree
+
+    .. seealso:: :meth:`py_trees.idioms.eventually`, :ref:`py-trees-demo-eventually-swiss-program`
+    """
+    # pylint: disable=too-many-arguments, too-many-locals
+    # This is acceptable, to give users maximum control over how the swiss knife
+    # idiom behaves.
+
+    # Create the subtree to handle `on_success`
+    if status_blackboard_key is None:
+        status_blackboard_key = f"/{name}/eventually_swiss_status"
+    unset_status = behaviours.UnsetBlackboardVariable(
+        name="Unset Status", key=status_blackboard_key
+    )
+    save_success_status = StatusToBlackboard(
+        name="Save Success Status",
+        child=on_success,
+        variable_name=status_blackboard_key,
+    )
+    on_success_sequence = composites.Sequence(
+        name="Work to Success",
+        memory=True,
+        children=[unset_status] + workers + [save_success_status],
+    )
+
+    # Create the subtree to handle `on_failure`. This subtree must start with
+    # `return_status_if_set` so that `on_failure` is not ticked if `on_success`
+    # fails.
+    check_status_exists = behaviours.CheckBlackboardVariableExists(
+        name="Wait for Status",
+        variable_name=status_blackboard_key,
+    )
+    # Note that we can get away with using an Inverter here because the only way
+    # we get to this branch is if either the `workers` or `on_success` fails.
+    # So the status either doesn't exist or is FAILURE.
+    return_status_if_set = Inverter(
+        name="Return Status if Set",
+        child=check_status_exists,
+    )
+    on_failure_always_fail = SuccessIsFailure(
+        name="On Failure Always Failure",
+        child=on_failure,
+    )
+    save_failure_status = StatusToBlackboard(
+        name="Save Failure Status",
+        child=on_failure_always_fail,
+        variable_name=status_blackboard_key,
+    )
+    on_failure_sequence = composites.Sequence(
+        name="On Failure",
+        memory=True,
+        children=[return_status_if_set, save_failure_status],
+    )
+
+    # Create the combined subtree to handle `on_success` and `on_failure`
+    combined = composites.Selector(
+        name="On Non-Preemption Subtree",
+        memory=True,
+        children=[on_success_sequence, on_failure_sequence],
+    )
+    # We force the outcome of this tree to FAILURE so that the Selector always
+    # goes on to tick the `on_preempt_subtree`, which is necessary to ensure
+    # that the `on_preempt` behavior will get run if the tree is preempted.
+    on_success_or_failure_subtree = ForceStatus(
+        name="On Non-Preemption",
+        child=combined,
+        status=common.Status.FAILURE,
+    )
+
+    # Create the subtree to handle `on_preempt`
+    on_preempt_subtree = OnPreempt(
+        name="On Preemption",
+        child=on_preempt,
+        # Returning FAILURE is necessary so (a) the Selector always moves on to
+        # `set_status_subtree`; and (b) the decorator's status is not INVALID (which would
+        # prevent the Selector from passing on a `stop(INVALID)` call to it).
+        update_status=common.Status.FAILURE,
+        single_tick=on_preempt_single_tick,
+        period_ms=on_preempt_period_ms,
+        timeout=on_preempt_timeout,
+    )
+
+    # Create the subtree to output the status once `on_success_or_failure_subtree`
+    # is done.
+    wait_for_status = behaviours.WaitForBlackboardVariable(
+        name="Wait for Status",
+        variable_name=status_blackboard_key,
+    )
+    blackboard_to_status = behaviours.BlackboardToStatus(
+        name="Blackboard to Status",
+        variable_name=status_blackboard_key,
+    )
+    set_status_subtree = composites.Sequence(
+        name="Set Status",
+        memory=True,
+        children=[wait_for_status, blackboard_to_status],
+    )
+
+    root = composites.Selector(
+        name=name,
+        memory=False,
+        children=[
+            on_success_or_failure_subtree,
+            on_preempt_subtree,
+            set_status_subtree,
+        ],
+    )
+    return root