From 05be610db35a9368a56615f70ad3e2ebcad8985a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20Enrique=20Dom=C3=ADnguez?=
<jdominguez@iri.upc.edu>
Date: Tue, 14 Apr 2020 13:37:41 +0200
Subject: [PATCH] Timeout added in case of preempted to allow new planning
after new goal
---
src/xml/nav_main_tree.xml | 24 +++++++++++++++++-------
src/xml/nav_with_gui_main_tree.xml | 24 +++++++++++++++++-------
2 files changed, 34 insertions(+), 14 deletions(-)
diff --git a/src/xml/nav_main_tree.xml b/src/xml/nav_main_tree.xml
index b921c22..1ff6145 100644
--- a/src/xml/nav_main_tree.xml
+++ b/src/xml/nav_main_tree.xml
@@ -17,13 +17,23 @@
<BehaviorTree ID="go_to_goal_nav_3d">
<Sequence>
<Action ID="set_goal_frame_nav_3d" frame_id="{frame_id}"/>
- <Timeout msec="600000">
- <RetryUntilSuccesful num_attempts="3">
- <!-- Pending: choose type of navigation required (only position/orientation) -->
- <Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
- </RetryUntilSuccesful>
- </Timeout>
- <Condition ID="is_succeded_nav_3d"/>
+ <RetryUntilSuccesful num_attempts="3">
+ <Fallback>
+ <Sequence>
+ <Timeout msec="600000">
+ <!-- Pending: choose type of navigation required (only position/orientation) -->
+ <Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_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>
+ </Fallback>
+ </RetryUntilSuccesful>
<Action ID="get_current_status_nav_3d" status_code="{status}"/>
</Sequence>
</BehaviorTree>
diff --git a/src/xml/nav_with_gui_main_tree.xml b/src/xml/nav_with_gui_main_tree.xml
index 6498004..26296c9 100644
--- a/src/xml/nav_with_gui_main_tree.xml
+++ b/src/xml/nav_with_gui_main_tree.xml
@@ -29,13 +29,23 @@
<BehaviorTree ID="go_to_goal_nav_3d">
<Sequence>
<Action ID="set_goal_frame_nav_3d" frame_id="{frame_id}"/>
- <Timeout msec="600000">
- <RetryUntilSuccesful num_attempts="3">
- <!-- Pending: choose type of navigation required (only position/orientation) -->
- <Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
- </RetryUntilSuccesful>
- </Timeout>
- <Condition ID="is_succeded_nav_3d"/>
+ <RetryUntilSuccesful num_attempts="3">
+ <Fallback>
+ <Sequence>
+ <Timeout msec="600000">
+ <!-- Pending: choose type of navigation required (only position/orientation) -->
+ <Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_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>
+ </Fallback>
+ </RetryUntilSuccesful>
<Action ID="get_current_status_nav_3d" status_code="{status}"/>
</Sequence>
</BehaviorTree>
--
GitLab