Skip to content

Commit a53308f

Browse files
amalnanavatiamalnanavati
authored
Unified cleanup using scoped behaviors (#103)
* Moved in decorator and idiom from py_trees#427 * Make OnPreempt decorator multi-tick, add on_preempt function to eventually_swiss * Integrate the code into this library * Implemented scoped behavior idiom * Added scoped behavior to tree for concise cleanup * Added unit tests for `eventually_swiss` Run: `colcon test --packages-select ada_feeding; colcon test-result --all --verbose` The current implementation fails on preemption order, and because `on_failure` runs if `on_success` fails * Updated eventually_swiss and verified that it passes tests * MoveTo bug fix * Updated test because we don't care about relative termination order between on_failure and on_success * Update tests so that after worker/on_success/on_failure terminate, on_preempt will no longer be called * Simplified eventually_swiss implementation * Generalized `eventually_swiss` return status, started unit tests for `scoped_behavior` * Completed scoped_behavior tests, simplified unit test generation * Removed option to return on_failure status from eventually_swiss * Updated scoped_behavior to only use one post behavior * Updated tests to preempt before the tree has started * Added nested test cases to scoped_behavior
1 parent 74dab3a commit a53308f

15 files changed

+1842
-96
lines changed

‎ada_feeding/CMakeLists.txt‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,13 +62,17 @@ install(DIRECTORY
6262
if(BUILD_TESTING)
6363
find_package(ament_cmake_pytest REQUIRED)
6464
set(_pytest_tests
65+
tests/__init__.py # not technically a test, but necessary for other tests
66+
tests/helpers.py # not technically a test, but necessary for other tests
67+
tests/test_eventually_swiss.py
68+
tests/test_scoped_behavior.py
6569
# Add other test files here
6670
)
6771
foreach(_test_path ${_pytest_tests})
6872
get_filename_component(_test_name ${_test_path} NAME_WE)
6973
ament_add_pytest_test(${_test_name} ${_test_path}
7074
APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
71-
TIMEOUT 60
75+
TIMEOUT 10
7276
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
7377
)
7478
endforeach()

‎ada_feeding/ada_feeding/behaviors/move_to.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -395,7 +395,7 @@ def terminate(self, new_status: py_trees.common.Status) -> None:
395395
# A termination request has not succeeded until the MoveIt2 action server is IDLE
396396
with self.moveit2_lock:
397397
while self.moveit2.query_state() != MoveIt2State.IDLE:
398-
self.node.logger.info(
398+
self.logger.info(
399399
f"MoveTo Update MoveIt2State not Idle {time.time()} {terminate_requested_time} "
400400
f"{self.terminate_timeout_s}"
401401
)

‎ada_feeding/ada_feeding/decorators/__init__.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,3 +22,6 @@
2222
from .set_joint_path_constraint import SetJointPathConstraint
2323
from .set_position_path_constraint import SetPositionPathConstraint
2424
from .set_orientation_path_constraint import SetOrientationPathConstraint
25+
26+
# On Preempt
27+
from .on_preempt import OnPreempt

‎ada_feeding/ada_feeding/decorators/on_preempt.py‎

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
"""
2+
NOTE: This is a multi-tick version of the decorator discussed in
3+
https://github.com/splintered-reality/py_trees/pull/427 . Once a
4+
multi-tick version of that decorator is merged into py_trees, this
5+
decorator should be removed in favor of the main py_trees one.
6+
"""
7+
8+
import time
9+
import typing
10+
11+
from py_trees import behaviour, common
12+
from py_trees.decorators import Decorator
13+
14+
15+
class OnPreempt(Decorator):
16+
"""
17+
Behaves identically to :class:`~py_trees.decorators.PassThrough` except
18+
that if it gets preempted (i.e., `terminate(INVALID)` is called on it)
19+
while its status is :data:`~py_trees.common.Status.RUNNING`, it will
20+
tick `on_preempt` either: (a) for a single tick; or (b) until `on_preempt`
21+
reaches a status other than :data:`~py_trees.common.Status.RUNNING` or
22+
times out. Note that `on_preempt` may be a behavior that exists elsewhere
23+
in the tree, or it may be a separate behavior.
24+
25+
This is useful to cleanup, restore a context switch or to
26+
implement a finally-like behaviour.
27+
28+
.. seealso:: :meth:`py_trees.idioms.eventually`, :meth:`py_trees.idioms.eventually_swiss`
29+
"""
30+
31+
# pylint: disable=too-many-arguments
32+
# This is acceptable, to give users maximum control over how this decorator
33+
# behaves.
34+
def __init__(
35+
self,
36+
name: str,
37+
child: behaviour.Behaviour,
38+
on_preempt: behaviour.Behaviour,
39+
single_tick: bool = True,
40+
period_ms: int = 0,
41+
timeout: typing.Optional[float] = None,
42+
):
43+
"""
44+
Initialise with the standard decorator arguments.
45+
46+
Args:
47+
name: the decorator name
48+
child: the child to be decorated
49+
on_preempt: the behaviour or subtree to tick on preemption
50+
single_tick: if True, tick the child once on preemption. Else,
51+
tick the child until it reaches a status other than
52+
:data:`~py_trees.common.Status.RUNNING`.
53+
period_ms: how long to sleep between ticks (in milliseconds)
54+
if `single_tick` is False. If 0, then do not sleep.
55+
timeout: how long (sec) to wait for the child to reach a status
56+
other than :data:`~py_trees.common.Status.RUNNING` if
57+
`single_tick` is False. If None, then do not timeout.
58+
"""
59+
super().__init__(name=name, child=child)
60+
self.on_preempt = on_preempt
61+
self.single_tick = single_tick
62+
self.period_ms = period_ms
63+
self.timeout = timeout
64+
65+
def update(self) -> common.Status:
66+
"""
67+
Just reflect the child status.
68+
69+
Returns:
70+
the behaviour's new status :class:`~py_trees.common.Status`
71+
"""
72+
return self.decorated.status
73+
74+
def stop(self, new_status: common.Status) -> None:
75+
"""
76+
Check if the child is running (dangling) and stop it if that is the case.
77+
78+
This function departs from the standard :meth:`~py_trees.decorators.Decorator.stop`
79+
in that it *first* stops the child, and *then* stops the decorator.
80+
81+
Args:
82+
new_status (:class:`~py_trees.common.Status`): the behaviour is transitioning
83+
to this new status
84+
"""
85+
self.logger.debug(f"{self.__class__.__name__}.stop({new_status})")
86+
# priority interrupt handling
87+
if new_status == common.Status.INVALID:
88+
self.decorated.stop(new_status)
89+
# if the decorator returns SUCCESS/FAILURE and should stop the child
90+
if self.decorated.status == common.Status.RUNNING:
91+
self.decorated.stop(common.Status.INVALID)
92+
self.terminate(new_status)
93+
self.status = new_status
94+
95+
def terminate(self, new_status: common.Status) -> None:
96+
"""Tick the child behaviour once."""
97+
self.logger.debug(
98+
f"{self.__class__.__name__}.terminate({self.status}->{new_status})"
99+
)
100+
if new_status == common.Status.INVALID and self.status == common.Status.RUNNING:
101+
terminate_start_s = time.monotonic()
102+
# Tick the child once
103+
self.on_preempt.tick_once()
104+
# If specified, tick until the child reaches a non-RUNNING status
105+
if not self.single_tick:
106+
while self.on_preempt.status == common.Status.RUNNING and (
107+
self.timeout is None
108+
or time.monotonic() - terminate_start_s < self.timeout
109+
):
110+
if self.period_ms > 0:
111+
time.sleep(self.period_ms / 1000.0)
112+
self.on_preempt.tick_once()
113+
# Do not need to stop the child here - this method
114+
# is only called by Decorator.stop() which will handle
115+
# that responsibility immediately after this method returns.

‎ada_feeding/ada_feeding/idioms/__init__.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,5 +3,7 @@
33
project.
44
"""
55
from .add_pose_path_constraints import add_pose_path_constraints
6+
from .eventually_swiss import eventually_swiss
67
from .pre_moveto_config import pre_moveto_config
78
from .retry_call_ros_service import retry_call_ros_service
9+
from .scoped_behavior import scoped_behavior

‎ada_feeding/ada_feeding/idioms/eventually_swiss.py‎

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
"""
2+
NOTE: This is a preempt-handling version of the idiom discussed in
3+
https://github.com/splintered-reality/py_trees/pull/427 . Once a
4+
preempt-handling version of that idiom is merged into py_trees, this
5+
idiom should be removed in favor of the main py_trees one.
6+
"""
7+
8+
import typing
9+
10+
from py_trees import behaviour, behaviours, composites
11+
12+
from ada_feeding.decorators import OnPreempt
13+
14+
15+
def eventually_swiss(
16+
name: str,
17+
workers: typing.List[behaviour.Behaviour],
18+
on_failure: behaviour.Behaviour,
19+
on_success: behaviour.Behaviour,
20+
on_preempt: behaviour.Behaviour,
21+
on_preempt_single_tick: bool = True,
22+
on_preempt_period_ms: int = 0,
23+
on_preempt_timeout: typing.Optional[float] = None,
24+
return_on_success_status: bool = True,
25+
) -> behaviour.Behaviour:
26+
"""
27+
Implement a multi-tick, general purpose 'try-except-else'-like pattern.
28+
29+
This is a swiss knife version of the eventually idiom
30+
that facilitates a multi-tick response for specialised
31+
handling work sequence's completion status. Specifically, this idiom
32+
guarentees the following:
33+
1. The on_success behaviour is ticked only if the workers all return SUCCESS.
34+
2. The on_failure behaviour is ticked only if at least one worker returns FAILURE.
35+
3. The on_preempt behaviour is ticked only if `stop(INVALID)` is called on the
36+
root behaviour returned from this idiom while the root behaviour's status is
37+
:data:`~py_trees.common.Status.RUNNING`.
38+
39+
The return status of this idiom in non-preemption cases is:
40+
- If the workers all return SUCCESS:
41+
- If `return_on_success_status` is True, then the status of the root behaviour
42+
returned from this idiom is status of `on_success`.
43+
- If `return_on_success_status` is False, then the status of the root behaviour
44+
returned from this idiom is :data:`~py_trees.common.Status.SUCCESS`.
45+
- If at least one worker returns FAILURE, return :data:`~py_trees.common.Status.FAILURE`.
46+
47+
.. graphviz:: dot/eventually-swiss.dot
48+
49+
Args:
50+
name: the name to use for the idiom root
51+
workers: the worker behaviours or subtrees
52+
on_success: the behaviour or subtree to tick on work success
53+
on_failure: the behaviour or subtree to tick on work failure
54+
on_preempt: the behaviour or subtree to tick on work preemption
55+
on_preempt_single_tick: if True, tick the on_preempt behaviour once
56+
on preemption. Else, tick the on_preempt behaviour until it
57+
reaches a status other than :data:`~py_trees.common.Status.RUNNING`.
58+
on_preempt_period_ms: how long to sleep between ticks (in milliseconds)
59+
if `on_preempt_single_tick` is False. If 0, then do not sleep.
60+
on_preempt_timeout: how long (sec) to wait for the on_preempt behaviour
61+
to reach a status other than :data:`~py_trees.common.Status.RUNNING`
62+
if `on_preempt_single_tick` is False. If None, then do not timeout.
63+
return_on_success_status: if True, pass the `on_success` status to the
64+
root, else return :data:`~py_trees.common.Status.SUCCESS`.
65+
66+
Returns:
67+
:class:`~py_trees.behaviour.Behaviour`: the root of the eventually_swiss subtree
68+
69+
.. seealso:: :meth:`py_trees.idioms.eventually`, :ref:`py-trees-demo-eventually-swiss-program`
70+
"""
71+
# pylint: disable=too-many-arguments, too-many-locals
72+
# This is acceptable, to give users maximum control over how this swiss-knife
73+
# idiom behaves.
74+
# pylint: disable=abstract-class-instantiated
75+
# behaviours.Failure and behaviours.Success are valid instantiations
76+
77+
workers_sequence = composites.Sequence(
78+
name="Workers",
79+
memory=True,
80+
children=workers,
81+
)
82+
on_failure_return_status = composites.Sequence(
83+
name="On Failure Return Failure",
84+
memory=True,
85+
children=[on_failure, behaviours.Failure(name="Failure")],
86+
)
87+
on_failure_subtree = composites.Selector(
88+
name="On Failure",
89+
memory=True,
90+
children=[workers_sequence, on_failure_return_status],
91+
)
92+
if return_on_success_status:
93+
on_success_return_status = on_success
94+
else:
95+
on_success_return_status = composites.Selector(
96+
name="On Success Return Success",
97+
memory=True,
98+
children=[on_success, behaviours.Success(name="Success")],
99+
)
100+
on_success_subtree = composites.Sequence(
101+
name="On Success",
102+
memory=True,
103+
children=[on_failure_subtree, on_success_return_status],
104+
)
105+
root = OnPreempt(
106+
name=name,
107+
child=on_success_subtree,
108+
on_preempt=on_preempt,
109+
single_tick=on_preempt_single_tick,
110+
period_ms=on_preempt_period_ms,
111+
timeout=on_preempt_timeout,
112+
)
113+
114+
return root

0 commit comments

Comments
 (0)