diff --git a/include/tiago_arm_module/tiago_arm_bt_module.h b/include/tiago_arm_module/tiago_arm_bt_module.h
index 49cc0f75322233159189992e6e936d3dfe9be554..dfb473fe1c50077d927d3259d30dab88b5ec12d9 100644
--- a/include/tiago_arm_module/tiago_arm_bt_module.h
+++ b/include/tiago_arm_module/tiago_arm_bt_module.h
@@ -6,6 +6,7 @@
#include "iri_bt_factory.h"
#include <tiago_arm_module/tiago_arm_module.h>
#include <sensor_msgs/JointState.h>
+#include <geometry_msgs/PoseStamped.h>
class CTiagoArmModuleBT
{
@@ -43,11 +44,9 @@ class CTiagoArmModuleBT
protected:
/**
- * \brief Synchronized set_workspace TIAGo gripper function.
+ * \brief Synchronized set_workspace TIAGo arm function.
*
- * This function calls set_workspace of tiago_arm_module. As this is a
- * synchronous action is_tiago_arm_finished() must be used to know when the action
- * has actually finished.
+ * This function calls set_workspace of tiago_arm_module.
*
* It has the following input ports:
*
@@ -65,7 +64,7 @@ class CTiagoArmModuleBT
*
* z_max (double): Workspace's z max.
*
- * \param self Self node with the required inputs ports:
+ * \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).
@@ -87,7 +86,7 @@ class CTiagoArmModuleBT
*
* joint_tol (double): [Optional] Joints tolerance.
*
- * \param self node with the required inputs ports:
+ * \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).
@@ -125,17 +124,15 @@ class CTiagoArmModuleBT
BT::NodeStatus async_tiago_arm_move_to_joints(BT::TreeNode& self);
/**
- * \brief Synchronized set_workspace TIAGo gripper function.
+ * \brief Synchronized get_current_joint_angles TIAGo arm function.
*
- * This function calls set_workspace of tiago_arm_module. As this is a
- * synchronous action is_tiago_arm_finished() must be used to know when the action
- * has actually finished.
+ * This function calls get_current_joint_angles of tiago_arm_module.
*
* It has the following output ports:
*
* joint_states (sensor_msgs::JointState): The current JointState.
*
- * \param self Self node with the required inputs ports:
+ * \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).
@@ -144,6 +141,258 @@ class CTiagoArmModuleBT
*/
BT::NodeStatus get_tiago_arm_current_joint_angles(BT::TreeNode& self);
+ /**
+ * \brief Synchronized move_to joints TIAGo arm function.
+ *
+ * This function calls move_to of tiago_arm_module. As this is a
+ * synchronous action is_tiago_arm_finished() must be used to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * pose (geometry_msgs::PoseStamped): The target pose.
+ *
+ * position_tol (double): [Optional] Position tolerance.
+ *
+ * orientation_tol (double): [Optional] Orientation tolerance.
+ *
+ * \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_arm_move_to_pose(BT::TreeNode& self);
+
+ /**
+ * \brief Async move_to joints TIAGo arm function.
+ *
+ * This function calls move_to of tiago_arm_module. As this is
+ * an asynchronous action is_tiago_arm_finished() is checked to know when the action
+ * has actually finished.
+ *
+ * It has the following input ports:
+ *
+ * pose (geometry_msgs::PoseStamped): The target pose.
+ *
+ * position_tol (double): [Optional] Position tolerance.
+ *
+ * orientation_tol (double): [Optional] Orientation tolerance.
+ *
+ * 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_arm_move_to_pose(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized get_current_pose TIAGo arm function.
+ *
+ * This function calls get_current_pose of tiago_arm_module.
+ *
+ * It has the following output ports:
+ *
+ * pose (geometry_msgs::PoseStamped): The current pose.
+ *
+ * \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 get_tiago_arm_current_pose(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized set_planner TIAGo arm function.
+ *
+ * This function calls set_planner of tiago_arm_module.
+ *
+ * It has the following input ports:
+ *
+ * planner_id (std::string): The planner id.
+ *
+ * \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_arm_planner(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized get_planner TIAGo arm function.
+ *
+ * This function calls get_planner of tiago_arm_module.
+ *
+ * It has the following output ports:
+ *
+ * planner_id (std::string): The planner id.
+ *
+ * \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 get_tiago_arm_planner(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized set_group_name TIAGo arm function.
+ *
+ * This function calls set_group_name of tiago_arm_module.
+ *
+ * It has the following input ports:
+ *
+ * group_id (std::string): The group id.
+ *
+ * \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_arm_group_name(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized get_group_name TIAGo arm function.
+ *
+ * This function calls get_group_name of tiago_arm_module.
+ *
+ * It has the following output ports:
+ *
+ * group_id (std::string): The group id.
+ *
+ * \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 get_tiago_arm_group_name(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized set_max_planning_attempts TIAGo arm function.
+ *
+ * This function calls set_max_planning_attempts of tiago_arm_module.
+ *
+ * It has the following input ports:
+ *
+ * num (unsigned int): The num of maximum planning attempts.
+ *
+ * \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_arm_max_planning_attempts(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized get_max_planning_attempts TIAGo arm function.
+ *
+ * This function calls get_max_planning_attempts of tiago_arm_module.
+ *
+ * It has the following output ports:
+ *
+ * num (unsigned int): The num of maximum planning attempts.
+ *
+ * \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 get_tiago_arm_max_planning_attempts(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized set_max_planning_time TIAGo arm function.
+ *
+ * This function calls set_max_planning_time of tiago_arm_module.
+ *
+ * It has the following input ports:
+ *
+ * time (unsigned int): The maximum planning time.
+ *
+ * \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_arm_max_planning_time(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized get_max_planning_time TIAGo arm function.
+ *
+ * This function calls get_max_planning_time of tiago_arm_module.
+ *
+ * It has the following output ports:
+ *
+ * time (unsigned int): The maximum planning time.
+ *
+ * \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 get_tiago_arm_max_planning_time(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized set_velocity_scale_factor TIAGo arm function.
+ *
+ * This function calls set_velocity_scale_factor of tiago_arm_module.
+ *
+ * It has the following input ports:
+ *
+ * scale (unsigned int): The velocity scale factor.
+ *
+ * \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_arm_velocity_scale_factor(BT::TreeNode& self);
+
+ /**
+ * \brief Synchronized get_velocity_scale_factor TIAGo arm function.
+ *
+ * This function calls get_velocity_scale_factor of tiago_arm_module.
+ *
+ * It has the following output ports:
+ *
+ * scale (unsigned int): The velocity scale factor.
+ *
+ * \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 get_tiago_arm_velocity_scale_factor(BT::TreeNode& self);
+
/**
* \brief Synchronized stop TIAGo arm function.
*
diff --git a/src/tiago_arm_bt_module.cpp b/src/tiago_arm_bt_module.cpp
index 3d6bf0706df9d95dac39e0f93d091a623ea641ab..7ef5c4cea03d3b8054d7f317050878cb18756429 100644
--- a/src/tiago_arm_bt_module.cpp
+++ b/src/tiago_arm_bt_module.cpp
@@ -18,7 +18,29 @@ void CTiagoArmModuleBT::init(IriBehaviorTreeFactory &factory)
BT::PortsList async_arm_move_to_joints_ports = {BT::InputPort<sensor_msgs::JointState>("target_joints"), BT::InputPort<double>("joint_tol"), BT::BidirectionalPort<bool>("new_goal")};
BT::PortsList get_current_joint_state_ports = {BT::OutputPort<sensor_msgs::JointState>("joint_states")};
+
+ BT::PortsList sync_arm_move_to_pose_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::InputPort<double>("position_tol"), BT::InputPort<double>("orientation_tol")};
+ BT::PortsList async_arm_move_to_pose_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::InputPort<double>("position_tol"), BT::InputPort<double>("orientation_tol"), BT::BidirectionalPort<bool>("new_goal")};
+
+ BT::PortsList get_current_pose_ports = {BT::OutputPort<geometry_msgs::PoseStamped>("pose")};
+ BT::PortsList set_planner_ports = {BT::InputPort<std::string>("planner_id")};
+ BT::PortsList get_planner_ports = {BT::OutputPort<std::string>("planner_id")};
+
+ BT::PortsList set_group_name_ports = {BT::InputPort<std::string>("group_id")};
+ BT::PortsList get_group_name_ports = {BT::OutputPort<std::string>("group_id")};
+
+ BT::PortsList set_max_planning_attempts_ports = {BT::InputPort<unsigned int>("num")};
+ BT::PortsList get_max_planning_attempts_ports = {BT::OutputPort<unsigned int>("num")};
+
+ BT::PortsList set_max_planning_time_ports = {BT::InputPort<double>("time")};
+ BT::PortsList get_max_planning_time_ports = {BT::OutputPort<double>("time")};
+
+ BT::PortsList set_velocity_scale_factor_ports = {BT::InputPort<double>("scale")};
+ BT::PortsList get_velocity_scale_factor_ports = {BT::OutputPort<double>("scale")};
+
+
+
//Registration of conditions
factory.registerSimpleCondition("is_tiago_arm_finished", std::bind(&CTiagoArmModuleBT::is_tiago_arm_finished, this));
@@ -44,6 +66,28 @@ void CTiagoArmModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerIriAsyncAction("async_tiago_arm_move_to_joints", std::bind(&CTiagoArmModuleBT::async_tiago_arm_move_to_joints, this, std::placeholders::_1), async_arm_move_to_joints_ports);
factory.registerSimpleAction("get_tiago_arm_current_joint_angles", std::bind(&CTiagoArmModuleBT::get_tiago_arm_current_joint_angles, this, std::placeholders::_1), get_current_joint_state_ports);
+
+ factory.registerSimpleAction("sync_tiago_arm_move_to_pose", std::bind(&CTiagoArmModuleBT::sync_tiago_arm_move_to_pose, this, std::placeholders::_1), sync_arm_move_to_pose_ports);
+ factory.registerIriAsyncAction("async_tiago_arm_move_to_pose", std::bind(&CTiagoArmModuleBT::async_tiago_arm_move_to_pose, this, std::placeholders::_1), async_arm_move_to_pose_ports);
+
+ factory.registerSimpleAction("get_tiago_arm_current_pose", std::bind(&CTiagoArmModuleBT::get_tiago_arm_current_pose, this, std::placeholders::_1), get_current_pose_ports);
+
+ factory.registerSimpleAction("set_tiago_arm_planner", std::bind(&CTiagoArmModuleBT::set_tiago_arm_planner, this, std::placeholders::_1), set_planner_ports);
+ factory.registerSimpleAction("get_tiago_arm_planner", std::bind(&CTiagoArmModuleBT::get_tiago_arm_planner, this, std::placeholders::_1), get_planner_ports);
+
+ factory.registerSimpleAction("set_tiago_arm_group_name", std::bind(&CTiagoArmModuleBT::set_tiago_arm_group_name, this, std::placeholders::_1), set_group_name_ports);
+ factory.registerSimpleAction("get_tiago_arm_group_name", std::bind(&CTiagoArmModuleBT::get_tiago_arm_group_name, this, std::placeholders::_1), get_group_name_ports);
+
+ factory.registerSimpleAction("set_tiago_arm_max_planning_attempts", std::bind(&CTiagoArmModuleBT::set_tiago_arm_max_planning_attempts, this, std::placeholders::_1), set_max_planning_attempts_ports);
+ factory.registerSimpleAction("get_tiago_arm_max_planning_attempts", std::bind(&CTiagoArmModuleBT::get_tiago_arm_max_planning_attempts, this, std::placeholders::_1), get_max_planning_attempts_ports);
+
+ factory.registerSimpleAction("set_tiago_arm_max_planning_time", std::bind(&CTiagoArmModuleBT::set_tiago_arm_max_planning_time, this, std::placeholders::_1), set_max_planning_time_ports);
+ factory.registerSimpleAction("get_tiago_arm_max_planning_time", std::bind(&CTiagoArmModuleBT::get_tiago_arm_max_planning_time, this, std::placeholders::_1), get_max_planning_time_ports);
+
+ factory.registerSimpleAction("set_tiago_arm_velocity_scale_factor", std::bind(&CTiagoArmModuleBT::set_tiago_arm_velocity_scale_factor, this, std::placeholders::_1), set_velocity_scale_factor_ports);
+ factory.registerSimpleAction("get_tiago_arm_velocity_scale_factor", std::bind(&CTiagoArmModuleBT::get_tiago_arm_velocity_scale_factor, this, std::placeholders::_1), get_velocity_scale_factor_ports);
+
+
}
CTiagoArmModuleBT::~CTiagoArmModuleBT(void)
@@ -109,12 +153,16 @@ BT::NodeStatus CTiagoArmModuleBT::sync_tiago_arm_move_to_joints(BT::TreeNode& se
// target_joints_goal.position = joint_positions.value();
target_joints_goal = target_joints.value();
if (!joint_tol)
- joint_tol_goal = 0.1;
+ {
+ if (!this->tiago_arm_module.move_to(target_joints_goal))
+ return BT::NodeStatus::FAILURE;
+ }
else
+ {
joint_tol_goal = joint_tol.value();
-
- if (!this->tiago_arm_module.move_to(target_joints_goal, joint_tol_goal))
- return BT::NodeStatus::FAILURE;
+ if (!this->tiago_arm_module.move_to(target_joints_goal, joint_tol_goal))
+ return BT::NodeStatus::FAILURE;
+ }
return BT::NodeStatus::SUCCESS;
}
@@ -149,12 +197,16 @@ BT::NodeStatus CTiagoArmModuleBT::async_tiago_arm_move_to_joints(BT::TreeNode& s
// target_joints_goal.position = joint_positions.value();
target_joints_goal = target_joints.value();
if (!joint_tol)
- joint_tol_goal = 0.1;
+ {
+ if (!this->tiago_arm_module.move_to(target_joints_goal))
+ return BT::NodeStatus::FAILURE;
+ }
else
+ {
joint_tol_goal = joint_tol.value();
-
- if (!this->tiago_arm_module.move_to(target_joints_goal, joint_tol_goal))
- return BT::NodeStatus::FAILURE;
+ if (!this->tiago_arm_module.move_to(target_joints_goal, joint_tol_goal))
+ return BT::NodeStatus::FAILURE;
+ }
}
if (this->tiago_arm_module.is_finished())
{
@@ -164,7 +216,8 @@ BT::NodeStatus CTiagoArmModuleBT::async_tiago_arm_move_to_joints(BT::TreeNode& s
else
return BT::NodeStatus::FAILURE;
}
- self.setOutput("new_goal", false);
+ if (new_goal)
+ self.setOutput("new_goal", false);
return BT::NodeStatus::RUNNING;
}
@@ -179,6 +232,254 @@ BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_current_joint_angles(BT::TreeNod
return BT::NodeStatus::SUCCESS;
}
+BT::NodeStatus CTiagoArmModuleBT::sync_tiago_arm_move_to_pose(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::sync_tiago_arm_move_to_pose-> sync_tiago_arm_move_to_pose");
+ BT::Optional<geometry_msgs::PoseStamped> pose = self.getInput<geometry_msgs::PoseStamped>("pose");
+ BT::Optional<double> position_tol = self.getInput<double>("position_tol");
+ BT::Optional<double> orientation_tol = self.getInput<double>("orientation_tol");
+
+ geometry_msgs::PoseStamped pose_goal;
+ double position_tol_goal, orientation_tol_goal;
+ if (!pose || (!position_tol && orientation_tol))
+ {
+ ROS_ERROR("CTiagoArmModuleBT::sync_tiago_arm_move_to_pose-> Incorrect or missing input. It needs the following input ports: pose(geometry_msgs::PoseStamped), [Optional] position_tol(double) and [Optional] orientation_tol(double)");
+ ROS_ERROR("CTiagoArmModuleBT::sync_tiago_arm_move_to_pose-> The correct input options are: pose, pose + position_tol or pose + position_tol + orientation_tol");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ pose_goal = pose.value();
+ if (!orientation_tol)
+ {
+ if (!position_tol)
+ this->tiago_arm_module.move_to(pose_goal);
+ else
+ {
+ position_tol_goal = position_tol.value();
+ this->tiago_arm_module.move_to(pose_goal, position_tol_goal);
+ }
+ }
+ else
+ {
+ orientation_tol_goal = orientation_tol.value();
+ position_tol_goal = position_tol.value();
+ this->tiago_arm_module.move_to(pose_goal, position_tol_goal, orientation_tol_goal);
+ }
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::async_tiago_arm_move_to_pose(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::async_tiago_arm_move_to_pose-> async_tiago_arm_move_to_pose");
+ BT::Optional<geometry_msgs::PoseStamped> pose = self.getInput<geometry_msgs::PoseStamped>("pose");
+ BT::Optional<double> position_tol = self.getInput<double>("position_tol");
+ BT::Optional<double> orientation_tol = self.getInput<double>("orientation_tol");
+ BT::Optional<bool> new_goal_in = self.getInput<bool>("new_goal");
+
+ bool new_goal;
+ if (!new_goal_in)
+ new_goal = false;
+ else
+ new_goal = new_goal_in.value();
+
+ if (new_goal)
+ {
+ geometry_msgs::PoseStamped pose_goal;
+ double position_tol_goal, orientation_tol_goal;
+ if (!pose || (!position_tol && orientation_tol))
+ {
+ ROS_ERROR("CTiagoArmModuleBT::sync_tiago_arm_move_to_pose-> Incorrect or missing input. It needs the following input ports: pose(geometry_msgs::PoseStamped), [Optional] position_tol(double) and [Optional] orientation_tol(double)");
+ ROS_ERROR("CTiagoArmModuleBT::sync_tiago_arm_move_to_pose-> The correct input options are: pose, pose + position_tol or pose + position_tol + orientation_tol");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ pose_goal = pose.value();
+ if (!orientation_tol)
+ {
+ if (!position_tol)
+ this->tiago_arm_module.move_to(pose_goal);
+ else
+ {
+ position_tol_goal = position_tol.value();
+ this->tiago_arm_module.move_to(pose_goal, position_tol_goal);
+ }
+ }
+ else
+ {
+ orientation_tol_goal = orientation_tol.value();
+ position_tol_goal = position_tol.value();
+ this->tiago_arm_module.move_to(pose_goal, position_tol_goal, orientation_tol_goal);
+ }
+ }
+ if (this->tiago_arm_module.is_finished())
+ {
+ tiago_arm_module_status_t tiago_arm_module_status = this->tiago_arm_module.get_status();
+ if (tiago_arm_module_status == TIAGO_ARM_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 CTiagoArmModuleBT::get_tiago_arm_current_pose(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::get_tiago_arm_current_pose-> get_tiago_arm_current_pose");
+
+ geometry_msgs::PoseStamped pose;
+ this->tiago_arm_module.get_current_pose(pose);
+
+ self.setOutput("pose", pose);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::set_tiago_arm_planner(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::set_tiago_arm_planner-> set_tiago_arm_planner");
+ BT::Optional<std::string> planner_id = self.getInput<std::string>("planner_id");
+
+ std::string planner_id_goal;
+ if (!planner_id)
+ {
+ ROS_ERROR("CTiagoArmModuleBT::set_tiago_arm_planner-> Incorrect or missing input. It needs the following input ports: planner_id(std::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ planner_id_goal = planner_id.value();
+
+ this->tiago_arm_module.set_planner(planner_id_goal);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_planner(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::get_tiago_arm_planner-> get_tiago_arm_planner");
+
+ std::string planner_id;
+ planner_id = this->tiago_arm_module.get_planner();
+
+ self.setOutput("planner_id", planner_id);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::set_tiago_arm_group_name(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::set_tiago_arm_group_name-> set_tiago_arm_group_name");
+ BT::Optional<std::string> group_id = self.getInput<std::string>("group_id");
+
+ std::string group_id_goal;
+ if (!group_id)
+ {
+ ROS_ERROR("CTiagoArmModuleBT::set_tiago_arm_group_name-> Incorrect or missing input. It needs the following input ports: group_id(std::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ group_id_goal = group_id.value();
+
+ this->tiago_arm_module.set_group_name(group_id_goal);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_group_name(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::get_tiago_arm_group_name-> get_tiago_arm_group_name");
+
+ std::string group_id;
+ group_id = this->tiago_arm_module.get_group_name();
+
+ self.setOutput("group_id", group_id);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::set_tiago_arm_max_planning_attempts(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::set_tiago_arm_max_planning_attempts-> set_tiago_arm_max_planning_attempts");
+ BT::Optional<unsigned int> num = self.getInput<unsigned int>("num");
+
+ unsigned int num_goal;
+ if (!num)
+ {
+ ROS_ERROR("CTiagoArmModuleBT::set_tiago_arm_max_planning_attempts-> Incorrect or missing input. It needs the following input ports: num(unsigned int)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ num_goal = num.value();
+
+ this->tiago_arm_module.set_max_planning_attempts(num_goal);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_max_planning_attempts(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::get_tiago_arm_max_planning_attempts-> get_tiago_arm_max_planning_attempts");
+
+ unsigned int num;
+ num = this->tiago_arm_module.get_max_planning_attempts();
+
+ self.setOutput("num", num);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::set_tiago_arm_max_planning_time(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::set_tiago_arm_max_planning_time-> set_tiago_arm_max_planning_time");
+ BT::Optional<double> time = self.getInput<double>("time");
+
+ double time_goal;
+ if (!time)
+ {
+ ROS_ERROR("CTiagoArmModuleBT::set_tiago_arm_max_planning_time-> Incorrect or missing input. It needs the following input ports: time(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ time_goal = time.value();
+
+ this->tiago_arm_module.set_max_planning_time(time_goal);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_max_planning_time(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::get_tiago_arm_max_planning_time-> get_tiago_arm_max_planning_time");
+
+ double time;
+ time = this->tiago_arm_module.get_max_planning_time();
+
+ self.setOutput("time", time);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::set_tiago_arm_velocity_scale_factor(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::set_tiago_arm_velocity_scale_factor-> set_tiago_arm_velocity_scale_factor");
+ BT::Optional<double> scale = self.getInput<double>("scale");
+
+ double scale_goal;
+ if (!scale)
+ {
+ ROS_ERROR("CTiagoArmModuleBT::set_tiago_arm_velocity_scale_factor-> Incorrect or missing input. It needs the following input ports: scale(double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ scale_goal = scale.value();
+
+ this->tiago_arm_module.set_velocity_scale_factor(scale_goal);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_velocity_scale_factor(BT::TreeNode& self)
+{
+ ROS_DEBUG("CTiagoArmModuleBT::get_tiago_arm_velocity_scale_factor-> get_tiago_arm_velocity_scale_factor");
+
+ double scale;
+ scale = this->tiago_arm_module.get_velocity_scale_factor();
+
+ self.setOutput("scale", scale);
+ return BT::NodeStatus::SUCCESS;
+}
+
BT::NodeStatus CTiagoArmModuleBT::syn_cancel_tiago_arm_action(void)
{
ROS_DEBUG("CTiagoArmModuleBT::syn_cancel_tiago_arm_action-> syn_cancel_tiago_arm_action");