Unified cleanup using scoped behaviors

ada_feeding/CMakeLists.txt

@@ -62,13 +62,17 @@ install(DIRECTORY
 if(BUILD_TESTING)
   find_package(ament_cmake_pytest REQUIRED)
   set(_pytest_tests
+    tests/__init__.py # not technically a test, but necessary for other tests
+    tests/helpers.py # not technically a test, but necessary for other tests
+    tests/test_eventually_swiss.py
+    tests/test_scoped_behavior.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,6 @@
 from .set_joint_path_constraint import SetJointPathConstraint
 from .set_position_path_constraint import SetPositionPathConstraint
 from .set_orientation_path_constraint import SetOrientationPathConstraint
+
+# On Preempt
+from .on_preempt import OnPreempt

ada_feeding/ada_feeding/decorators/on_preempt.py

@@ -0,0 +1,115 @@
+"""
+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):
+    """
+    Behaves identically to :class:`~py_trees.decorators.PassThrough` except
+    that if it gets preempted (i.e., `terminate(INVALID)` is called on it)
+    while its status is :data:`~py_trees.common.Status.RUNNING`, it will
+    tick `on_preempt` either: (a) for a single tick; or (b) until `on_preempt`
+    reaches a status other than :data:`~py_trees.common.Status.RUNNING` or
+    times out. Note that `on_preempt` may be a behavior that exists elsewhere
+    in the tree, or it may be a separate behavior.
+
+    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,
+        on_preempt: behaviour.Behaviour,
+        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
+            on_preempt: the behaviour or subtree to tick on preemption
+            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.on_preempt = on_preempt
+        self.single_tick = single_tick
+        self.period_ms = period_ms
+        self.timeout = timeout
+
+    def update(self) -> common.Status:
+        """
+        Just reflect the child status.
+
+        Returns:
+            the behaviour's new status :class:`~py_trees.common.Status`
+        """
+        return self.decorated.status
+
+    def stop(self, new_status: common.Status) -> None:
+        """
+        Check if the child is running (dangling) and stop it if that is the case.
+
+        This function departs from the standard :meth:`~py_trees.decorators.Decorator.stop`
+        in that it *first* stops the child, and *then* stops the decorator.
+
+        Args:
+            new_status (:class:`~py_trees.common.Status`): the behaviour is transitioning
+            to this new status
+        """
+        self.logger.debug(f"{self.__class__.__name__}.stop({new_status})")
+        # priority interrupt handling
+        if new_status == common.Status.INVALID:
+            self.decorated.stop(new_status)
+        # if the decorator returns SUCCESS/FAILURE and should stop the child
+        if self.decorated.status == common.Status.RUNNING:
+            self.decorated.stop(common.Status.INVALID)
+        self.terminate(new_status)
+        self.status = new_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 new_status == common.Status.INVALID and self.status == common.Status.RUNNING:
+            terminate_start_s = time.monotonic()
+            # Tick the child once
+            self.on_preempt.tick_once()
+            # If specified, tick until the child reaches a non-RUNNING status
+            if not self.single_tick:
+                while self.on_preempt.status == common.Status.RUNNING and (
+                    self.timeout is None
+                    or time.monotonic() - terminate_start_s < self.timeout
+                ):
+                    if self.period_ms > 0:
+                        time.sleep(self.period_ms / 1000.0)
+                    self.on_preempt.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,114 @@
+"""
+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, composites
+
+from ada_feeding.decorators import 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,
+    return_on_success_status: bool = True,
+) -> 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 behaviour returned from this idiom while the root behaviour's status is
+       :data:`~py_trees.common.Status.RUNNING`.
+
+    The return status of this idiom in non-preemption cases is:
+    - If the workers all return SUCCESS:
+        - If `return_on_success_status` is True, then the status of the root behaviour
+          returned from this idiom is status of `on_success`.
+        - If `return_on_success_status` is False, then the status of the root behaviour
+          returned from this idiom is :data:`~py_trees.common.Status.SUCCESS`.
+    - If at least one worker returns FAILURE, return :data:`~py_trees.common.Status.FAILURE`.
+
+    .. 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.
+        return_on_success_status: if True, pass the `on_success` status to the
+            root, else return :data:`~py_trees.common.Status.SUCCESS`.
+
+    Returns:
+        :class:`~py_trees.behaviour.Behaviour`: the root of the eventually_swiss 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 this swiss-knife
+    # idiom behaves.
+    # pylint: disable=abstract-class-instantiated
+    # behaviours.Failure and behaviours.Success are valid instantiations
+
+    workers_sequence = composites.Sequence(
+        name="Workers",
+        memory=True,
+        children=workers,
+    )
+    on_failure_return_status = composites.Sequence(
+        name="On Failure Return Failure",
+        memory=True,
+        children=[on_failure, behaviours.Failure(name="Failure")],
+    )
+    on_failure_subtree = composites.Selector(
+        name="On Failure",
+        memory=True,
+        children=[workers_sequence, on_failure_return_status],
+    )
+    if return_on_success_status:
+        on_success_return_status = on_success
+    else:
+        on_success_return_status = composites.Selector(
+            name="On Success Return Success",
+            memory=True,
+            children=[on_success, behaviours.Success(name="Success")],
+        )
+    on_success_subtree = composites.Sequence(
+        name="On Success",
+        memory=True,
+        children=[on_failure_subtree, on_success_return_status],
+    )
+    root = OnPreempt(
+        name=name,
+        child=on_success_subtree,
+        on_preempt=on_preempt,
+        single_tick=on_preempt_single_tick,
+        period_ms=on_preempt_period_ms,
+        timeout=on_preempt_timeout,
+    )
+
+    return root