Make toggle collision object asynch

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