From d68adde4d41f00361d364b64d5f907eaf6a969a0 Mon Sep 17 00:00:00 2001
From: abhagwan <albert.bhagwan@upc.edu>
Date: Tue, 10 Dec 2019 12:52:12 +0100
Subject: [PATCH] Added xml files with navigation BT
---
src/xml/nav_main_tree.xml | 56 ++++++++++++++++++++++++++++++++++++
src/xml/nav_restart_tree.xml | 15 ++++++++++
2 files changed, 71 insertions(+)
create mode 100644 src/xml/nav_main_tree.xml
create mode 100644 src/xml/nav_restart_tree.xml
diff --git a/src/xml/nav_main_tree.xml b/src/xml/nav_main_tree.xml
new file mode 100644
index 0000000..49ea3e2
--- /dev/null
+++ b/src/xml/nav_main_tree.xml
@@ -0,0 +1,56 @@
+<root main_tree_to_execute="NAV_MAIN">
+ <BehaviorTree ID="NAV_MAIN">
+ <Fallback>
+ <SequenceStar>
+ <Action ID="set_goal_frame" frame_id="/map"/>
+ <Fallback>
+ <SubTree ID="go_to_goal" x="x" y="y" yaw="yaw" heading_tol="heading_tol" x_y_pos_tol="x_y_pos_tol" update_goal="update_goal"/>
+ <ForceFailure>
+ <Action ID="current_status" status_code="{NAV_BT_status}"/>
+ </ForceFailure>
+ </Fallback>
+ </SequenceStar>
+ <ForceFailure>
+ <Action ID="sync_stop"/>
+ </ForceFailure>
+ </Fallback>
+ </BehaviorTree>
+
+ <BehaviorTree ID="go_to_goal">
+ <Sequence>
+ <Action ID="set_goal_frame" frame_id="/map"/>
+ <Timeout msec="6000000">
+ <RetryUntilSuccesful num_attempts="3">
+ <Action ID="async_go_to_pose" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" update_goal="{update_goal}"/>
+ </RetryUntilSuccesful>
+ </Timeout>
+ <Condition ID="is_succeded"/>
+ <Action ID="current_status" status_code="{NAV_BT_status}"/>
+ </Sequence>
+ </BehaviorTree>
+
+<!--
+ <BehaviorTree ID="change_orientation">
+ <Sequence>
+ <Timeout msec="20000">
+ <RetryUntilSuccesful num_attempts="3">
+ <Action ID="async_go_to_orientation" yaw="{yaw}" heading_tol="0.1"/>
+ </RetryUntilSuccesful>
+ </Timeout>
+ <Condition ID="is_succeded"/>
+ </Sequence>
+ </BehaviorTree>
+
+ <BehaviorTree ID="change_position">
+ <Sequence>
+ <Action ID="set_goal_frame" frame_id="/ana/base_link"/>
+ <Timeout msec="10000">
+ <RetryUntilSuccesful num_attempts="3">
+ <Action ID="async_go_to_position" x="-0.5" y="0.0" x_y_pos_tol="0.2"/>
+ </RetryUntilSuccesful>
+ </Timeout>
+ <Condition ID="is_succeded"/>
+ </Sequence>
+ </BehaviorTree>
+ -->
+</root>
diff --git a/src/xml/nav_restart_tree.xml b/src/xml/nav_restart_tree.xml
new file mode 100644
index 0000000..d1d7567
--- /dev/null
+++ b/src/xml/nav_restart_tree.xml
@@ -0,0 +1,15 @@
+<root main_tree_to_execute="NAV_RESTART">
+ <BehaviorTree ID="NAV_RESTART">
+ <SequenceStar>
+ <Action ID="sync_stop"/>
+ <ReactiveFallback>
+ <Condition ID="is_finished"/>
+ <Timeout msec="10000">
+ <Action ID="NOP"/>
+ </Timeout>
+ </ReactiveFallback>
+ <Action ID="costmaps_clear"/>
+ <Action ID="reset_goal_variables"/>
+ </SequenceStar>
+ </BehaviorTree>
+</root>
--
GitLab