[ROS2] Add cleanup function to create_action_servers.py

[amalnanavati]

Some trees make changes (e.g., allowing certain collisions, toggling perception nodes on) that should be undone when the tree ends. The tree can end in a couple of circumstances:

1. Tree succeeds: The tree should be designed in such a way that all behaviors necessary to undo the changes are taken care of if the tree succeeds.
2. Tree fails: In this case, we can add a selector with memory to run certain cleanup behaviors after failure.
3. ROS action fails or is aborted: In this case, we need a cleanup function in create_action_servers.py that cleans up any state from the action.

This Issue is to implement #3.

[amalnanavati]

I'd propose the following:

1. action_server_bt.py already has a preempt_goal(...) function that is called when the action is preempted or the watchdog fails. This function is intended to block until preemption is complete. We should use this function to achieve the desired functionality.
2. Currently, preempt_goal(...) calls the tree's stop method, which in turn calls every behavior's terminate method. This is important (e.g., to close any threads or other state the tree created in setup), but does not allow us to do tree-level cleanup (e.g., ensure certain ROS services are called).
3. Therefore, I propose changing create_tree into two functions that must be overridden, create_main_tree and create_cleanup_tree (which returns None if there is no cleanup to be done). create_tree calls both, stores the cleanup tree as an instance attribute, and returns a final tree consisting of both put together in a SelectorWithMemory. preempt_goal still calls stop on the tree that is passed to it (the final tree), but then also ticks the cleanup tree until completion. We will need to determine how often to tick it though.

The downside of the above solution is that it still relies on tree logic. This is nice because it makes sense that we would want to run certain behaviors in the cleanup. But the downside is that we have to tick it to completion and it introduces this other variable of how often to tick the tree.

However, the other solution I can think of is having each tree create an arbitrary cleanup function. However, then it would have to re-create the service clients that is necessary to cleanup, which seems overkill to me when the service client is already in the behavior. (Although, actually most likely the cleanup tree that is in the Fallback and the cleanup tree that is called in preempt_goal need to be separate, because py_trees doesn't allow the same behavior in multiple trees, so maybe we should; do an arbitrary cleanup function anyway because each behavior will have its own service client regardless.)

[amalnanavati]

A good test for this is to run MoveToMouth, terminate at the staging location, and verifying that face detection is toggled off.

[amalnanavati]

What we decided:

1. To address it in the most straightforward way for now, we will add a undo_on_preempt flag to all the behaviors that we want to undo on preemption. For those behaviors, we will modify the terminate function such that if they go to INVALID status, the behaviors will block until they undo what they did.
2. The cleaner way to solve this is creating an idiom that takes in behaviors Pre, Post, and Main and puts them together in the following way:

Sequence
  |  Pre
  |  ForceSuccess
  |    |  StatusToBlackboard
  |    |    |  Main
  |  Post
  |  BlackboardToStatus

That handles clean termination when a behavior within the tree fails. To handle preemptions, Pre needs to run Post synchronously in its terminate clause, which probably requires a subclass of behavior to make it clean.

[amalnanavati]

EDIT: Following the discussion here, and wanting to avoid reading/writing the status to blackboard where not necessary, I actually think the right "clean" solution is the following:

Parallel
  | SelectorWithMemory
  |    | SequenceWithMemory
  |    |    | Pre
  |    |    | Main
  |    |    | Post
  |    | SequenceWithMemory
  |    |    | Post
  |    |    | Failure
  | OnTerminateMultiTick
  |    | Post

[amalnanavati]

Closed with #103