Fixed compilation bugs on waypoints functions

src/tiago_nav_bt_module.cpp

@@ -437,11 +437,11 @@ BT::NodeStatus CTiagoNavModuleBT::sync_tiago_nav_go_to_waypoints(BT::TreeNode& s
   first_index_goal = first_index.value();
   num_goal = num.value();
   if (!continue_on_error)
-    this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal);
+    this->tiago_nav_module.go_to_waypoints(group_goal, first_index_goal, num_goal);
   else
   {
     continue_on_error_goal = continue_on_error.value();
-    this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal, continue_on_error_goal);
+    this->tiago_nav_module.go_to_waypoints(group_goal, first_index_goal, num_goal, continue_on_error_goal);
   }
   return BT::NodeStatus::SUCCESS;
 }
@@ -463,7 +463,10 @@ BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints(BT::TreeNode&
 
   if (new_goal)
   {
-    double x_goal, y_goal, x_y_pos_tol_goal;
+    std::string group_goal;
+    unsigned int first_index_goal, num_goal;
+    bool continue_on_error_goal;
+
     if (!group || !first_index || !num)
     {
       ROS_ERROR("CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints-> Incorrect or missing input. It needs the following input ports: group(std::string), first_index(unsigned int), num(unsigned int) and [Optional] continue_on_error(bool)");
@@ -474,11 +477,11 @@ BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints(BT::TreeNode&
     first_index_goal = first_index.value();
     num_goal = num.value();
     if (!continue_on_error)
-      this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal);
+      this->tiago_nav_module.go_to_waypoints(group_goal, first_index_goal, num_goal);
     else
     {
       continue_on_error_goal = continue_on_error.value();
-      this->tiago_nav_module.go_to_waypoint(group_goal, first_index_goal, num_goal, continue_on_error_goal);
+      this->tiago_nav_module.go_to_waypoints(group_goal, first_index_goal, num_goal, continue_on_error_goal);
     }
   }
   if (this->tiago_nav_module.is_finished())