From 567d2abfb0d1363611931f5fc7d51ac20ec7e0bd Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Thu, 14 Sep 2023 13:30:52 -0700
Subject: [PATCH] Make toggle collision object asynch

---
 .../behaviors/toggle_collision_object.py      | 38 ++++++++++++++++---
 1 file changed, 32 insertions(+), 6 deletions(-)

diff --git a/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py b/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py
index abbb8914..9e980906 100644
--- a/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py
+++ b/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py
@@ -60,6 +60,18 @@ def __init__(
             self.node,
         )
 
+    # pylint: disable=attribute-defined-outside-init
+    # For attributes that are only used during the execution of the tree
+    # and get reset before the next execution, it is reasonable to define
+    # them in `initialise`.
+    def initialise(self) -> None:
+        """
+        Reset the service_future.
+        """
+        self.logger.info(f"{self.name} [ToggleCollisionObject::initialise()]")
+
+        self.service_future = None
+
     def update(self) -> py_trees.common.Status:
         """
         (Dis)allow collisions between the robot and a collision
@@ -71,10 +83,24 @@ def update(self) -> py_trees.common.Status:
         """
         self.logger.info(f"{self.name} [ToggleCollisionObject::update()]")
         # (Dis)allow collisions between the robot and the collision object
-        with self.moveit2_lock:
-            succ = self.moveit2.allow_collisions(self.collision_object_id, self.allow)
+        if self.service_future is None:
+            with self.moveit2_lock:
+                service_future = self.moveit2.allow_collisions(
+                    self.collision_object_id, self.allow
+                )
+            if service_future is None:  # service not available
+                return py_trees.common.Status.FAILURE
+            self.service_future = service_future
+            return py_trees.common.Status.RUNNING
+
+        # Check if the service future is done
+        if self.service_future.done():
+            with self.moveit2_lock:
+                succ = self.moveit2.process_allow_collision_future(self.service_future)
+            # Return success/failure
+            if succ:
+                return py_trees.common.Status.SUCCESS
+            return py_trees.common.Status.FAILURE
 
-        # Return success
-        if succ:
-            return py_trees.common.Status.SUCCESS
-        return py_trees.common.Status.FAILURE
+        # Return running
+        return py_trees.common.Status.RUNNING