Added BT module layer

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 trajectory_msgs std_srvs control_msgs iri_base_algorithm)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib trajectory_msgs std_srvs control_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -102,7 +102,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES gripper_module 
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib trajectory_msgs std_srvs control_msgs iri_base_algorithm
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib trajectory_msgs std_srvs control_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree
 
 #  DEPENDS system_lib
 )
@@ -153,6 +153,13 @@ add_dependencies(${module_name} ${${PROJECT_NAME}_EXPORTED_TARGETS})
 #   ${catkin_LIBRARIES}
 # )
 
+set(module_bt_name tiago_gripper_module_bt)
+add_library(${module_bt_name} src/tiago_gripper_module_bt.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})
+
 set(client_name tiago_gripper_client)
 add_executable(${client_name} src/tiago_gripper_client_alg.cpp src/tiago_gripper_client_alg_node.cpp)
 

include/tiago_gripper_module/tiago_gripper_module.h

@@ -90,8 +90,6 @@ class CTiagoGripperModule : public CModule<tiago_gripper_module::TiagoGripperMod
     /** \brief Function to stop gripper movement action
      */
     void stop(void);
-//?????
-    void set_current_limit();
     /** \brief Function to check if process is finished
         \return bool
     */

include/tiago_gripper_module/tiago_gripper_module_bt.h

+#ifndef _IRI_TIAGO_GRIPPER_MODULE_BT_H
+#define _IRI_TIAGO_GRIPPER_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+
+#include "iri_bt_factory.h"
+#include <tiago_gripper_module/tiago_gripper_module.h>
+
+class CTiagoGripperModuleBT
+{
+  private:
+    // object of tiago_gripper_module
+    CTiagoGripperModule tiago_gripper_module;
+
+  public:
+    /**
+      * \brief Constructor
+      *
+      */
+    CTiagoGripperModuleBT();
+
+    /**
+      * \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.
+    */
+    ~CTiagoGripperModuleBT();
+
+  protected:
+
+    /**
+      * \brief Synchronized close TIAGo gripper function.
+      *
+      * This function calls close of tiago_gripper_module. As this is a
+      * synchronous action is_tiago_gripper_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   duration (double): Time from start to close the gripper.
+      *
+      * \param self node with the required inputs 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_close_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Async close TIAGo gripper function.
+      *
+      * This function calls close of tiago_gripper_module. As this is
+      * an asynchronous action is_tiago_gripper_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   duration (double): Time from start to close the gripper.
+      *
+      * 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_close_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized open TIAGo gripper function.
+      *
+      * This function calls open of tiago_gripper_module. As this is a
+      * synchronous action is_tiago_gripper_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   duration (double): Time from start to open the gripper.
+      *
+      * \param self node with the required inputs 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_open_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Async open TIAGo gripper function.
+      *
+      * This function calls open of tiago_gripper_module. As this is
+      * an asynchronous action is_tiago_gripper_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   duration (double): Time from start to open the gripper.
+      *
+      * 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_open_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized move_fingers TIAGo gripper function.
+      *
+      * This function calls move_fingers of tiago_gripper_module. As this is a
+      * synchronous action is_tiago_gripper_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   left_finger (double): Left finger position.
+      *
+      *   right_finger (double): Right finger position.
+      *
+      *   duration (double): Time from start to move the gripper. 
+      *
+      * \param self node with the required inputs 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_move_fingers_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Async move_fingers TIAGo gripper function.
+      *
+      * This function calls move_fingers of tiago_gripper_module. As this is
+      * an asynchronous action is_tiago_gripper_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   left_finger (double): Left finger position.
+      *
+      *   right_finger (double): Right finger position.
+      *
+      *   duration (double): Time from start to move the gripper. 
+      *
+      * 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_move_fingers_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized move_fingers TIAGo gripper function.
+      *
+      * This function calls move_fingers of tiago_gripper_module. As this is a
+      * synchronous action is_tiago_gripper_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   left_finger (std::vector<double>): Left finger position.
+      *
+      *   right_finger (std::vector<double>): Right finger position.
+      *
+      *   duration (std::vector<double>): Time from start to move the gripper. 
+      *
+      * \param self node with the required inputs 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_move_fingers_multi_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Async move_fingers TIAGo gripper function.
+      *
+      * This function calls move_fingers of tiago_gripper_module. As this is
+      * an asynchronous action is_tiago_gripper_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   left_finger (std::vector<double>): Left finger position.
+      *
+      *   right_finger (std::vector<double>): Right finger position.
+      *
+      *   duration (std::vector<double>): Time from start to move the gripper.  
+      *
+      * 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_move_fingers_multi_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized fingers_distance TIAGo gripper function.
+      *
+      * This function calls fingers_distance of tiago_gripper_module. As this is a
+      * synchronous action is_tiago_gripper_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   distance (double): Distance between fingers.
+      *
+      *   duration (std::vector<double>): Time from start to move the gripper. 
+      *
+      * \param self node with the required inputs 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_fingers_distance_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Async fingers_distance TIAGo gripper function.
+      *
+      * This function calls fingers_distance of tiago_gripper_module. As this is
+      * an asynchronous action is_tiago_gripper_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   distance (double): Distance between fingers.
+      *
+      *   duration (std::vector<double>): Time from start to move the gripper.  
+      *
+      * 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_fingers_distance_tiago_gripper(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized stop TIAGo gripperfunction.
+      *
+      * This function calls stop of tiago_gripper_module. As this is a
+      * synchronous action is_tiago_gripper_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 cancel_tiago_gripper_action(void);
+
+    /**
+      * \brief Checks if the module is idle and ther isn't any new movement or grasp request.
+      *
+      * This function must be used when any sync action is called.
+      *
+      * \return a BT:NodeStatus indicating whether the last movement or grasp has
+      * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE),
+      * regardless of its success or not.
+      *
+      */
+    BT::NodeStatus is_tiago_gripper_finished(void);
+
+    /**
+      * \brief Checks if the module is idle and ther isn't any new movement or grasping 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 or grasp has
+      * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::RUNNING),
+      * regardless of its success or not.
+      *
+      */
+    BT::NodeStatus async_is_tiago_gripper_finished(void);
+
+    ///TIAGo gripper 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_gripper_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_gripper_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_gripper_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_gripper_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_gripper_module_fb_watchdog(void);
+
+    /**
+      * \brief Checks if the navigation 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
+      * navigation goal could not be reached.
+      *
+      * \return a BT:NodeStatus indicating whether the navigation goal could not
+      * be reached (BT:NodeStatus::SUCCESS) or if it failed due to another
+      * reason (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_tiago_gripper_module_aborted(void);
+
+    /**
+      * \brief Checks if the current navigation 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 navigation goal has been cancelled by another goal.
+      *
+      * \return a BT:NodeStatus indicating whether the current navigation 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_gripper_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_gripper_module_rejected(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_gripper_module_failed_grasp(void);
+
+  };
+
+  #endif

package.xml

@@ -48,6 +48,8 @@
   <build_depend>std_srvs</build_depend>
   <build_depend>control_msgs</build_depend>
   <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>iri_base_bt_client</build_depend>
+  <build_depend>iri_behaviortree</build_depend>
   <!-- new dependencies -->
   <!--<build_depend>new build dependency</build_depend>-->
   <run_depend>iri_ros_tools</run_depend>
@@ -58,6 +60,8 @@
   <run_depend>std_srvs</run_depend>
   <run_depend>control_msgs</run_depend>
   <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>iri_base_bt_client</run_depend>
+  <run_depend>iri_behaviortree</run_depend>
   <!-- new dependencies -->
   <!--<run_depend>new run dependency</run_depend>-->
 

src/tiago_gripper_module_bt.cpp

+#include <tiago_gripper_module/tiago_gripper_module_bt.h>
+#include <tiago_gripper_module/tiago_gripper_module.h>
+
+CTiagoGripperModuleBT::CTiagoGripperModuleBT() :
+  tiago_gripper_module("tiago_gripper_module",ros::this_node::getName())
+{
+
+}
+
+void CTiagoGripperModuleBT::init(IriBehaviorTreeFactory &factory)
+{
+  //Definition of ports
+  BT::PortsList sync_open_close_ports = {BT::InputPort<double>("duration")};
+  BT::PortsList async_open_close_ports = {BT::InputPort<double>("duration"), BT::BidirectionalPort<bool>("new_goal")};
+
+  BT::PortsList sync_move_fingers_ports = {BT::InputPort<double>("left_finger"), BT::InputPort<double>("right_finger"), BT::InputPort<double>("duration")};
+  BT::PortsList async_move_fingers_ports = {BT::InputPort<double>("left_finger"), BT::InputPort<double>("right_finger"), BT::InputPort<double>("duration"), BT::BidirectionalPort<bool>("new_goal")};
+
+  BT::PortsList sync_move_fingers_multi_ports = {BT::InputPort<std::vector<std::string>>("left_finger"), BT::InputPort<std::vector<double>>("right_finger"), BT::InputPort<std::vector<double>>("duration")};
+  BT::PortsList async_move_fingers_multi_ports = {BT::InputPort<std::vector<std::string>>("left_finger"), BT::InputPort<std::vector<double>>("right_finger"), BT::InputPort<std::vector<double>>("duration"), BT::BidirectionalPort<bool>("new_goal")};
+
+  BT::PortsList sync_fingers_distance_ports = {BT::InputPort<double>("distance"), BT::InputPort<double>("duration")};
+  BT::PortsList async_fingers_distance_ports = {BT::InputPort<double>("distance"), BT::InputPort<double>("duration"), BT::BidirectionalPort<bool>("new_goal")};
+
+  //Registration of conditions
+  factory.registerSimpleCondition("is_tiago_gripper_finished",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_finished, this));
+
+  factory.registerSimpleCondition("is_tiago_gripper_module_running",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_running, this));
+  factory.registerSimpleCondition("is_tiago_gripper_module_success",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_success, this));
+  factory.registerSimpleCondition("is_tiago_gripper_module_action_start_fail",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_action_start_fail, this));
+  factory.registerSimpleCondition("is_tiago_gripper_module_timeout",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_timeout, this));
+  factory.registerSimpleCondition("is_tiago_gripper_module_fb_watchdog",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_fb_watchdog, this));
+  factory.registerSimpleCondition("is_tiago_gripper_module_aborted",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_aborted, this));
+  factory.registerSimpleCondition("is_tiago_gripper_module_preempted",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_preempted, this));
+  factory.registerSimpleCondition("is_tiago_gripper_module_rejected",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_rejected, this));
+  factory.registerSimpleCondition("is_tiago_gripper_module_failed_grasp",  std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_failed_grasp, this));
+
+  //Registration of actions
+  factory.registerSimpleAction("cancel_tiago_gripper_action",  std::bind(&CTiagoGripperModuleBT::cancel_tiago_gripper_action, this));
+  factory.registerIriAsyncAction("async_is_tiago_gripper_finished",  std::bind(&CTiagoGripperModuleBT::async_is_tiago_gripper_finished, this));
+
+  factory.registerSimpleAction("sync_close_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::sync_close_tiago_gripper, this, std::placeholders::_1), sync_open_close_ports);
+  factory.registerIriAsyncAction("async_close_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::async_close_tiago_gripper, this, std::placeholders::_1), async_open_close_ports);
+
+  factory.registerSimpleAction("sync_open_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::sync_open_tiago_gripper, this, std::placeholders::_1), sync_open_close_ports);
+  factory.registerIriAsyncAction("async_open_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::async_open_tiago_gripper, this, std::placeholders::_1), async_open_close_ports);
+
+  factory.registerSimpleAction("sync_move_fingers_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::sync_move_fingers_tiago_gripper, this, std::placeholders::_1), sync_move_fingers_ports);
+  factory.registerIriAsyncAction("async_move_fingers_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::async_move_fingers_tiago_gripper, this, std::placeholders::_1), async_move_fingers_ports);
+
+  factory.registerSimpleAction("sync_move_fingers_multi_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::sync_move_fingers_multi_tiago_gripper, this, std::placeholders::_1), sync_move_fingers_multi_ports);
+  factory.registerIriAsyncAction("async_move_fingers_multi_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::async_move_fingers_multi_tiago_gripper, this, std::placeholders::_1), async_move_fingers_multi_ports);
+
+  factory.registerSimpleAction("sync_fingers_distance_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::sync_fingers_distance_tiago_gripper, this, std::placeholders::_1), sync_fingers_distance_ports);
+  factory.registerIriAsyncAction("async_fingers_distance_tiago_gripper",  std::bind(&CTiagoGripperModuleBT::async_fingers_distance_tiago_gripper, this, std::placeholders::_1), async_fingers_distance_ports);
+}
+
+CTiagoGripperModuleBT::~CTiagoGripperModuleBT(void)
+{
+  // [free dynamic memory]
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::sync_close_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::sync_close_tiago_gripper-> sync_close_tiago_gripper");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+
+  double duration_goal;
+  if (!duration)
+    duration_goal = -1.0;
+  else
+    duration_goal = duration.value();
+
+  this->tiago_gripper_module.close(duration_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::async_close_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::async_close_tiago_gripper-> async_close_tiago_gripper");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+  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 duration_goal;
+    if (!duration)
+      duration_goal = -1.0;
+    else
+      duration_goal = duration.value();
+
+    self.setOutput("new_goal", false);
+    this->tiago_gripper_module.close(duration_goal);
+  }
+  if (this->tiago_gripper_module.is_finished())
+  {
+    tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+    if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::sync_open_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::sync_open_tiago_gripper-> sync_open_tiago_gripper");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+
+  double duration_goal;
+  if (!duration)
+    duration_goal = -1.0;
+  else
+    duration_goal = duration.value();
+
+  this->tiago_gripper_module.open(duration_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::async_open_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::async_open_tiago_gripper-> async_open_tiago_gripper");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+  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 duration_goal;
+    if (!duration)
+      duration_goal = -1.0;
+    else
+      duration_goal = duration.value();
+
+    self.setOutput("new_goal", false);
+    this->tiago_gripper_module.open(duration_goal);
+  }
+  if (this->tiago_gripper_module.is_finished())
+  {
+    tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+    if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::sync_move_fingers_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::sync_move_fingers_tiago_gripper-> sync_move_fingers_tiago_gripper");
+  BT::Optional<double> left_finger = self.getInput<double>("left_finger");
+  BT::Optional<double> right_finger = self.getInput<double>("right_finger");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+
+  if (!left_finger || !right_finger)
+  {
+    ROS_ERROR("CTiagoGripperModuleBT::sync_move_fingers_tiago_gripper-> Incorrect or missing input. It needs the following input ports: left_finger(double), right_finger(double) and duration(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  double left_finger_goal, right_finger_goal;
+  left_finger_goal = left_finger.value();
+  right_finger_goal = right_finger.value();
+
+  double duration_goal;
+  if (!duration)
+    duration_goal = -1.0;
+  else
+    duration_goal = duration.value();
+
+  this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::async_move_fingers_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::async_move_fingers_tiago_gripper-> async_move_fingers_tiago_gripper");
+  BT::Optional<double> left_finger = self.getInput<double>("left_finger");
+  BT::Optional<double> right_finger = self.getInput<double>("right_finger");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+  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)
+  {
+    if (!left_finger || !right_finger)
+    {
+      ROS_ERROR("CTiagoGripperModuleBT::async_move_fingers_tiago_gripper-> Incorrect or missing input. It needs the following input ports: left_finger(double), right_finger(double) and duration(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    double left_finger_goal, right_finger_goal;
+    left_finger_goal = left_finger.value();
+    right_finger_goal = right_finger.value();
+
+    double duration_goal;
+    if (!duration)
+      duration_goal = -1.0;
+    else
+      duration_goal = duration.value();
+
+    self.setOutput("new_goal", false);
+    this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
+  }
+  if (this->tiago_gripper_module.is_finished())
+  {
+    tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+    if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::sync_move_fingers_multi_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::sync_move_fingers_multi_tiago_gripper-> sync_move_fingers_multi_tiago_gripper");
+  BT::Optional<std::vector<double> > left_finger = self.getInput<std::vector<double> >("left_finger");
+  BT::Optional<std::vector<double> > right_finger = self.getInput<std::vector<double> >("right_finger");
+  BT::Optional<std::vector<double> > duration = self.getInput<std::vector<double> >("duration");
+
+  if (!left_finger || !right_finger || !duration)
+  {
+    ROS_ERROR("CTiagoGripperModuleBT::sync_move_fingers_multi_tiago_gripper-> Incorrect or missing input. It needs the following input ports: left_finger(std::vector<double>), right_finger(std::vector<double>) and duration(std::vector<double>)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  std::vector<double> left_finger_goal, right_finger_goal, duration_goal;
+  left_finger_goal = left_finger.value();
+  right_finger_goal = right_finger.value();
+  duration_goal = duration.value();
+
+  this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::async_move_fingers_multi_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::async_move_fingers_multi_tiago_gripper-> async_move_fingers_multi_tiago_gripper");
+  BT::Optional<std::vector<double> > left_finger = self.getInput<std::vector<double> >("left_finger");
+  BT::Optional<std::vector<double> > right_finger = self.getInput<std::vector<double> >("right_finger");
+  BT::Optional<std::vector<double> > duration = self.getInput<std::vector<double> >("duration");
+  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)
+  {
+    if (!left_finger || !right_finger || !duration)
+    {
+      ROS_ERROR("CTiagoGripperModuleBT::async_move_fingers_multi_tiago_gripper-> Incorrect or missing input. It needs the following input ports: left_finger(std::vector<double>), right_finger(std::vector<double>) and duration(std::vector<double>)");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    std::vector<double> left_finger_goal, right_finger_goal, duration_goal;
+    left_finger_goal = left_finger.value();
+    right_finger_goal = right_finger.value();
+    duration_goal = duration.value();
+
+    self.setOutput("new_goal", false);
+    this->tiago_gripper_module.move_fingers(left_finger_goal, right_finger_goal, duration_goal);
+  }
+  if (this->tiago_gripper_module.is_finished())
+  {
+    tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+    if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::sync_fingers_distance_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::sync_fingers_distance_tiago_gripper-> sync_fingers_distance_tiago_gripper");
+  BT::Optional<double> distance = self.getInput<double>("distance");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+
+  if (!distance)
+  {
+    ROS_ERROR("CTiagoGripperModuleBT::sync_fingers_distance_tiago_gripper-> Incorrect or missing input. It needs the following input ports: distance(double) and duration(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  double distance_goal;
+  distance_goal = distance.value();
+
+  double duration_goal;
+  if (!duration)
+    duration_goal = -1.0;
+  else
+    duration_goal = duration.value();
+
+  this->tiago_gripper_module.fingers_distance(distance_goal, duration_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::async_fingers_distance_tiago_gripper(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::async_fingers_distance_tiago_gripper-> async_fingers_distance_tiago_gripper");
+  BT::Optional<double> distance = self.getInput<double>("distance");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+  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)
+  {
+    if (!distance)
+    {
+      ROS_ERROR("CTiagoGripperModuleBT::async_fingers_distance_tiago_gripper-> Incorrect or missing input. It needs the following input ports: distance(double) and duration(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    double distance_goal;
+    distance_goal = distance.value();
+
+    double duration_goal;
+    if (!duration)
+      duration_goal = -1.0;
+    else
+      duration_goal = duration.value();
+
+    self.setOutput("new_goal", false);
+    this->tiago_gripper_module.fingers_distance(distance_goal, duration_goal);
+  }
+  if (this->tiago_gripper_module.is_finished())
+  {
+    tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+    if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::cancel_tiago_gripper_action(void)
+{
+  ROS_DEBUG("CTiagoGripperModuleBT::cancel_tiago_gripper_action-> cancel_tiago_gripper_action");
+  this->tiago_gripper_module.stop();
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_finished(void)
+{
+  if (this->tiago_gripper_module.is_finished())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::async_is_tiago_gripper_finished(void)
+{
+  if (this->tiago_gripper_module.is_finished())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_running(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_RUNNING)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_success(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_SUCCESS)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_action_start_fail(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_ACTION_START_FAIL)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_timeout(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_TIMEOUT)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_fb_watchdog(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_FB_WATCHDOG)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_aborted(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_ABORTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_preempted(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_PREEMPTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_rejected(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_REJECTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_module_failed_grasp(void)
+{
+  tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status();
+
+  if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_FAILED_GRASP)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}