diff --git a/include/iri_joints_module/iri_joints_module.h b/include/iri_joints_module/iri_joints_module.h
index 720299599fd56bc85f71363376a16937f40f5917..bd36c48cc6f854c45590954d2086bccd0c406b25 100644
--- a/include/iri_joints_module/iri_joints_module.h
+++ b/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.
diff --git a/include/iri_joints_module/iri_joints_module_bt.h b/include/iri_joints_module/iri_joints_module_bt.h
index 74cb8301fc3f3e335af3ed936687bb77a170179b..6d9f86bd61fa41ac61203dd225981cb6f3a718de 100644
--- a/include/iri_joints_module/iri_joints_module_bt.h
+++ b/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
diff --git a/src/iri_joints_module.cpp b/src/iri_joints_module.cpp
index 433ea0b16c59596dfa47316eef4201fbdb2a6a9c..8bfb914f4b0519f82ff88b10f9c2790a061cf1de 100644
--- a/src/iri_joints_module.cpp
+++ b/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;
diff --git a/src/iri_joints_module_bt.cpp b/src/iri_joints_module_bt.cpp
index 8a878f966b8700a4a629aafaca4298c527c39f2b..62952b7aa6cd8f70a34cc7feacdbafaff1dd0409 100644
--- a/src/iri_joints_module_bt.cpp
+++ b/src/iri_joints_module_bt.cpp
@@ -1,6 +1,6 @@
#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