Check action finished

[jjzapf]

A plan action is implemented by a behavior tree such as the one found here

https://github.com/IntelligentRoboticsLabs/ros2_planning_system/blob/master/plansys2_executor/behavior_trees/plansys2_action_bt.xml

In this behavior tree the WAIT_PREV_ACTIONS placeholder is either removed or replaced by one or more WaitAction nodes. A WaitAction node is responsible for ensuring that the current action does not start before the action specified by the WaitAction node has finished. In the current implementation, this is handled by a call to the is_finished function in the action executor. However, this function only returns the state the ExecuteAction node. It does not guarantee that the rest of the nodes in the action behavior tree have finished. Fortunately, we already have flags in each action within the action map indicating whether the "at start" and "at end" effects have been applied. By adding checks to the WaitAction node for whether the "at start" effects and "at end" effects have been applied, we can safely assume that the action tree has indeed finished.

To test this PR, run the example provided by @roveri-marco in Issue #211. With these changes, the example should finish with success. Without these changes the example will fail. In this case, the failure is caused by an "at start" effect taking place prematurely, which conflicts with an "over all" condition in a running action.

[jjzapf]

Hi @fmrico, hold off on this for a bit. I'm still looking into one more change that I may add to this.

[jjzapf]

Ok, I think this should be good to go now. I moved the leftover requirement check after the state update in the get_graph function. Recall that this check is used to verify that the leftover requirements (i.e the requirements that were not matched to satisfying nodes) are indeed also satisfied. Theoretically, we should not have to worry about these leftover requirements as they should be satisfied by the initial state. However, I still think it's a good idea to include this check as a safeguard against any logic errors, which I'm sure was your original intent as well. To me it seems even safer to verify these requirements after the state update to make sure that we haven't incorrectly constructed a graph that leads to contradictions.

[fmrico]

Ok. LGTM!!

The Behavior Tree template for executing an action lets us customize the execution of a plan. This PR makes me think that the behavior tree template is very closely coupled with the code. If someone wanted to remove any of these nodes, execution might be blocked. Perhaps we could put a BT node (no optional) at the end of this tree that would mark the end of the complete execution of an action. Since this would be a separate improvement, let's keep that in mind and merge this now, which fixes the issue properly.

Thanks @jjzapf !!

[jjzapf]

That's a good point, @fmrico. You're right, if a user were to remove the ExecuteAction, ApplyAtStartEffect, or ApplyAtEndEffect nodes from the behavior tree, the execution would be blocked. In our case, I don't think we would ever do that, but we should try to make the code as flexible as is reasonable. I think adding a "no optional" BT node at the end of the tree to mark the end of the action is a good idea.