diff --git a/src/xml/nav_rotation_main_tree.xml b/src/xml/nav_rotation_main_tree.xml new file mode 100644 index 0000000000000000000000000000000000000000..359449cdec7a68cee0892ebffaaaa1472787d6cd --- /dev/null +++ b/src/xml/nav_rotation_main_tree.xml @@ -0,0 +1,40 @@ +<root main_tree_to_execute="NAV_3D_ROTATION_MAIN"> + + <BehaviorTree ID="NAV_3D_ROTATION_MAIN"> + <Fallback> + <SubTree ID="go_to_orientation_nav_3d" yaw_goal="yaw_in" heading_tol_goal="heading_tol_in"update_goal="update_goal_in" status="NAV_3D_BT_status_out" frame_id="frame_id_in"/> + <ForceFailure> + <Sequence> + <Action ID="get_current_status_nav_3d" status_code="{NAV_3D_BT_status_out}"/> + <!-- If navigation fails we stop the movement of the robot --> + <Action ID="sync_stop_nav_3d"/> + <!-- We are not waiting until the navigation really stops --> + </Sequence> + </ForceFailure> + </Fallback> + </BehaviorTree> + + <BehaviorTree ID="go_to_orientation_nav_3d"> + <Sequence> + <Action ID="set_goal_frame_nav_3d" frame_id="{frame_id}"/> + <RetryUntilSuccesful num_attempts="3"> + <ReactiveFallback> + <Sequence> + <Timeout msec="60000"> + <Action ID="async_go_to_orientation_nav_3d" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" update_goal="{update_goal}"/> + </Timeout> + <Condition ID="is_succeded_nav_3d"/> + </Sequence> + <Sequence> + <Condition ID="is_preempted_nav_3d"/> + <Timeout msec="5000"> + <Action ID="NOP"/> + </Timeout> + </Sequence> + </ReactiveFallback> + </RetryUntilSuccesful> + <Action ID="get_current_status_nav_3d" status_code="{status}"/> + </Sequence> + </BehaviorTree> + +</root>