diff --git a/src/tiago_nav_bt_module.cpp b/src/tiago_nav_bt_module.cpp
index 68ab2bff2465d6aee2d483004feaf5b395097b19..6fc53573c2eff61cda2d89834a24d19ecc52f3d3 100644
--- a/src/tiago_nav_bt_module.cpp
+++ b/src/tiago_nav_bt_module.cpp
@@ -441,7 +441,7 @@ BT::NodeStatus CTiagoNavModuleBT::sync_tiago_nav_go_to_waypoints(BT::TreeNode& s
   else
   {
     continue_on_error_goal = continue_on_error.value();
-    this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal);
+    this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal, continue_on_error_goal);
   }
   return BT::NodeStatus::SUCCESS;
 }
@@ -478,7 +478,7 @@ BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints(BT::TreeNode&
     else
     {
       continue_on_error_goal = continue_on_error.value();
-      this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal);
+      this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal, continue_on_error_goal);
     }
   }
   if (this->tiago_nav_module.is_finished())
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
index 14097279c1f378fa98c8e1f9247d27c19baec479..7b5632e97faaeb24b989b9e9a2a5ae17620189a0 100644
--- a/src/xml/bt_test.xml
+++ b/src/xml/bt_test.xml
@@ -56,36 +56,36 @@
             <input_port name="x"> desired X positon with respect to the goal frame</input_port>
             <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
             <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
-            <input_port default="-1.0" name="heading_tol"> heading tolerance for the goal</input_port>
-            <input_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</input_port>
+            <input_port name="heading_tol"> heading tolerance for the goal</input_port>
+            <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
         </Action>
         <Action ID="async_tiago_nav_go_to_pose">
             <input_port name="x"> desired X positon with respect to the goal frame</input_port>
             <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
             <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
-            <input_port default="-1.0" name="heading_tol"> heading tolerance for the goal</input_port>
-            <input_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</input_port>
+            <input_port name="heading_tol"> heading tolerance for the goal</input_port>
+            <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
             <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="sync_tiago_nav_go_to_orientation">
             <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
-            <input_port default="-1.0" name="heading_tol"> heading tolerance for the goal</input_port>
+            <input_port name="heading_tol"> heading tolerance for the goal</input_port>
         </Action>
         <Action ID="async_tiago_nav_go_to_orientation">
             <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
-            <input_port default="-1.0" name="heading_tol"> heading tolerance for the goal</input_port>
-            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+            <input_port name="heading_tol"> heading tolerance for the goal</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="sync_tiago_nav_go_to_position">
             <input_port name="x"> desired X positon with respect to the goal frame</input_port>
             <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
-            <input_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</input_port>
+            <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
         </Action>
         <Action ID="async_tiago_nav_go_to_position">
             <input_port name="x"> desired X positon with respect to the goal frame</input_port>
             <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
-            <input_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</input_port>
-            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+            <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="sync_tiago_nav_go_to_poi">
             <input_port name="poi"> name of the desired point of interest to move to</input_port>
@@ -97,14 +97,14 @@
             <input_port name="group"> Name of the desired group of points of interest</input_port>
             <input_port name="first_index"> The index of the first waypoint to execute inside the list of interest points inside the group</input_port>
             <input_port name="num"> The number of waypoints to execute. If 0 is provided, all of them will be executed</input_port>
-            <input_port default="False" name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
+            <input_port name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
         </Action>
         <Action ID="async_tiago_nav_go_to_waypoints">
             <input_port name="group"> Name of the desired group of points of interest</input_port>
             <input_port name="first_index"> The index of the first waypoint to execute inside the list of interest points inside the group</input_port>
             <input_port name="num"> The number of waypoints to execute. If 0 is provided, all of them will be executed</input_port>
-            <input_port default="False" name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
-            <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+            <input_port name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
+            <input_port name="new_goal"> If it's a new_goal</input_port>
         </Action>
         <Action ID="get_tiago_nav_current_waypoint">
             <output_port name="waypoint"> The index of the current waypoint being executed</output_port>
@@ -163,9 +163,9 @@
             <output_port name="x"> desired X positon with respect to the goal frame</output_port>
             <output_port name="y"> desired Y positon with respect to the goal frame</output_port>
             <output_port name="yaw"> desired orientation with respect to the goal frame</output_port>
-            <output_port default="-1.0" name="heading_tol"> heading tolerance for the goal</output_port>
-            <output_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</output_port>
-            <output_port default="True" name="new_goal"> If it's a new_goal</output_port>
+            <output_port name="heading_tol"> heading tolerance for the goal</output_port>
+            <output_port name="x_y_pos_tol"> position tolerance for the goal</output_port>
+            <output_port name="new_goal"> If it's a new_goal</output_port>
         </Action>
         <Action ID="set_tiago_nav_goal_frame_inputs">
             <output_port name="frame_id"> The name of the new reference frame for the future goals</output_port>