Unified cleanup using scoped behaviors

[amalnanavati]

Description

In service of #54 .

In our trees, there are numerous times when we want to flip a switch, move the robot, and then flip the switch. For example, turning face detection on/off, adding/removing a collision object in the world, allowing/disallowing collisions with certain objects, etc.

What is challenging is that we want to run the "cleanup" for flipping the switch in three different cases:

1. If the robot motion succeeds.
2. If the robot motion fails.
3. If the entire tree is preempted (e.g., the user terminates the robot motion from the app).

Achieving the above 2 is straightforward using Selectors. Achieving the third is more difficult, because when tree.stop(INVALID) is called, it is called on all the behaviors one-by-one, which in turn calls each behaviors' terminate function. Therefore, achieving the proper cleanup behavior in the third case requires performing that cleanup in a terminate function of a behavior.

Building off of the discussion and contribution in py_trees#427, this PR implements a scoped_behavior idiom that executes a pre-behavior, main-behavior, and post-behaviour, with a guarantee that the post-behavior will execute even if the behavior is preempted.

Testing procedure

Run the necessary nodes as documented in the README. Verify the following:

• Sim:
  • ros2 action send_goal /MoveToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback
    • Runs to completion successfully.
  • ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedback
    • Runs to completion successfully.
    • When the CLI action call is terminated while face detection is on, it toggles face detection off.
    • When the CLI action call is terminated during the cartesian motion to the mouth, the robot immediately stops moving and re-enables collisions with the wheelchair_collision object.
  • ros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback
    • Runs to completion successfully.
    • When the CLI action call is terminated during the cartesian motion away from the mouth, the robot immediately stops moving and re-enables collisions with the wheelchair_collision object.
    • When the CLI action call is terminated during the motion to the end configuration, the robot immediately stops moving and removes the in_front_of_wheelchair_wall from the collision scene.
  • (Note that you'll have to run MoveToMouth and MoveFromMouthToRestingPosition several times iteratively to test all of the above.)
• Real:
  • ros2 action send_goal /MoveToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback
    • Runs to completion successfully.
  • ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedback
    • Runs to completion successfully.
    • When the CLI action call is terminated while face detection is on, it toggles face detection off.
    • When the CLI action call is terminated during the cartesian motion to the mouth, the robot immediately stops moving and re-enables collisions with the wheelchair_collision object.
  • ros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedback
    • Runs to completion successfully.
    • When the CLI action call is terminated during the cartesian motion away from the mouth, the robot immediately stops moving and re-enables collisions with the wheelchair_collision object.
    • When the CLI action call is terminated during the motion to the end configuration, the robot immediately stops moving and removes the in_front_of_wheelchair_wall from the collision scene.
  • (Note that you'll have to run MoveToMouth and MoveFromMouthToRestingPosition several times iteratively to test all of the above.)
• Run the unit tests and verify that they succeed: colcon test --packages-select ada_feeding; colcon test-result --all --verbose

Before opening a pull request

• Format your code using black formatter python3 -m black .
• Run your code through pylint and address all warnings/errors. The only warnings that are acceptable to not address is TODOs that should be addressed in a future PR. From the top-level ada_feeding directory, run: pylint --recursive=y --rcfile=.pylintrc ..

Before Merging

• Squash & Merge

[amalnanavati]

Tested in sim. The only issue I noticed is that when terminating the CLI action call during robot arm motion, the tree runs the post-behavior before terminating the movement, which can be particularly harmful if the post-behavior blocks (e.g., toggling a collision object off). Instead, we want to first terminate the robot movement, and then run the post behavior.

I should write comprehensive test cases for this idiom, covering:

1. worker is running: behavior should be running, and on_success, on_failure, and on_preempt are not ticked.
2. worker succeeds and on_succeed is running: behavior should be running, and on_failure and on_preempt are not ticked.
3. worker succeeds and on_succeed is succeeds: behavior should succeed, and on_failure and on_preempt are not ticked.
4. worker succeeds and on_succeed fails: behavior should fail, and on_failure and on_preempt are not ticked.
5. worker fails and on_failure is running: behavior should be running, and on_succeed and on_preempt are not ticked.
6. worker fails and on_failure succeeds: behavior should fail, and on_succeed and on_preempt are not ticked.
7. worker fails and on_failure fails: behavior should fail, and on_succeed and on_preempt are not ticked.
8. tree is stopped before any ticks: worker, on_success, on_failure, and on_preempt are not ticked.
9. tree is stopped after a single tick where worker is running, fails, or succeeds: worker is ticked once, on_success and on_failure are not ticked, and on_preempt is ticked to completion.
10. tree is stopped after multiple ticks where worker fails/succeeds and on_success/on_failure is running: worker is ticked, on_success/on_failure is ticked, and on_preempt is ticked to completion.

[amalnanavati]

Wrote comprehensive test cases, and modified eventually_swiss to satisfy those test cases. Simply put, the test cases guarantee that eventually_swiss behaves as follows:

1. on_success is run if and only if workers succeed.
2. on_failure is run if and only if workers fail
3. on_preempt is run if and only if a node above the root calls root.stop(INVALID).
4. If workers succeed, the return status is the success/failure status of on_success.
5. If workers fail, the return status is failure.
6. If a node above the root calls root.stop(INVALID), first terminate(INVALID) is called on workers, on_success/on_failure, and THEN on_preempt is ticked to completion.

The tree has become a bit unwieldy (reproduced below), but the comprehensive test cases mean that we can be fairly certain it works as outlined above.

SelectorWithoutMemory
  |-ForceFailure
  |   |-SelectorWithMemory
  |   |    |-SequenceWithMemory
  |   |    |    |-UnsetBlackboardVariable
  |   |    |    |-worker
  |   |    |    |-StatusToBlackboard
  |   |    |    |    |-on_success
  |   |    |-SequenceWithMemory
  |   |    |    |-Inverter
  |   |    |    |    |-CheckBlackboardVariableExists
  |   |    |    |-StatusToBlackboard
  |   |    |    |    |-SuccessIsFailure
  |   |    |    |    |    |-on_failure
  |-OnTerminateMultiTick
  |   |-on_preempt
  |-SequenceWithMemory
  |   |-WaitForBlackboardVariable
  |   |-BlackboardToStatus

All the test cases in sim pass. Yet to test on the real robot.

[egordon]

See comment for OnPreempt, stopping the review there for now.

[amalnanavati]

Note that even the new scoped_behavior implementation is not exactly what we want. Say the worker succeeds and post starts ticking, and then the tree is preempted. Our current implementation ticks a completely new copy of post to completion. In actuality, we want to continue ticking that same copy of post. (I have an idea for how to address this, will work on it later).

The tests that still need to be implemented for scoped_behavior are:

1. Preemptions: That it pre-empts correctly when preempted before, during, and after execution.
2. Nested Scope: That it behaves correctly, including with preempts, when we have nested scopes.

[amalnanavati]

Addressed all changes, added comprehensive unit tests for scoped_behavior including nested scope. Also added unit tests for both eventually_swiss and scoped_behavior for if the tree is preempted before the idiom is ticked, and if the tree is preempted after the idiom has terminated.

@egordon note that while our discussed approach worked for eventually_swiss, it didn't work for scoped_behavior. That's because using eventually_swiss as a sub-idiom for scoped_behavior would require multiple copies of post, whereas we want only one copy (so that e.g., if post starts ticking before preemption, the same behavior will continue to get ticked). I implemented a slightly different structure for scoped_behavior:

OnPreempt(same `post` as below)
  |-SequenceWithMemory
  |   |-UnsetBlackboardVariable
  |   |-FailureIsSuccess
  |   |    |-StatusToBlackboard
  |   |    |    |-SequenceWithMemory
  |   |    |    |    |-pre
  |   |    |    |    |-workers[1]
  |   |    |    |    |-...
  |   |    |    |    |-workers[n]
  |   |-FailureIsSuccess
  |   |    |-post
  |   |-BlackboardToStatus

The only danger of the above formulation is potential name clashes with the blackboard status. However, I nest the blackboard variable within the idiom name, so the only way names will clash is if the programmer uses multiple idioms with the same name and doesn't pass a custom status_blackboard_key to it, which seems unlikely. Regardless, if you're concerned about name clashes, I can de-dup it by appending the timestamp to it. LMK.