diff --git a/include/tiago_arm_module/tiago_arm_bt_module.h b/include/tiago_arm_module/tiago_arm_bt_module.h index f2a62b66c1fd8f1d68d1f40655acb46846849ad8..4b05272e1d5278781847cdd9fa3da80c9d70e1dc 100644 --- a/include/tiago_arm_module/tiago_arm_bt_module.h +++ b/include/tiago_arm_module/tiago_arm_bt_module.h @@ -418,6 +418,280 @@ class CTiagoArmModuleBT */ BT::NodeStatus get_tiago_arm_velocity_scale_factor(BT::TreeNode& self); + /** + * \brief Synchronized add_object TIAGo arm function. + * + * This function calls add_object of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * pose (geometry_msgs::PoseStamped): The object pose. + * + * width (double): The object width. + * + * length (double): The object length. + * + * height (double): The object height. + * + * \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 tiago_arm_add_object(BT::TreeNode& self); + + /** + * \brief Synchronized update_object_pose TIAGo arm function. + * + * This function calls update_object_pose of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * pose (geometry_msgs::PoseStamped): The new object pose. + * + * \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 tiago_arm_update_object_pose(BT::TreeNode& self); + + /** + * \brief Synchronized update_object_size TIAGo arm function. + * + * This function calls update_object_size of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * width (double): The object width. + * + * length (double): The object length. + * + * height (double): The object height. + * + * \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 tiago_arm_update_object_size(BT::TreeNode& self); + + /** + * \brief Synchronized remove_object TIAGo arm function. + * + * This function calls remove_object of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * \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 tiago_arm_remove_object(BT::TreeNode& self); + + /** + * \brief Synchronized add_object_to_environment TIAGo arm function. + * + * This function calls add_object_to_environment of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * \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 tiago_arm_add_object_to_environment(BT::TreeNode& self); + + /** + * \brief Synchronized remove_object_from_environment TIAGo arm function. + * + * This function calls remove_object_from_environment of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * \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 tiago_arm_remove_object_from_environment(BT::TreeNode& self); + + /** + * \brief Synchronized attach_object_to_robot TIAGo arm function. + * + * This function calls attach_object_to_robot of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * link (std::string): [Optional] The gripper link. + * + * \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 tiago_arm_attach_object_to_robot(BT::TreeNode& self); + + /** + * \brief Synchronized dettach_object_from_robot TIAGo arm function. + * + * This function calls dettach_object_from_robot of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * \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 tiago_arm_dettach_object_from_robot(BT::TreeNode& self); + + /** + * \brief Synchronized disable_collision TIAGo arm function. + * + * This function calls disable_collision of tiago_arm_module. + * + * It has the following input ports: + * + * name (std::string): The object name. + * + * \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 tiago_arm_disable_collision(BT::TreeNode& self); + + /** + * \brief Synchronized get_current_planning_scene TIAGo arm function. + * + * This function calls get_current_planning_scene of tiago_arm_module. + * + * It has the following input ports: + * + * component (int): Planing scene component. + * + * \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 tiago_arm_get_current_planning_scene(BT::TreeNode& self); + + /** + * \brief Synchronized clear_path_constraints TIAGo arm function. + * + * This function calls clear_path_constraints 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. + * + * \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 tiago_arm_clear_path_constraints(void); + + /** + * \brief Synchronized add_orientation_path_constraint TIAGo arm function. + * + * This function calls add_orientation_path_constraint of tiago_arm_module. + * + * It has the following input ports: + * + * quat (geometry_msgs::QuaternionStamped): The orientation constrain. + * + * orientation_tol (double): [Optional] The orientation constrain tolerance. + * + * \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 tiago_arm_add_orientation_path_constraint(BT::TreeNode& self); + + /** + * \brief Synchronized add_plane_path_constraint TIAGo arm function. + * + * This function calls add_plane_path_constraint of tiago_arm_module. + * + * It has the following input ports: + * + * plane (path_const_plane_t): The plane definition. + * + * frame_id (std::string): The plane frame_id. + * + * offset (double): [Optional] The plane offset. + * + * position_tol (double): [Optional] The position tolerance. + * + * \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 tiago_arm_add_plane_path_constraint(BT::TreeNode& self); + + /** + * \brief Synchronized add_plane_path_constraint TIAGo arm function. + * + * This function calls add_plane_path_constraint of tiago_arm_module. + * + * It has the following input ports: + * + * normal (std::vector<double>): The plane definition. + * + * frame_id (std::string): The plane frame_id. + * + * offset (double): [Optional] The plane offset. + * + * position_tol (double): [Optional] The position tolerance. + * + * \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 tiago_arm_add_plane_path_constraint_normal(BT::TreeNode& self); + + /** + * \brief Synchronized add_joint_goal_constraint TIAGo arm function. + * + * This function calls add_joint_goal_constraint of tiago_arm_module. + * + * It has the following input ports: + * + * joint_names (std::vector<std::string>): The joint names. + * + * position (std::vector<double>): The joint positions. + * + * tol (std::vector<double>): The joints tolerance. + * + * \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 tiago_arm_add_joint_goal_constraint(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 8270a5b01046324aece51c7f74aa7aaec3f2a8df..ac6b40d5d5c41d5e6ac30d9aaad493ad920fb731 100644 --- a/src/tiago_arm_bt_module.cpp +++ b/src/tiago_arm_bt_module.cpp @@ -45,6 +45,16 @@ void CTiagoArmModuleBT::init(IriBehaviorTreeFactory &factory) BT::PortsList set_velocity_scale_factor_ports = {BT::InputPort<double>("scale")}; BT::PortsList get_velocity_scale_factor_ports = {BT::OutputPort<double>("scale")}; + BT::PortsList add_object_ports = {BT::InputPort<std::string>("name"), BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::InputPort<double>("width"), BT::InputPort<double>("length"), BT::InputPort<double>("height")}; + BT::PortsList update_object_pose_ports = {BT::InputPort<std::string>("name"), BT::InputPort<geometry_msgs::PoseStamped>("pose")}; + BT::PortsList update_object_size_ports = {BT::InputPort<std::string>("name"), BT::InputPort<double>("width"), BT::InputPort<double>("length"), BT::InputPort<double>("height")}; + BT::PortsList name_port = {BT::InputPort<std::string>("name")}; + BT::PortsList attach_object_to_robot_ports = {BT::InputPort<std::string>("name"), BT::InputPort<std::string>("link")}; + BT::PortsList get_current_scene_ports = {BT::InputPort<int>("component")}; + BT::PortsList add_orientation_path_constraints_ports = {BT::InputPort<geometry_msgs::QuaternionStamped>("quat"), BT::InputPort<double>("orientation_tol")}; + BT::PortsList add_plane_path_constraints_ports = {BT::InputPort<path_const_plane_t>("plane"), BT::InputPort<std::string>("frame_id"), BT::InputPort<double>("offset"), BT::InputPort<double>("position_tol")}; + BT::PortsList add_plane_path_constraints_normal_ports = {BT::InputPort<std::vector<double> >("normal"), BT::InputPort<std::string>("frame_id"), BT::InputPort<double>("offset"), BT::InputPort<double>("position_tol")}; + BT::PortsList tiago_arm_add_joint_goal_constraint_ports = {BT::InputPort<std::vector<std::string> >("joint_names"), BT::InputPort<std::vector<double> >("position"), BT::InputPort<std::vector<double> >("tol")}; //Registration of conditions @@ -95,6 +105,20 @@ void CTiagoArmModuleBT::init(IriBehaviorTreeFactory &factory) 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); + factory.registerSimpleAction("tiago_arm_add_object", std::bind(&CTiagoArmModuleBT::tiago_arm_add_object, this, std::placeholders::_1), add_object_ports); + factory.registerSimpleAction("tiago_arm_update_object_pose", std::bind(&CTiagoArmModuleBT::tiago_arm_update_object_pose, this, std::placeholders::_1), update_object_pose_ports); + factory.registerSimpleAction("tiago_arm_update_object_size", std::bind(&CTiagoArmModuleBT::tiago_arm_update_object_size, this, std::placeholders::_1), update_object_size_ports); + factory.registerSimpleAction("tiago_arm_remove_object", std::bind(&CTiagoArmModuleBT::tiago_arm_remove_object, this, std::placeholders::_1), name_port); + factory.registerSimpleAction("tiago_arm_add_object_to_environment", std::bind(&CTiagoArmModuleBT::tiago_arm_add_object_to_environment, this, std::placeholders::_1), name_port); + factory.registerSimpleAction("tiago_arm_remove_object_from_environment", std::bind(&CTiagoArmModuleBT::tiago_arm_remove_object_from_environment, this, std::placeholders::_1), name_port); + factory.registerSimpleAction("tiago_arm_attach_object_to_robot", std::bind(&CTiagoArmModuleBT::tiago_arm_attach_object_to_robot, this, std::placeholders::_1), attach_object_to_robot_ports); + factory.registerSimpleAction("tiago_arm_dettach_object_from_robot", std::bind(&CTiagoArmModuleBT::tiago_arm_dettach_object_from_robot, this, std::placeholders::_1), name_port); + factory.registerSimpleAction("tiago_arm_disable_collision", std::bind(&CTiagoArmModuleBT::tiago_arm_disable_collision, this, std::placeholders::_1), name_port); + factory.registerSimpleAction("tiago_arm_get_current_planning_scene", std::bind(&CTiagoArmModuleBT::tiago_arm_get_current_planning_scene, this, std::placeholders::_1), get_current_scene_ports); + factory.registerSimpleAction("tiago_arm_add_orientation_path_constraint", std::bind(&CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint, this, std::placeholders::_1), add_orientation_path_constraints_ports); + factory.registerSimpleAction("tiago_arm_add_plane_path_constraint", std::bind(&CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint, this, std::placeholders::_1), add_plane_path_constraints_ports); + factory.registerSimpleAction("tiago_arm_add_plane_path_constraint_normal", std::bind(&CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal, this, std::placeholders::_1), add_plane_path_constraints_normal_ports); + factory.registerSimpleAction("tiago_arm_add_joint_goal_constraint", std::bind(&CTiagoArmModuleBT::tiago_arm_add_joint_goal_constraint, this, std::placeholders::_1), tiago_arm_add_joint_goal_constraint_ports); } @@ -487,6 +511,354 @@ BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_velocity_scale_factor(BT::TreeNo return BT::NodeStatus::SUCCESS; } +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_object(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_object-> tiago_arm_add_object"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + BT::Optional<geometry_msgs::PoseStamped> pose = self.getInput<geometry_msgs::PoseStamped>("pose"); + BT::Optional<double> width = self.getInput<double>("width"); + BT::Optional<double> length = self.getInput<double>("length"); + BT::Optional<double> height = self.getInput<double>("height"); + + double width_goal, length_goal, height_goal; + std::string name_goal; + geometry_msgs::PoseStamped pose_goal; + if (!name || !pose || !width || !length || !height) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_object-> Incorrect or missing input. It needs the following input ports: name(std::string), pose(geometry_msgs::PoseStamped), width(double), length(double), height(double)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + pose_goal = pose.value(); + width_goal = width.value(); + length_goal = length.value(); + height_goal = height.value(); + + this->tiago_arm_module.add_object(name_goal, pose_goal, width_goal, length_goal, height_goal); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_update_object_pose(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_update_object_pose-> tiago_arm_update_object_pose"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + BT::Optional<geometry_msgs::PoseStamped> pose = self.getInput<geometry_msgs::PoseStamped>("pose"); + + std::string name_goal; + geometry_msgs::PoseStamped pose_goal; + if (!name || !pose) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_update_object_pose-> Incorrect or missing input. It needs the following input ports: name(std::string), pose(geometry_msgs::PoseStamped)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + pose_goal = pose.value(); + + this->tiago_arm_module.update_object_pose(name_goal, pose_goal); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_update_object_size(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_update_object_size-> tiago_arm_update_object_size"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + BT::Optional<double> width = self.getInput<double>("width"); + BT::Optional<double> length = self.getInput<double>("length"); + BT::Optional<double> height = self.getInput<double>("height"); + + double width_goal, length_goal, height_goal; + std::string name_goal; + if (!name || !width || !length || !height) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_update_object_size-> Incorrect or missing input. It needs the following input ports: name(std::string), width(double), length(double), height(double)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + width_goal = width.value(); + length_goal = length.value(); + height_goal = height.value(); + + this->tiago_arm_module.update_object_size(name_goal, width_goal, length_goal, height_goal); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_remove_object(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_remove_object-> tiago_arm_remove_object"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + + std::string name_goal; + if (!name) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_remove_object-> Incorrect or missing input. It needs the following input ports: name(std::string)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + + if (this->tiago_arm_module.remove_object(name_goal)) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_object_to_environment(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_object_to_enviroment-> tiago_arm_add_object_to_enviroment"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + + std::string name_goal; + if (!name) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_object_to_enviroment-> Incorrect or missing input. It needs the following input ports: name(std::string)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + + this->tiago_arm_module.add_object_to_environment(name_goal); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_remove_object_from_environment(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_remove_object_to_enviroment-> tiago_arm_remove_object_to_enviroment"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + + std::string name_goal; + if (!name) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_remove_object_to_enviroment-> Incorrect or missing input. It needs the following input ports: name(std::string)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + + if (this->tiago_arm_module.remove_object_from_environment(name_goal)) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_attach_object_to_robot(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_attach_object_to_robot-> tiago_arm_attach_object_to_robot"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + BT::Optional<std::string> link = self.getInput<std::string>("link"); + + std::string name_goal; + if (!name) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_attach_object_to_robot-> Incorrect or missing input. It needs the following input ports: name(std::string) and [Optional] link(std::string)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + + if (!link) + this->tiago_arm_module.attach_object_to_robot(name_goal); + else + { + std::string link_goal = link.value(); + this->tiago_arm_module.attach_object_to_robot(name_goal, link_goal); + } + return BT::NodeStatus::SUCCESS; +} + + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_dettach_object_from_robot(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_dettach_object_from_robot-> tiago_arm_dettach_object_from_robot"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + + std::string name_goal; + if (!name) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_dettach_object_from_robot-> Incorrect or missing input. It needs the following input ports: name(std::string)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + + if (this->tiago_arm_module.dettach_object_from_robot(name_goal)) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_disable_collision(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_disable_collision-> tiago_arm_disable_collision"); + BT::Optional<std::string> name = self.getInput<std::string>("name"); + + std::string name_goal; + if (!name) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_disable_collision-> Incorrect or missing input. It needs the following input ports: name(std::string)"); + return BT::NodeStatus::FAILURE; + } + + name_goal = name.value(); + + if (this->tiago_arm_module.disable_collision(name_goal)) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_get_current_planning_scene(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_get_current_planning_scene-> tiago_arm_get_current_planning_scene"); + BT::Optional<int> component = self.getInput<int>("component"); + + int component_goal; + if (!component) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_get_current_planning_scene-> Incorrect or missing input. It needs the following input ports: component(int)"); + return BT::NodeStatus::FAILURE; + } + + component_goal = component.value(); + + if (this->tiago_arm_module.get_current_planning_scene(component_goal)) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_clear_path_constraints(void) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_clear_path_constraints-> tiago_arm_clear_path_constraints"); + this->tiago_arm_module.clear_path_constraints(); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> tiago_arm_add_orientation_path_constraint"); + BT::Optional<geometry_msgs::QuaternionStamped> quat = self.getInput<geometry_msgs::QuaternionStamped>("quat"); + BT::Optional<double> orientation_tol = self.getInput<double>("orientation_tol"); + + geometry_msgs::QuaternionStamped quat_goal; + if (!quat) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> Incorrect or missing input. It needs the following input ports: quat(geometry_msgs::QuaternionStamped) and [Optional] orientation_tol(double)"); + return BT::NodeStatus::FAILURE; + } + + quat_goal = quat.value(); + if (!orientation_tol) + this->tiago_arm_module.add_orientation_path_constraint(quat_goal); + else + { + double orientation_tol_goal = orientation_tol.value(); + this->tiago_arm_module.add_orientation_path_constraint(quat_goal, orientation_tol_goal); + } + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> tiago_arm_add_orientation_path_constraint"); + BT::Optional<path_const_plane_t> plane = self.getInput<path_const_plane_t>("plane"); + BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id"); + BT::Optional<double> offset = self.getInput<double>("offset"); + BT::Optional<double> position_tol = self.getInput<double>("position_tol"); + + path_const_plane_t plane_goal; + std::string frame_id_goal; + double offset_goal; + if (!plane || !frame_id || (!offset && position_tol)) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> Incorrect or missing input. It needs the following input ports: plane(geometry_msgs::QuaternionStamped), frame_id(geometry_msgs::QuaternionStamped), [Optional] offset(double) and [Optional] position_tol(double)"); + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> The correct input options are: plane + frame_id, plane + frame_id + offset or plane + frame_id + offset + position_tol"); + return BT::NodeStatus::FAILURE; + } + + plane_goal = plane.value(); + frame_id_goal = frame_id.value(); + if (!position_tol) + { + if (!offset) + this->tiago_arm_module.add_plane_path_constraint(plane_goal, frame_id_goal); + else + { + offset_goal = offset.value(); + this->tiago_arm_module.add_plane_path_constraint(plane_goal, frame_id_goal, offset_goal); + } + } + else + { + double position_tol_goal = position_tol.value(); + this->tiago_arm_module.add_plane_path_constraint(plane_goal, frame_id_goal, offset_goal, position_tol_goal); + } + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal-> tiago_arm_add_plane_path_constraint_normal"); + BT::Optional<std::vector<double> > normal = self.getInput<std::vector<double> >("normal"); + BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id"); + BT::Optional<double> offset = self.getInput<double>("offset"); + BT::Optional<double> position_tol = self.getInput<double>("position_tol"); + + std::vector<double> normal_goal; + std::string frame_id_goal; + double offset_goal; + if (!normal || !frame_id || (!offset && position_tol)) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal-> Incorrect or missing input. It needs the following input ports: normal(geometry_msgs::QuaternionStamped), frame_id(geometry_msgs::QuaternionStamped), [Optional] offset(double) and [Optional] position_tol(double)"); + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal-> The correct input options are: normal + frame_id, normal + frame_id + offset or normal + frame_id + offset + position_tol"); + return BT::NodeStatus::FAILURE; + } + + normal_goal = normal.value(); + frame_id_goal = frame_id.value(); + if (!position_tol) + { + if (!offset) + this->tiago_arm_module.add_plane_path_constraint(normal_goal, frame_id_goal); + else + { + offset_goal = offset.value(); + this->tiago_arm_module.add_plane_path_constraint(normal_goal, frame_id_goal, offset_goal); + } + } + else + { + double position_tol_goal = position_tol.value(); + this->tiago_arm_module.add_plane_path_constraint(normal_goal, frame_id_goal, offset_goal, position_tol_goal); + } + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_joint_goal_constraint(BT::TreeNode& self) +{ + ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_joint_goal_constraint-> tiago_arm_add_joint_goal_constraint"); + BT::Optional<std::vector<std::string> > joint_names = self.getInput<std::vector<std::string> >("joint_names"); + BT::Optional<std::vector<double> > position = self.getInput<std::vector<double> >("position"); + BT::Optional<std::vector<double> > tol = self.getInput<std::vector<double> >("tol"); + + std::vector<std::string> joint_names_goal; + std::vector<double> position_goal, tol_goal; + if (!joint_names || !position || !tol) + { + ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_joint_goal_constraint-> Incorrect or missing input. It needs the following input ports: joint_names(std::vector<std::string>), position(std::vector<double>) and tol(std::vector<double>)"); + return BT::NodeStatus::FAILURE; + } + + joint_names_goal = joint_names.value(); + position_goal = position.value(); + tol_goal = tol.value(); + + this->tiago_arm_module.add_joint_goal_constraint(joint_names_goal, position_goal, tol_goal); + return BT::NodeStatus::SUCCESS; +} + BT::NodeStatus CTiagoArmModuleBT::sync_cancel_tiago_arm_action(void) { ROS_DEBUG("CTiagoArmModuleBT::sync_cancel_tiago_arm_action-> sync_cancel_tiago_arm_action");