Added BT module functions

include/iri_joints_module/iri_joints_module.h

@@ -90,7 +90,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
       *
       */
     bool new_point_goal;
-    bool cancel_poin_to;
+    bool cancel_point_to_pending;
 
     // dynamic reconfigure
     /**
@@ -174,7 +174,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
       *
       * \return True if finished.
       */
-    bool point_to_finished(void);
+    bool is_point_to_finished(void);
 
     /**
       * \brief Function to change the inverse kinematics solver tolerance.

include/iri_joints_module/iri_joints_module_bt.h

@@ -6,11 +6,6 @@
 #include <behaviortree_cpp_v3/behavior_tree.h>
 #include "behaviortree_cpp_v3/blackboard.h"
 
-#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
-#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
-#include "behaviortree_cpp_v3/loggers/bt_minitrace_logger.h"
-#include "behaviortree_cpp_v3/loggers/bt_file_logger.h"
-
 #include "iri_bt_factory.h"
 #include <iri_joints_module/iri_joints_module.h>
 
@@ -49,6 +44,273 @@ class CIriJointsModuleBT
 
   protected:
 
+    /**
+      * \brief Synchronized point_to function.
+      *
+      * This function calls point_to of iri_joints_module and waits to is_finished.
+      *
+      * It has the following input ports:
+      *
+      *   x (double): Point X coordenate on its reference frame.
+      *
+      *   y (double): Point Y coordenate on its reference frame.
+      *
+      *   z (double): Point Z coordenate on its reference frame.
+      *
+      *   max_vel (double): The maximum velocity for the servos.
+      *
+      * \param self node with the required inputs defined as follows:
+      *
+      * \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_point_to(BT::TreeNode& self);
+
+    /**
+      * \brief Async point_to function.
+      *
+      * This function calls point_to of iri_joints_module without waiting to is_finished.
+      *
+      * It has the following input ports:
+      *
+      *   x (double): Point X coordenate on its reference frame.
+      *
+      *   y (double): Point Y coordenate on its reference frame.
+      *
+      *   z (double): Point Z coordenate on its reference frame.
+      *
+      *   max_vel (double): The maximum velocity for the servos.
+      *
+      * \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_point_to(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized cancel_point_to function.
+      *
+      * This function calls cancel_point_to of iri_joints_module and waits to be 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_point_to(void);
+
+    /**
+      * \brief Synchronized set_tolerance function.
+      *
+      * This function calls set_tolerance of iri_joints_module and waits to be finished.
+      *
+      * It has the following input ports:
+      *
+      *   tol (double): Inverse kinematic solver tolerance.
+      *
+      * \param self node with the required inputs defined as follows:
+      *
+      * \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_tolerance(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized set_base_frame function.
+      *
+      * This function calls set_base_frame of iri_joints_module and waits to be finished.
+      *
+      * It has the following input ports:
+      *
+      *   base_frame (string): Servo kinematic chain base frame.
+      *
+      * \param self node with the required inputs defined as follows:
+      *
+      * \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_base_frame(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized set_urdf_param function.
+      *
+      * This function calls set_urdf_param of iri_joints_module and waits to be finished.
+      *
+      * It has the following input ports:
+      *
+      *   urdf_param (string): Ros param where is stored the urdf model.
+      *
+      * \param self node with the required inputs defined as follows:
+      *
+      * \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_urdf_param(BT::TreeNode& self);
+
+    /**
+      * \brief Checks if the last point_to goal has finished or not.
+      *
+      * This function must be used when sync_point_to is called.
+      *
+      * \return a BT:NodeStatus indicating whether the last point_to goal has
+      * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE),
+      * regardless of its success or not.
+      *
+      */
+    BT::NodeStatus is_point_to_finished(void);
+
+    /**
+      * \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_point_to_running(void);
+
+    /**
+      * \brief Checks if the last point_to goal has been reached successfully.
+      *
+      * This function must be used to know whether the last point_to goal has
+      * been reached successfully or not, once it has finished.
+      *
+      * \return a BT:NodeStatus indicating whether the last point_to goal has
+      * been reached successfully (BT:NodeStatus::SUCCESS) or not
+      * (BT:NodeStatus::FAILURE), regardless of how it has failed.
+      *
+      */
+    BT::NodeStatus is_point_to_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_point_to_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_point_to_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_point_to_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_point_to_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_point_to_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_point_to_rejected(void);
+
+    /**
+      * \brief Checks if it was impossible to set the value of a parameter.
+      *
+      * This function must be used to know the type of failure, once the last
+      * goal has finished. Specifically, it checks if it failed because it was
+      * impossible to set the value of a parameter.
+      *
+      * \return a BT:NodeStatus indicating whether it was impossible to set the
+      * value of a parameter (BT:NodeStatus::SUCCESS) or if it failed due to
+      * another reason (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_point_to_set_param_fail(void);
+
+    /**
+      * \brief Checks if the parameter to be changed is not present in the
+      * remote node.
+      *
+      * 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
+      * parameter to be changed is not present in the remote node.
+      *
+      * \return a BT:NodeStatus indicating whether the parameter to be changed
+      * is not present in the remote node (BT:NodeStatus::SUCCESS) or if it
+      * failed due to another reason (BT:NodeStatus::FAILURE).
+      *
+      */
+    BT::NodeStatus is_point_to_param_not_present(void);
+
   };
 
   #endif

src/iri_joints_module.cpp

@@ -93,29 +93,13 @@ void CIriJointsModule::state_machine(void)
       }
       if (this->new_point_goal)
       {
+        this->state = IRI_JOINTS_MODULE_IDLE;
+        this->status = IRI_JOINTS_MODULE_PREEMPTED;
         this->point_to_action.stop_timeout();
-        switch (this->point_to_action.make_request(this->point_to_goal))
-        {
-          case ACT_SRV_SUCCESS: 
-            this->state = IRI_JOINTS_MODULE_POINT_WAIT;
-            this->new_point_goal = false;
-            ROS_DEBUG("CIriJointsModule : PointHeadAction goal start successfull");
-          break;
-          case ACT_SRV_PENDING: 
-            this->state = IRI_JOINTS_MODULE_POINT_WAIT;
-            ROS_WARN("CIriJointsModule : PointHeadAction goal start pending");
-          break;
-          case ACT_SRV_FAIL: 
-            this->state = IRI_JOINTS_MODULE_IDLE;
-            this->status = IRI_JOINTS_MODULE_ACTION_START_FAIL;
-            this->new_point_goal = false;
-            ROS_ERROR("CIriJointsModule: PointHeadAction goal start failed");
-          break;
-        }
       }
-      if (this->cancel_poin_to)
+      if (this->cancel_point_to_pending)
       {
-        this->cancel_poin_to = false;
+        this->cancel_point_to_pending = false;
         this->point_to_action.cancel();
       }
     break;
@@ -198,10 +182,10 @@ void CIriJointsModule::point_to(double x, double y, double z, double max_vel)
 void CIriJointsModule::cancel_point_to(void)
 {
   if(this->state != IRI_JOINTS_MODULE_IDLE)
-    this->cancel_poin_to = true;
+    this->cancel_point_to_pending = true;
 }
 
-bool CIriJointsModule::point_to_finished(void)
+bool CIriJointsModule::is_point_to_finished(void)
 {
   if(this->state == IRI_JOINTS_MODULE_IDLE && this->new_point_goal == false)
     return true;

src/iri_joints_module_bt.cpp

 #include <iri_joints_module/iri_joints_module_bt.h>
 #include <iri_joints_module/iri_joints_module.h>
-#include "behaviortree_cpp_v3/blackboard.h"
+#include <stdlib.h> 
 
 CIriJointsModuleBT::CIriJointsModuleBT() :
   joints_module("joints_module",ros::this_node::getName())
@@ -10,10 +10,259 @@ CIriJointsModuleBT::CIriJointsModuleBT() :
 
 void CIriJointsModuleBT::init(IriBehaviorTreeFactory &factory)
 {
+	//Definition of ports
+  BT::PortsList point_to_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("z"), BT::InputPort<double>("max_vel")};
+  BT::PortsList async_point_to_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("z"), BT::InputPort<double>("max_vel"), BT::BidirectionalPort<bool>("new_goal")};
+  BT::PortsList set_tolerance_ports = {BT::InputPort<double>("tol")};
+  BT::PortsList set_base_frame_ports = {BT::InputPort<std::string>("base_frame")};
+  BT::PortsList set_urdf_param_ports = {BT::InputPort<std::string>("urdf_param")};
 
+  //Registration of conditions
+  factory.registerSimpleCondition("is_point_to_finished",  std::bind(&CIriJointsModuleBT::is_point_to_finished, this));
+  factory.registerSimpleCondition("is_point_to_running",  std::bind(&CIriJointsModuleBT::is_point_to_running, this));
+  factory.registerSimpleCondition("is_point_to_success",  std::bind(&CIriJointsModuleBT::is_point_to_success, this));
+  factory.registerSimpleCondition("is_point_to_action_start_fail",  std::bind(&CIriJointsModuleBT::is_point_to_action_start_fail, this));
+  factory.registerSimpleCondition("is_point_to_timeout",  std::bind(&CIriJointsModuleBT::is_point_to_timeout, this));
+  factory.registerSimpleCondition("is_point_to_fb_watchdog",  std::bind(&CIriJointsModuleBT::is_point_to_fb_watchdog, this));
+  factory.registerSimpleCondition("is_point_to_aborted",  std::bind(&CIriJointsModuleBT::is_point_to_aborted, this));
+  factory.registerSimpleCondition("is_point_to_preempted",  std::bind(&CIriJointsModuleBT::is_point_to_preempted, this));
+  factory.registerSimpleCondition("is_point_to_rejected",  std::bind(&CIriJointsModuleBT::is_point_to_rejected, this));
+  factory.registerSimpleCondition("is_point_to_set_param_fail",  std::bind(&CIriJointsModuleBT::is_point_to_set_param_fail, this));
+  factory.registerSimpleCondition("is_point_to_param_not_present",  std::bind(&CIriJointsModuleBT::is_point_to_param_not_present, this));
+
+  //Registration of actions
+  factory.registerSimpleAction("sync_point_to",  std::bind(&CIriJointsModuleBT::sync_point_to, this, std::placeholders::_1), point_to_ports);
+  factory.registerIriAsyncAction("async_point_to",  std::bind(&CIriJointsModuleBT::async_point_to, this, std::placeholders::_1), async_point_to_ports);
+  factory.registerSimpleAction("cancel_point_to",  std::bind(&CIriJointsModuleBT::cancel_point_to, this));
+  factory.registerSimpleAction("set_tolerance",  std::bind(&CIriJointsModuleBT::set_tolerance, this, std::placeholders::_1), set_tolerance_ports);
+  factory.registerSimpleAction("set_base_frame",  std::bind(&CIriJointsModuleBT::set_base_frame, this, std::placeholders::_1), set_base_frame_ports);
+  factory.registerSimpleAction("set_urdf_param",  std::bind(&CIriJointsModuleBT::set_urdf_param, this, std::placeholders::_1), set_urdf_param_ports);
 }
 
 CIriJointsModuleBT::~CIriJointsModuleBT(void)
 {
   // [free dynamic memory]
+}
+
+////////////////////////////////////////////////////////////// Actions //////////////////////////////////////////////////////////////
+BT::NodeStatus CIriJointsModuleBT::sync_point_to(BT::TreeNode& self)
+{
+  ROS_INFO("sync_point_to");
+  BT::Optional<double> x = self.getInput<double>("x");
+  BT::Optional<double> y = self.getInput<double>("y");
+  BT::Optional<double> z = self.getInput<double>("z");
+  BT::Optional<double> max_vel = self.getInput<double>("max_vel");
+
+  if (!x || !y || !z || !max_vel)
+  {
+    ROS_ERROR("Incorrect or missing input. It needs the following input ports: x(double), y(double), z(double) and max_vel(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  double x_goal, y_goal, z_goal, max_vel_goal;
+  x_goal = x.value();
+  y_goal = y.value();
+  z_goal = z.value();
+  max_vel_goal = max_vel.value();
+
+  this->joints_module.point_to(x_goal, y_goal, z_goal, max_vel_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriJointsModuleBT::async_point_to(BT::TreeNode& self)
+{
+  ROS_INFO("async_point_to");
+  BT::Optional<double> x = self.getInput<double>("x");
+  BT::Optional<double> y = self.getInput<double>("y");
+  BT::Optional<double> z = self.getInput<double>("z");
+  BT::Optional<double> max_vel = self.getInput<double>("max_vel");
+  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 (!x || !y || !z || !max_vel)
+    {
+      ROS_ERROR("Incorrect or missing input. It needs the following input ports: x(double), y(double), z(double) and max_vel(double)");
+      return BT::NodeStatus::FAILURE;
+    }
+
+    double x_goal, y_goal, z_goal, max_vel_goal;
+    x_goal = x.value();
+    y_goal = y.value();
+    z_goal = z.value();
+    max_vel_goal = max_vel.value();
+
+    self.setOutput("new_goal", false);
+    this->joints_module.point_to(x_goal, y_goal, z_goal, max_vel_goal);
+  }
+  if (this->joints_module.is_point_to_finished())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CIriJointsModuleBT::cancel_point_to(void)
+{
+  ROS_INFO("cancel_point_to");
+  this->joints_module.cancel_point_to();
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriJointsModuleBT::set_tolerance(BT::TreeNode& self)
+{
+  ROS_INFO("set_tolerance");
+  BT::Optional<double> tolerance = self.getInput<double>("tol");
+
+  if (!tolerance)
+  {
+    ROS_ERROR("Incorrect or missing input. It needs the following input ports: tol(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  double tol;
+  tol = tolerance.value();
+  if (this->joints_module.set_tolerance(tol))
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::set_base_frame(BT::TreeNode& self)
+{
+  ROS_INFO("set_base_frame");
+  BT::Optional<std::string> base_frame_in = self.getInput<std::string>("base_frame");
+
+  if (!base_frame_in)
+  {
+    ROS_ERROR("Incorrect or missing input. It needs the following input ports: base_frame(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  std::string base_frame;
+  base_frame = base_frame_in.value();
+  if (this->joints_module.set_base_frame(base_frame))
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::set_urdf_param(BT::TreeNode& self)
+{
+  ROS_INFO("set_urdf_param");
+  BT::Optional<std::string> urdf_param_in = self.getInput<std::string>("urdf_param");
+
+  if (!urdf_param_in)
+  {
+    ROS_ERROR("Incorrect or missing input. It needs the following input ports: urdf_param(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  std::string urdf_param;
+  urdf_param = urdf_param_in.value();
+  if (this->joints_module.set_urdf_param(urdf_param))
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+////////////////////////////////////////////////////////////// Conditions //////////////////////////////////////////////////////////////
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_finished(void)
+{
+  if (this->joints_module.is_point_to_finished())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_running(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_RUNNING)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_success(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_SUCCESS)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_action_start_fail(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_ACTION_START_FAIL)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_timeout(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_TIMEOUT)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_fb_watchdog(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_FB_WATCHDOG)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_aborted(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_ABORTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_preempted(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_PREEMPTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_rejected(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_REJECTED)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_set_param_fail(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_SET_PARAM_FAIL)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriJointsModuleBT::is_point_to_param_not_present(void)
+{
+  iri_joints_module_status_t joints_module_status = this->joints_module.get_status();
+
+  if (joints_module_status == IRI_JOINTS_MODULE_PARAM_NOT_PRESENT)
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
 }
\ No newline at end of file