Adding Failure Recovery to your Behavior Tree

Tutorial Level: Beginner

In the previous tutorial, we built a simple pick and place Objective that will keep running until failure. Now, we will use the Wait For User Trajectory Approval Behavior to ask the operator for approval before moving to a waypoint. Should the operator choose not to approve the motion, the behavior will return a failure status.

1. Add User Approval

Lets modify the Move to Waypoint Subtree Objective to include a user approval. From the User Input category add the Wait For User Trajectory Approval Behavior after the motion plan is completed but before it is executed. Make sure to change the solution port to match the key used by Plan MTC Task so it can visualize the {move_to_waypoint_solution}.

warning

When modifying a subtree, any objectives that contains the subtree will be affected. If you are unsure which objectives might be affected, click the three dots next to its name in the Objective Builder and clone it instead.

Since the Pick and Place Objective we created earlier includes the Move to Waypoint subtree we just modified, we can run it to see the new trajectory approval behavior.

2. Add a Fallback Control Node

If the user does not approve the trajectory previewed during Move to Waypoint, the objective will fail. Lets add the a Fallback control node and another Move to Waypoint, so if the operator does not approve of the first trajectory they are presented with an option to move to an alternate position. Edit the Pick and Place Objective to match the one below (Gripper objectives are optional).

3. Recovery Using Teleoperation

Providing the operator with alternate waypoints should one of the motions fail is a good first step, but lets give the operator a little more control. Instead of moving to a secondary waypoint when the operator rejects a trajectory, we will request teleoperation.

Edit your Pick and Place Objective and replace the second Move to Waypoint with a Request Teleoperation Subtree. Modify the enable_user_interaction port and set it to true. This will allow the subtree to exit when the operator is satisfied with the new robot pose. You can provide a prompt to the operator using the user_interaction_prompt port.

Next time we run our Pick and Place Objective and reject a trajectory, the operator will be prompted to teleoperate the robot. At any time during teleoperation the operator can click Abort or Success to complete the Request Teleoperation subtree and resume (or end) the Pick and Place Objective.

Now that we can ask an operator to teleoperate the robot, please take a look at our guide to teleoperation.