diff --git a/include/tiago_gripper_module/tiago_gripper_module_bt.h b/include/tiago_gripper_module/tiago_gripper_module_bt.h index cc2cab49a361a973b1612c87f4b89e916e1e7032..d234c53b15c17d5806a8d34cff2da9d12bc1a2b6 100644 --- a/include/tiago_gripper_module/tiago_gripper_module_bt.h +++ b/include/tiago_gripper_module/tiago_gripper_module_bt.h @@ -292,7 +292,7 @@ class CTiagoGripperModuleBT BT::NodeStatus async_fingers_distance_tiago_gripper(BT::TreeNode& self); /** - * \brief Synchronized stop TIAGo gripperfunction. + * \brief Synchronized stop TIAGo gripper function. * * This function calls stop of tiago_gripper_module. As this is a * synchronous action is_tiago_gripper_finished() must be used to know when the action @@ -303,7 +303,23 @@ class CTiagoGripperModuleBT * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE. * */ - BT::NodeStatus cancel_tiago_gripper_action(void); + BT::NodeStatus sync_cancel_tiago_gripper_action(void); + + /** + * \brief Async stop TIAGo gripper function. + * + * This function calls stop of tiago_nav_module. As this is + * an asynchronous action is_tiago_nav_finished() is checked to know when the action + * has actually finished. + * + * \return a BT:NodeStatus indicating whether the request has been + * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the + * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still + * in progress (BT:NodeStatus::RUNNING). If inputs are missing or incorrect + * it also returns BT:NodeStatus::FAILURE + * + */ + BT::NodeStatus async_cancel_tiago_gripper_action(void); /** * \brief Checks if the module is idle and ther isn't any new movement or grasp request. diff --git a/src/tiago_gripper_module_bt.cpp b/src/tiago_gripper_module_bt.cpp index 82adfd2970a46b7ee76ec2ffb5e1b9205f21052f..934717b62d7607a554fe5a0018b4b19be7a56c74 100644 --- a/src/tiago_gripper_module_bt.cpp +++ b/src/tiago_gripper_module_bt.cpp @@ -36,7 +36,9 @@ void CTiagoGripperModuleBT::init(IriBehaviorTreeFactory &factory) factory.registerSimpleCondition("is_tiago_gripper_module_failed_grasp", std::bind(&CTiagoGripperModuleBT::is_tiago_gripper_module_failed_grasp, this)); //Registration of actions - factory.registerSimpleAction("cancel_tiago_gripper_action", std::bind(&CTiagoGripperModuleBT::cancel_tiago_gripper_action, this)); + factory.registerSimpleAction("sync_cancel_tiago_gripper_action", std::bind(&CTiagoGripperModuleBT::sync_cancel_tiago_gripper_action, this)); + factory.registerSimpleAction("async_cancel_tiago_gripper_action", std::bind(&CTiagoGripperModuleBT::async_cancel_tiago_gripper_action, this)); + factory.registerIriAsyncAction("async_is_tiago_gripper_finished", std::bind(&CTiagoGripperModuleBT::async_is_tiago_gripper_finished, this)); factory.registerSimpleAction("sync_close_tiago_gripper", std::bind(&CTiagoGripperModuleBT::sync_close_tiago_gripper, this, std::placeholders::_1), sync_open_close_ports); @@ -368,13 +370,28 @@ BT::NodeStatus CTiagoGripperModuleBT::async_fingers_distance_tiago_gripper(BT::T return BT::NodeStatus::RUNNING; } -BT::NodeStatus CTiagoGripperModuleBT::cancel_tiago_gripper_action(void) +BT::NodeStatus CTiagoGripperModuleBT::sync_cancel_tiago_gripper_action(void) { - ROS_DEBUG("CTiagoGripperModuleBT::cancel_tiago_gripper_action-> cancel_tiago_gripper_action"); + ROS_DEBUG("CTiagoGripperModuleBT::sync_cancel_tiago_gripper_action-> sync_cancel_tiago_gripper_action"); this->tiago_gripper_module.stop(); return BT::NodeStatus::SUCCESS; } +BT::NodeStatus CTiagoGripperModuleBT::async_cancel_tiago_gripper_action(void) +{ + ROS_DEBUG("CTiagoNavModuleBT::async_cancel_tiago_gripper_action-> async_cancel_tiago_gripper_action"); + this->tiago_gripper_module.stop(); + if (this->tiago_gripper_module.is_finished()) + { + tiago_gripper_module_status_t tiago_gripper_module_status = this->tiago_gripper_module.get_status(); + if (tiago_gripper_module_status == TIAGO_GRIPPER_MODULE_SUCCESS) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; + } + return BT::NodeStatus::RUNNING; +} + BT::NodeStatus CTiagoGripperModuleBT::is_tiago_gripper_finished(void) { if (this->tiago_gripper_module.is_finished()) diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml index 953177456c2a3129da506f47bd02c305aa3ca01b..36bf5f9ad94ada1e8a5b448f04e222b1911a256a 100644 --- a/src/xml/bt_test.xml +++ b/src/xml/bt_test.xml @@ -22,7 +22,8 @@ </BehaviorTree> <!-- ////////// --> <TreeNodesModel> - <Action ID="cancel_tiago_gripper_action"/> + <Action ID="sync_cancel_tiago_gripper_action"/> + <Action ID="async_cancel_tiago_gripper_action"/> <Action ID="async_is_tiago_gripper_finished"/> <Action ID="sync_close_tiago_gripper"> <input_port default="-1.0" name="duration"> Time from start to close the gripper</input_port>