diff --git a/src/xml/nav_with_gui_main_tree.xml b/src/xml/nav_with_gui_main_tree.xml
index fb71571ab4108db531e30d77de22647613315fac..e05408ba4c679b0b78d79e12fd1bed7a3ddb875f 100644
--- a/src/xml/nav_with_gui_main_tree.xml
+++ b/src/xml/nav_with_gui_main_tree.xml
@@ -1,97 +1,43 @@
 <?xml version="1.0"?>
 <root main_tree_to_execute="NAV_WITH_GUI_MAIN">
-    <!-- Pending: adapt demo to changes applied to the basic navigation demo -->
+
     <BehaviorTree ID="NAV_WITH_GUI_MAIN">
-        <Sequence>
-            <Action ID="sync_start_nav_gui"/>
-            <Fallback>
-                <ReactiveSequence>
-                    <Action ID="get_status_text" nav_status="{status_text}"/>
-                    <Action ID="set_nav_status_nav_gui" nav_status="{status_text}"/>
-                    <Action ID="get_current_velocity" velocity="{velocity}"/>
-                    <Action ID="set_current_velocity_nav_gui" velocity="{velocity}"/>
-                    <Action ID="get_current_path_distance" path_distance="{path_distance}"/>
-                    <Action ID="set_distance_to_goal_nav_gui" distance_to_goal="{path_distance}"/>
-                    <Fallback>
-                        <Sequence>
-                            <SubTree ID="go_to_goal" f_id="f_id" heading_tol="heading_tol" output_go_to_goal="NAV_BT_status" update_goal="update_goal" x="x" x_y_pos_tol="x_y_pos_tol" y="y" yaw="yaw"/>
-                            <SetBlackboard output_key="output_nav" value="{NAV_BT_status}"/>
-                        </Sequence>
-                        <ForceFailure>
-                            <Sequence>
-                                <Action ID="get_current_status_nav" status_code="{NAV_BT_status}"/>
-                                <SetBlackboard output_key="output_nav" value="{NAV_BT_status}"/>
-                            </Sequence>
-                        </ForceFailure>
-                    </Fallback>
-                </ReactiveSequence>
-                <ForceFailure>
-                    <Action ID="sync_stop_nav"/>
-                </ForceFailure>
-            </Fallback>
-        </Sequence>
+      <Sequence>
+        <Action ID="sync_start_nav_gui"/>
+        <Fallback>
+          <ReactiveSequence>
+            <Action ID="get_status_text" nav_status="{status_text}"/>
+            <Action ID="set_nav_status_nav_gui" nav_status="{status_text}"/>
+            <Action ID="get_current_velocity" velocity="{velocity}"/>
+            <Action ID="set_current_velocity_nav_gui" velocity="{velocity}"/>
+            <Action ID="get_current_path_distance" path_distance="{path_distance}"/>
+            <Action ID="set_distance_to_goal_nav_gui" distance_to_goal="{path_distance}"/>
+            <SubTree ID="go_to_goal" x_goal="x_in" y_goal="y_in" yaw_goal="yaw_in" heading_tol_goal="heading_tol_in" x_y_pos_tol_goal="x_y_pos_tol_in" update_goal="update_goal_in" status="NAV_BT_status_out" frame_id="frame_id_in"/>
+          </ReactiveSequence>
+          <ForceFailure>
+            <Sequence>
+              <Action ID="get_current_status_nav" status_code="{NAV_BT_status_out}"/>
+              <!-- If navigation fails we stop the movement of the robot -->
+              <Action ID="sync_stop_nav"/>
+              <!-- We are not waiting until the navigation really stops -->
+            </Sequence>
+          </ForceFailure>
+        </Fallback>
+      </Sequence>
     </BehaviorTree>
-    <!-- ////////// -->
+
     <BehaviorTree ID="go_to_goal">
-        <Sequence>
-            <Action ID="set_goal_frame" frame_id="{f_id}"/>
-            <Timeout msec="600000">
-                <RetryUntilSuccesful num_attempts="3">
-                    <Action ID="async_go_to_pose" heading_tol="{heading_tol}" update_goal="{update_goal}" x="{x}" x_y_pos_tol="{x_y_pos_tol}" y="{y}" yaw="{yaw}"/>
-                </RetryUntilSuccesful>
-            </Timeout>
-            <Condition ID="is_succeded_nav"/>
-            <Action ID="get_current_status_nav" status_code="{NAV_BT_status}"/>
-            <SetBlackboard output_key="output_go_to_goal" value="{NAV_BT_status}"/>
-        </Sequence>
-    </BehaviorTree>
-    <!-- ////////// -->
-    <TreeNodesModel>
-        <Action ID="async_go_to_pose">
-            <inout_port name="heading_tol"/>
-            <inout_port name="update_goal"/>
-            <inout_port name="x"/>
-            <inout_port name="x_y_pos_tol"/>
-            <inout_port name="y"/>
-            <inout_port name="yaw"/>
-        </Action>
-        <Action ID="get_current_path_distance">
-            <inout_port name="path_distance"/>
-        </Action>
-        <Action ID="get_current_status_nav">
-            <inout_port name="status_code"/>
-        </Action>
-        <Action ID="get_current_velocity">
-            <inout_port name="velocity"/>
-        </Action>
-        <Action ID="get_status_text">
-            <inout_port name="nav_status"/>
-        </Action>
-        <SubTree ID="go_to_goal">
-            <inout_port name="f_id"/>
-            <inout_port name="heading_tol"/>
-            <inout_port name="output_go_to_goal"/>
-            <inout_port name="update_goal"/>
-            <inout_port name="x"/>
-            <inout_port name="x_y_pos_tol"/>
-            <inout_port name="y"/>
-            <inout_port name="yaw"/>
-        </SubTree>
+      <Sequence>
+        <Action ID="set_goal_frame" 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" 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"/>
-        <Action ID="set_current_velocity_nav_gui">
-            <inout_port name="velocity"/>
-        </Action>
-        <Action ID="set_distance_to_goal_nav_gui">
-            <inout_port name="distance_to_goal"/>
-        </Action>
-        <Action ID="set_goal_frame">
-            <inout_port name="frame_id"/>
-        </Action>
-        <Action ID="set_nav_status_nav_gui">
-            <inout_port name="nav_status"/>
-        </Action>
-        <Action ID="sync_start_nav_gui"/>
-        <Action ID="sync_stop_nav"/>
-    </TreeNodesModel>
-    <!-- ////////// -->
+        <Action ID="get_current_status_nav" status_code="{status}"/>
+      </Sequence>
+    </BehaviorTree>
+
 </root>