diff --git a/CMakeLists.txt b/CMakeLists.txt
index 43cb1e78abb520d8f1b0e814ba564de356d390df..fde2ae8103177d629dee3095d7e433b612b99224 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs iri_base_algorithm)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs iri_base_algorithm iri_behaviortree)
 find_package(pal_waypoint_msgs)
 
 if(${pal_waypoint_msgs_FOUND})
@@ -106,7 +106,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES tiago_nav_module 
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs iri_base_algorithm
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs iri_base_algorithm iri_behaviortree
 #  DEPENDS system_lib
 )
 
@@ -147,6 +147,13 @@ target_link_libraries(${client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_
 add_dependencies(${client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name})
 add_dependencies(${client_name} ${tiago_nav_client_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+set(module_bt_name tiago_nav_module_bt)
+add_library(${module_bt_name} src/tiago_nav_bt_module.cpp)
+target_link_libraries(${module_bt_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
+target_link_libraries(${module_bt_name} ${catkin_LIBRARIES})
+target_link_libraries(${module_bt_name} ${iriutils_LIBRARIES})
+add_dependencies(${module_bt_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${module_name})
+
 #############
 ## Install ##
 #############
diff --git a/include/tiago_nav_module/tiago_nav_bt_module.h b/include/tiago_nav_module/tiago_nav_bt_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..33b00c33484cce1da86d74e65f47e45853994097
--- /dev/null
+++ b/include/tiago_nav_module/tiago_nav_bt_module.h
@@ -0,0 +1,384 @@
+#ifndef _IRI_TIAGO_NAV_MODULE_BT_H
+#define _IRI_TIAGO_NAV_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+
+#include "iri_bt_factory.h"
+#include <tiago_nav_module/tiago_nav_module.h>
+
+class CTiagoNavModuleBT
+{
+  private:
+    // object of tiago_nav_module
+    CTiagoNavModule tiago_nav_module;
+
+  public:
+    /**
+      * \brief Constructor
+      *
+      */
+    CTiagoNavModuleBT();
+
+    /**
+      * \brief Register all conditions and actions needed for using the module
+      *
+      * This function registers all conditions and actions needed for using the module
+      * to the factory provided including all input and output ports required
+      * by actions.
+      *
+      * \param factory (IriBehaviorTreeFactory)
+      *
+      */
+    void init(IriBehaviorTreeFactory &factory);
+
+    /**
+    * \brief Destructor
+    *
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~CTiagoNavModuleBT();
+
+  protected:
+
+    /**
+      * \brief Synchronized go_to_pose TIAGo nav function.
+      *
+      * This function calls go_to_pose of tiago_nav_module. As this is a
+      * synchronous action is_tiago_nav_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   x (double): desired X positon with respect to the goal frame set by the
+      *          set_goal_frame() function.
+      *
+      *   y (double): desired Y positon with respect to the goal frame set by the
+      *          set_goal_frame() function.
+      *
+      *   yaw (double): desired orientation with respect to the goal frame set by the
+      *            set_goal_frame() function.
+      *
+      *   heading_tol (double): [Optional] heading tolerance for the goal.
+      *
+      *   x_y_pos_tol (double): [Optional] position tolerance for the goal.
+      *
+      * \param self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus sync_tiago_nav_go_to_pose(BT::TreeNode& self);
+
+    /**
+      * \brief Async go_to_pose TIAGo nav function.
+      *
+      * This function calls go_to_pose of tiago_nav_module. As this is
+      * an asynchronous action is_tiago_nav_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   x (double): desired X positon with respect to the goal frame set by the
+      *          set_goal_frame() function.
+      *
+      *   y (double): desired Y positon with respect to the goal frame set by the
+      *          set_goal_frame() function.
+      *
+      *   yaw (double): desired orientation with respect to the goal frame set by the
+      *            set_goal_frame() function.
+      *
+      *   heading_tol (double): [Optional] heading tolerance for the goal.
+      *
+      *   x_y_pos_tol (double): [Optional] position tolerance for the goal.
+      *
+      * It also has a bidirectional port:
+      *
+      *   new_goal (bool): If it's a new_goal.
+      *
+      * \param self node with the required inputs defined as follows:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
+      * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
+      * in progress (BT:NodeStatus::RUNNING). If inputs are missing or incorrect
+      * it also returns BT:NodeStatus::FAILURE
+      *
+      */
+    BT::NodeStatus async_tiago_nav_go_to_pose(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized set_workspace TIAGo nav function.
+      *
+      * This function calls set_workspace of tiago_nav_module.
+      *
+      * It has the following input ports:
+      *
+      *   frame_id (std::string): name of the new reference frame for the future goals.
+      *                 This frame can be any that exist inside the TF tree
+      *                 of the robot.
+      *
+      * \param self Self node with the required ports:
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus set_tiago_nav_goal_frame(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized stop TIAGo nav function.
+      *
+      * This function calls stop of tiago_nav_module. As this is a
+      * synchronous action is_tiago_nav_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus sync_cancel_tiago_nav_action(void);
+
+    /**
+      * \brief Async stop TIAGo nav function.
+      *
+      * This function calls stop of tiago_nav_module. As this is
+      * an asynchronous action is_tiago_nav_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
+      * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
+      * in progress (BT:NodeStatus::RUNNING). If inputs are missing or incorrect
+      * it also returns BT:NodeStatus::FAILURE
+      *
+      */
+    BT::NodeStatus async_cancel_tiago_nav_action(void);
+
+    /**
+      * \brief Checks if the module is idle and there isn't any new movement request.
+      *
+      * This function must be used when any sync action is called.
+      *
+      * \return a BT:NodeStatus indicating whether the last movement has
+      * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE),
+      * regardless of its success or not.
+      *
+      */
+    BT::NodeStatus is_tiago_nav_finished(void);
+
+    /**
+      * \brief Checks if the module is idle and there isn't any new movement request.
+      *
+      * This function must be used when any sync action is called and 
+      * a BT timeout is used.
+      *
+      * \return a BT:NodeStatus indicating whether the last movement has
+      * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::RUNNING),
+      * regardless of its success or not.
+      *
+      */
+    BT::NodeStatus async_is_tiago_nav_finished(void);
+
+    ///TIAGo nav module status
+    /**
+      * \brief Checks if the module is active and operating properly.
+      *
+      * This function must be used to know whether the module is active and
+      * operating properly, or not.
+      *
+      * \return a BT:NodeStatus indicating whether the module is active and
+      * operating properly (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_running(void);
+
+    /**
+      * \brief Checks if the last action goal has been reached successfully.
+      *
+      * This function must be used to know whether the last action goal has
+      * been reached successfully or not, once it has finished.
+      *
+      * \return a BT:NodeStatus indicating whether the last action goal has
+      * been reached successfully (BT:NodeStatus::SUCCESS) or not
+      * (BT:NodeStatus::FAILURE), regardless of how it has failed.
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_success(void);
+
+    /**
+      * \brief Checks if the goal could not be started.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * goal could not be started.
+      *
+      * \return a BT:NodeStatus indicating whether the goal could not be started
+      * (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+      * (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_action_start_fail(void);
+
+    /**
+      * \brief Checks if the goal did not complete in the allowed time.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * goal did not complete in the allowed time.
+      *
+      * \return a BT:NodeStatus indicating whether the goal did not complete in
+      * the allowed time (BT:NodeStatus::SUCCESS) or if it failed due to another
+      * reason (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_timeout(void);
+
+    /**
+      * \brief Checks if the feedback has not been received for a long time.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * feedback has not been received for a long time.
+      *
+      * \return a BT:NodeStatus indicating whether the feedback has not been
+      * received for a long time (BT:NodeStatus::SUCCESS) or if it failed due
+      * to another reason (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_fb_watchdog(void);
+
+    /**
+      * \brief Checks if the action goal could not be reached.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * action goal could not be reached.
+      *
+      * \return a BT:NodeStatus indicating whether the action goal could not
+      * be reached (BT:NodeStatus::SUCCESS) or if it failed due to another
+      * reason (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_aborted(void);
+
+    /**
+      * \brief Checks if the current action goal has been cancelled by
+      * another goal.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * current action goal has been cancelled by another goal.
+      *
+      * \return a BT:NodeStatus indicating whether the current action goal
+      * has been cancelled by another goal (BT:NodeStatus::SUCCESS) or if it
+      * failed due to another reason (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_preempted(void);
+
+    /**
+      * \brief Checks if the goal information is not valid.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * goal information is not valid and the goal will not be executed.
+      *
+      * \return a BT:NodeStatus indicating whether the goal information is not
+      * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+      * (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_rejected(void);
+
+    /**
+      * \brief Checks if set_param function has failed.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because
+      * set_param function has failed.
+      *
+      * \return a BT:NodeStatus indicating whether the goal information is not
+      * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+      * (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_set_param_fail(void);
+
+    /**
+      * \brief Checks if the requested parameter is not present.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * requested parameter is not present.
+      *
+      * \return a BT:NodeStatus indicating whether the goal information is not
+      * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+      * (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_param_not_present(void);
+
+    /**
+      * \brief Checks if no odom messages have been received on a while.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because 
+      * no odom messages have been received on a while.
+      *
+      * \return a BT:NodeStatus indicating whether the goal information is not
+      * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+      * (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_no_odom(void);
+
+    /**
+      * \brief Checks if the requested transform does not exist.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * requested transform does not exist.
+      *
+      * \return a BT:NodeStatus indicating whether the goal information is not
+      * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+      * (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_no_transform(void);
+
+    /**
+      * \brief Checks if the map is unknown.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * map is unknown.
+      *
+      * \return a BT:NodeStatus indicating whether the goal information is not
+      * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+      * (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_unknown_map(void);
+
+    /**
+      * \brief Checks if the navigation mode is invalid.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because the
+      * navigation mode is invalid.
+      *
+      * \return a BT:NodeStatus indicating whether the goal information is not
+      * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+      * (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_nav_module_invalid_mode(void);
+
+  };
+
+  #endif
diff --git a/package.xml b/package.xml
index b902432d97687fba55446aecb9b6f9e26c66969d..0ad29cccdb388ef1cae171abece98e6e3bf621f2 100644
--- a/package.xml
+++ b/package.xml
@@ -51,6 +51,7 @@
   <build_depend>pal_waypoint_msgs</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>iri_behaviortree</build_depend>
   <!-- new dependencies -->
   <!--<build_depend>new build dependency</build_depend>-->
   <run_depend>iri_ros_tools</run_depend>
@@ -64,6 +65,7 @@
   <run_depend>pal_waypoint_msgs</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>iri_behaviortree</run_depend>
   <!-- new dependencies -->
   <!--<run_depend>new run dependency</run_depend>-->
 
diff --git a/src/tiago_nav_bt_module.cpp b/src/tiago_nav_bt_module.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d5a7a066ca567bb3db4b97f6fff7eba86a976ca9
--- /dev/null
+++ b/src/tiago_nav_bt_module.cpp
@@ -0,0 +1,336 @@
+#include <tiago_nav_module/tiago_nav_bt_module.h>
+#include <tiago_nav_module/tiago_nav_module.h>
+
+CTiagoNavModuleBT::CTiagoNavModuleBT() :
+  tiago_nav_module("tiago_nav_module",ros::this_node::getName())
+{
+
+}
+
+void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
+{
+  //Definition of ports
+  BT::PortsList sync_go_to_pose_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol")};
+  BT::PortsList async_go_to_pose_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("new_goal")};
+
+  BT::PortsList set_goal_frame_ports = {BT::InputPort<std::string>("frame_id")};
+
+  //Registration of conditions
+  factory.registerSimpleCondition("is_tiago_nav_finished",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_finished, this));
+
+  factory.registerSimpleCondition("is_tiago_nav_module_running",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_module_running, this));
+  factory.registerSimpleCondition("is_tiago_nav_module_success",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_module_success, this));
+  factory.registerSimpleCondition("is_tiago_nav_module_action_start_fail",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_module_action_start_fail, this));
+  factory.registerSimpleCondition("is_tiago_nav_module_timeout",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_module_timeout, this));
+  factory.registerSimpleCondition("is_tiago_nav_module_fb_watchdog",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_module_fb_watchdog, this));
+  factory.registerSimpleCondition("is_tiago_nav_module_aborted",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_module_aborted, this));
+  factory.registerSimpleCondition("is_tiago_nav_module_preempted",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_module_preempted, this));
+  factory.registerSimpleCondition("is_tiago_nav_module_rejected",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_module_rejected, this));
+
+  //Registration of actions
+  factory.registerIriAsyncAction("async_is_tiago_nav_finished",  std::bind(&CTiagoNavModuleBT::async_is_tiago_nav_finished, this));
+
+  factory.registerSimpleAction("sync_cancel_tiago_nav_action",  std::bind(&CTiagoNavModuleBT::sync_cancel_tiago_nav_action, this));
+  factory.registerSimpleAction("async_cancel_tiago_nav_action",  std::bind(&CTiagoNavModuleBT::async_cancel_tiago_nav_action, this));
+
+  factory.registerSimpleAction("sync_tiago_nav_go_to_pose",  std::bind(&CTiagoNavModuleBT::sync_tiago_nav_go_to_pose, this, std::placeholders::_1), sync_go_to_pose_ports);
+  factory.registerIriAsyncAction("async_tiago_nav_go_to_pose",  std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_pose, this, std::placeholders::_1), async_go_to_pose_ports);
+  
+  factory.registerSimpleAction("set_tiago_nav_goal_frame",  std::bind(&CTiagoNavModuleBT::set_tiago_nav_goal_frame, this, std::placeholders::_1), set_goal_frame_ports);
+}
+
+CTiagoNavModuleBT::~CTiagoNavModuleBT(void)
+{
+  // [free dynamic memory]
+}
+
+BT::NodeStatus CTiagoNavModuleBT::sync_tiago_nav_go_to_pose(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::sync_tiago_nav_go_to_pose-> sync_tiago_nav_go_to_pose");
+  BT::Optional<double> x = self.getInput<double>("x");
+  BT::Optional<double> y = self.getInput<double>("y");
+  BT::Optional<double> yaw = self.getInput<double>("yaw");
+  BT::Optional<double> heading_tol = self.getInput<double>("heading_tol");
+  BT::Optional<double> x_y_pos_tol = self.getInput<double>("x_y_pos_tol");
+
+  double x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal;
+  if (!x || !y || !yaw || (!heading_tol && x_y_pos_tol))
+  {
+    ROS_ERROR("CTiagoNavModuleBT::sync_tiago_nav_go_to_pose-> Incorrect or missing input. It needs the following input ports: x(double), y(double), yaw(double), [Optional] heading_tol(double) and [Optional] x_y_pos_tol(double)");
+    ROS_ERROR("CTiagoNavModuleBT::sync_tiago_nav_go_to_pose-> The correct input options are: x + y + yaw, x + y + yaw + heading_tol or x + y + yaw + heading_tol + x_y_pos_tol");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  x_goal = x.value();
+  y_goal = y.value();
+  yaw_goal = yaw.value();
+  if (!x_y_pos_tol)
+  {
+    if (!heading_tol)
+    {
+      if (!this->tiago_nav_module.go_to_pose(x_goal, y_goal, yaw_goal))
+        return BT::NodeStatus::FAILURE;
+    }
+    else
+    {
+      heading_tol_goal = heading_tol.value();
+      if (!this->tiago_nav_module.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal))
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+  else
+  {
+    heading_tol_goal = heading_tol.value();
+    x_y_pos_tol_goal = x_y_pos_tol.value();
+    if (!this->tiago_nav_module.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal))
+      return BT::NodeStatus::FAILURE;
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::async_tiago_nav_go_to_pose(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::async_tiago_nav_go_to_pose-> async_tiago_nav_go_to_pose");
+  BT::Optional<double> x = self.getInput<double>("x");
+  BT::Optional<double> y = self.getInput<double>("y");
+  BT::Optional<double> yaw = self.getInput<double>("yaw");
+  BT::Optional<double> heading_tol = self.getInput<double>("heading_tol");
+  BT::Optional<double> x_y_pos_tol = self.getInput<double>("x_y_pos_tol");
+  BT::Optional<bool> new_goal_in = self.getInput<bool>("new_goal");
+
+  bool new_goal;
+  if (!new_goal_in)
+    new_goal = false;
+  else
+    new_goal = new_goal_in.value();
+
+  if (new_goal)
+  {
+    double x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal;
+    if (!x || !y || !yaw || (!heading_tol && x_y_pos_tol))
+    {
+      ROS_ERROR("CTiagoNavModuleBT::sync_tiago_nav_go_to_pose-> Incorrect or missing input. It needs the following input ports: x(double), y(double), yaw(double), [Optional] heading_tol(double) and [Optional] x_y_pos_tol(double)");
+      ROS_ERROR("CTiagoNavModuleBT::sync_tiago_nav_go_to_pose-> The correct input options are: x + y + yaw, x + y + yaw + heading_tol or x + y + yaw + heading_tol + x_y_pos_tol");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    x_goal = x.value();
+    y_goal = y.value();
+    yaw_goal = yaw.value();
+    if (!x_y_pos_tol)
+    {
+      if (!heading_tol)
+      {
+        if (!this->tiago_nav_module.go_to_pose(x_goal, y_goal, yaw_goal))
+          return BT::NodeStatus::FAILURE;
+      }
+      else
+      {
+        heading_tol_goal = heading_tol.value();
+        if (!this->tiago_nav_module.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal))
+          return BT::NodeStatus::FAILURE;
+      }
+    }
+    else
+    {
+      heading_tol_goal = heading_tol.value();
+      x_y_pos_tol_goal = x_y_pos_tol.value();
+      if (!this->tiago_nav_module.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal))
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+  if (this->tiago_nav_module.is_finished())
+  {
+    tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+    if (tiago_nav_module_status == TIAGO_NAV_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  if (new_goal)
+    self.setOutput("new_goal", false);
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::set_tiago_nav_goal_frame(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::set_tiago_nav_goal_frame-> set_tiago_nav_goal_frame");
+  BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
+
+  std::string frame_id_goal;
+  if (!frame_id)
+  {
+    ROS_ERROR("CTiagoNavModuleBT::set_tiago_nav_goal_frame-> Incorrect or missing input. It needs the following input ports: frame_id(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  frame_id_goal = frame_id.value();
+
+  this->tiago_nav_module.set_goal_frame(frame_id_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::sync_cancel_tiago_nav_action(void)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::sync_cancel_tiago_nav_action-> sync_cancel_tiago_nav_action");
+  this->tiago_nav_module.stop();
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::async_cancel_tiago_nav_action(void)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::async_cancel_tiago_nav_action-> async_cancel_tiago_nav_action");
+  this->tiago_nav_module.stop();
+  if (this->tiago_nav_module.is_finished())
+  {
+    tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+    if (tiago_nav_module_status == TIAGO_NAV_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_finished(void)
+{
+  if (this->tiago_nav_module.is_finished())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::async_is_tiago_nav_finished(void)
+{
+  if (this->tiago_nav_module.is_finished())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_running(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_RUNNING)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_success(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_SUCCESS)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_action_start_fail(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_ACTION_START_FAIL)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_timeout(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_TIMEOUT)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_fb_watchdog(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_FB_WATCHDOG)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_aborted(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_ABORTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_preempted(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_PREEMPTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_rejected(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_REJECTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_set_param_fail(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_SET_PARAM_FAIL)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_param_not_present(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_PARAM_NOT_PRESENT)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_no_odom(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_NO_ODOM)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_no_transform(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_NO_TRANSFORM)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_unknown_map(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_UNKNOWN_MAP)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::is_tiago_nav_module_invalid_mode(void)
+{
+  tiago_nav_module_status_t tiago_nav_module_status = this->tiago_nav_module.get_status();
+
+  if (tiago_nav_module_status == TIAGO_NAV_MODULE_INVALID_MODE)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+