diff --git a/src/xml/nav_main_tree.xml b/src/xml/nav_main_tree.xml
index b921c22a44980bc8496d2a4184e8b03b9068514f..1ff614519c1f3b08069ab451637ca5d255614816 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 649800490da0b6353ff636f89d067b8b079bda37..26296c9ecd05448ef164be0f9056f1c1cf924138 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>