From 01ca63b02f507b445683ee5bfaca6b15d397cbb4 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Tue, 17 Nov 2020 17:27:46 +0100
Subject: [PATCH] Added async_cancel function
---
.../tiago_gripper_module_bt.h | 20 ++++++++++++++--
src/tiago_gripper_module_bt.cpp | 23 ++++++++++++++++---
src/xml/bt_test.xml | 3 ++-
3 files changed, 40 insertions(+), 6 deletions(-)
diff --git a/include/tiago_gripper_module/tiago_gripper_module_bt.h b/include/tiago_gripper_module/tiago_gripper_module_bt.h
index cc2cab4..d234c53 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 82adfd2..934717b 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 9531774..36bf5f9 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>
--
GitLab