From 33252e8471e6d12491c16f19413814e415a437af Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20Enrique=20Dom=C3=ADnguez?=
<jdominguez@iri.upc.edu>
Date: Thu, 21 May 2020 08:58:17 +0200
Subject: [PATCH] Rotation movement subtree added
---
src/xml/nav_rotation_main_tree.xml | 40 ++++++++++++++++++++++++++++++
1 file changed, 40 insertions(+)
create mode 100644 src/xml/nav_rotation_main_tree.xml
diff --git a/src/xml/nav_rotation_main_tree.xml b/src/xml/nav_rotation_main_tree.xml
new file mode 100644
index 0000000..359449c
--- /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>
--
GitLab