Added Point_to(PointStamped), cancel and conditions to the module BT 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 control_msgs iri_base_algorithm)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs iri_base_algorithm 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 tiago_head_module
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs iri_base_algorithm
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib control_msgs iri_base_algorithm iri_behaviortree
 
 #  DEPENDS system_lib
 )
@@ -148,6 +148,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_head_client_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+set(module_bt_name tiago_head_module_bt)
+add_library(${module_bt_name} src/tiago_head_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 ##
 #############

include/tiago_head_module/tiago_head_bt_module.h

+#ifndef _IRI_TIAGO_HEAD_MODULE_BT_H
+#define _IRI_TIAGO_HEAD_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+
+#include "iri_bt_factory.h"
+#include <tiago_head_module/tiago_head_module.h>
+#include <sensor_msgs/JointState.h>
+#include <geometry_msgs/PointStamped.h>
+
+class CTiagoHeadModuleBT
+{
+  private:
+    // object of tiago_head_module
+    CTiagoHeadModule tiago_head_module;
+
+  public:
+    /**
+      * \brief Constructor
+      *
+      */
+    CTiagoHeadModuleBT();
+
+    /**
+      * \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.
+    */
+    ~CTiagoHeadModuleBT();
+
+  protected:
+
+    /**
+      * \brief Synchronized point_to TIAGo head function.
+      *
+      * This function calls point_to of tiago_head_module. As this is a
+      * synchronous action is_tiago_head_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   point (geometry_msgs::PointStamped): The target JointState
+      *
+      *   duration (double): [Optional] Time from start to move the head.
+      *
+      * \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_head_point_to_pointstamped(BT::TreeNode& self);
+
+    /**
+      * \brief Async point_to TIAGo head function.
+      *
+      * This function calls point_to of tiago_head_module. As this is
+      * an asynchronous action is_tiago_head_finished() is checked to know when the action
+      * has actually finished.
+      *
+      * It has the following input ports:
+      *
+      *   point (geometry_msgs::PointStamped): The target JointState
+      *
+      *   duration (double): [Optional] Time from start to move the head.
+      *
+      * 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_head_point_to_pointstamped(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized set_workspace TIAGo head function.
+      *
+      * This function calls set_workspace of tiago_head_module.
+      *
+      * It has the following input ports:
+      *
+      *   vel (double): Head maximum velocity.
+      *
+      * \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_head_max_velocity(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized stop TIAGo head function.
+      *
+      * This function calls stop of tiago_head_module. As this is a
+      * synchronous action is_tiago_head_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_head_action(void);
+
+    /**
+      * \brief Async stop TIAGo head function.
+      *
+      * This function calls stop of tiago_head_module. As this is
+      * an asynchronous action is_tiago_head_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_head_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_head_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_head_finished(void);
+
+    ///TIAGo head 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_head_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_head_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_head_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_head_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_head_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_head_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_head_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_head_module_rejected(void);
+
+  };
+
+  #endif

package.xml

@@ -43,6 +43,7 @@
   <build_depend>actionlib</build_depend>
   <build_depend>control_msgs</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>
@@ -51,6 +52,7 @@
   <run_depend>actionlib</run_depend>
   <run_depend>control_msgs</run_depend>
   <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>iri_behaviortree</run_depend>
   <!-- new dependencies -->
   <!--<run_depend>new run dependency</run_depend>-->
 

src/tiago_head_bt_module.cpp

+#include <tiago_head_module/tiago_head_bt_module.h>
+#include <tiago_head_module/tiago_head_module.h>
+
+CTiagoHeadModuleBT::CTiagoHeadModuleBT() :
+  tiago_head_module("tiago_head_module",ros::this_node::getName())
+{
+
+}
+
+void CTiagoHeadModuleBT::init(IriBehaviorTreeFactory &factory)
+{
+  //Definition of ports
+  BT::PortsList sync_head_point_to_pointstamped_ports = {BT::InputPort<geometry_msgs::PointStamped>("point"), BT::InputPort<double>("duration")};
+  BT::PortsList async_head_point_to_pointstamped_ports = {BT::InputPort<geometry_msgs::PointStamped>("point"), BT::InputPort<double>("duration"), BT::BidirectionalPort<bool>("new_goal")};
+
+  BT::PortsList set_max_velocity_ports = {BT::InputPort<double>("vel")};
+
+  //Registration of conditions
+  factory.registerSimpleCondition("is_tiago_head_finished",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_finished, this));
+
+  factory.registerSimpleCondition("is_tiago_head_module_running",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_module_running, this));
+  factory.registerSimpleCondition("is_tiago_head_module_success",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_module_success, this));
+  factory.registerSimpleCondition("is_tiago_head_module_action_start_fail",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_module_action_start_fail, this));
+  factory.registerSimpleCondition("is_tiago_head_module_timeout",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_module_timeout, this));
+  factory.registerSimpleCondition("is_tiago_head_module_fb_watchdog",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_module_fb_watchdog, this));
+  factory.registerSimpleCondition("is_tiago_head_module_aborted",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_module_aborted, this));
+  factory.registerSimpleCondition("is_tiago_head_module_preempted",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_module_preempted, this));
+  factory.registerSimpleCondition("is_tiago_head_module_rejected",  std::bind(&CTiagoHeadModuleBT::is_tiago_head_module_rejected, this));
+
+  //Registration of actions
+  factory.registerIriAsyncAction("async_is_tiago_head_finished",  std::bind(&CTiagoHeadModuleBT::async_is_tiago_head_finished, this));
+
+  factory.registerSimpleAction("sync_cancel_tiago_head_action",  std::bind(&CTiagoHeadModuleBT::sync_cancel_tiago_head_action, this));
+  factory.registerSimpleAction("async_cancel_tiago_head_action",  std::bind(&CTiagoHeadModuleBT::async_cancel_tiago_head_action, this));
+
+  factory.registerSimpleAction("sync_tiago_head_point_to_pointstamped",  std::bind(&CTiagoHeadModuleBT::sync_tiago_head_point_to_pointstamped, this, std::placeholders::_1), sync_head_point_to_pointstamped_ports);
+  factory.registerIriAsyncAction("async_tiago_head_point_to_pointstamped",  std::bind(&CTiagoHeadModuleBT::async_tiago_head_point_to_pointstamped, this, std::placeholders::_1), async_head_point_to_pointstamped_ports);
+  
+  factory.registerSimpleAction("set_tiago_head_max_velocity",  std::bind(&CTiagoHeadModuleBT::set_tiago_head_max_velocity, this, std::placeholders::_1), set_max_velocity_ports);
+}
+
+CTiagoHeadModuleBT::~CTiagoHeadModuleBT(void)
+{
+  // [free dynamic memory]
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::sync_tiago_head_point_to_pointstamped(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::sync_tiago_head_point_to_pointstamped-> sync_tiago_head_point_to_pointstamped");
+  BT::Optional<geometry_msgs::PointStamped> point = self.getInput<geometry_msgs::PointStamped>("point");
+  BT::Optional<double> duration = self.getInput<double>("duration");
+
+  geometry_msgs::PointStamped point_goal;
+  double duration_goal;
+  if (!point)
+  {
+    ROS_ERROR("CTiagoHeadModuleBT::sync_tiago_head_point_to_pointstamped-> Incorrect or missing input. It needs the following input ports: point(geometry_msgs::PointStamped) and [Optional] duration(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  point_goal = point.value();
+  if (!duration)
+    this->tiago_head_module.point_to(point_goal);
+  else
+  {
+    duration_goal = duration.value();
+    this->tiago_head_module.point_to(point_goal, duration_goal);
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::async_tiago_head_point_to_pointstamped(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::async_tiago_head_point_to_pointstamped-> async_tiago_head_point_to_pointstamped");
+  BT::Optional<geometry_msgs::PointStamped> point = self.getInput<geometry_msgs::PointStamped>("point");
+  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)
+  {
+    geometry_msgs::PointStamped point_goal;
+    double duration_goal;
+    if (!point)
+    {
+      ROS_ERROR("CTiagoHeadModuleBT::sync_tiago_head_move_to_joints-> Incorrect or missing input. It needs the following input ports: point(geometry_msgs::PointStamped) and [Optional] duration(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    point_goal = point.value();
+    if (!duration)
+      this->tiago_head_module.point_to(point_goal);
+    else
+    {
+      duration_goal = duration.value();
+      this->tiago_head_module.point_to(point_goal, duration_goal);
+    }
+  }
+  if (this->tiago_head_module.is_finished())
+  {
+    tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+    if (tiago_head_module_status == TIAGO_HEAD_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 CTiagoHeadModuleBT::set_tiago_head_max_velocity(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::set_tiago_head_max_velocity-> set_tiago_head_max_velocity");
+  BT::Optional<double> vel = self.getInput<double>("vel");
+
+  double vel_goal;
+  if (!vel)
+  {
+    ROS_ERROR("CTiagoHeadModuleBT::set_tiago_head_max_velocity-> Incorrect or missing input. It needs the following input ports: vel(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  vel_goal = vel.value();
+
+  this->tiago_head_module.set_max_velocity(vel_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::sync_cancel_tiago_head_action(void)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::sync_cancel_tiago_head_action-> sync_cancel_tiago_head_action");
+  this->tiago_head_module.stop();
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::async_cancel_tiago_head_action(void)
+{
+  ROS_DEBUG("CTiagoHeadModuleBT::async_cancel_tiago_head_action-> async_cancel_tiago_head_action");
+  this->tiago_head_module.stop();
+  if (this->tiago_head_module.is_finished())
+  {
+    tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+    if (tiago_head_module_status == TIAGO_HEAD_MODULE_SUCCESS)
+      return BT::NodeStatus::SUCCESS;
+    else
+      return BT::NodeStatus::FAILURE;
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_finished(void)
+{
+  if (this->tiago_head_module.is_finished())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::async_is_tiago_head_finished(void)
+{
+  if (this->tiago_head_module.is_finished())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_module_running(void)
+{
+  tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+
+  if (tiago_head_module_status == TIAGO_HEAD_MODULE_RUNNING)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_module_success(void)
+{
+  tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+
+  if (tiago_head_module_status == TIAGO_HEAD_MODULE_SUCCESS)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_module_action_start_fail(void)
+{
+  tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+
+  if (tiago_head_module_status == TIAGO_HEAD_MODULE_ACTION_START_FAIL)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_module_timeout(void)
+{
+  tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+
+  if (tiago_head_module_status == TIAGO_HEAD_MODULE_TIMEOUT)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_module_fb_watchdog(void)
+{
+  tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+
+  if (tiago_head_module_status == TIAGO_HEAD_MODULE_FB_WATCHDOG)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_module_aborted(void)
+{
+  tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+
+  if (tiago_head_module_status == TIAGO_HEAD_MODULE_ABORTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_module_preempted(void)
+{
+  tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+
+  if (tiago_head_module_status == TIAGO_HEAD_MODULE_PREEMPTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoHeadModuleBT::is_tiago_head_module_rejected(void)
+{
+  tiago_head_module_status_t tiago_head_module_status = this->tiago_head_module.get_status();
+
+  if (tiago_head_module_status == TIAGO_HEAD_MODULE_REJECTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}