hard-coded heracles hold objects

Author: toyat522

spot_tools/src/spot_executor/spot_executor.py

@@ -294,13 +294,24 @@ def execute_pick(self, command, feedback):
 
             feedback.pick_image_feedback(sem_img, outline_img)
 
+        if success:
+            # Update object holding state
+            # TODO: don't hard-code the object to hold
+            feedback.set_robot_holding_state(True, "O43")
+
         feedback.print("INFO", "Finished `pick` command")
         feedback.print("INFO", f"Pick skill success: {success}")
         return success
 
     def execute_place(self, command, feedback):
         feedback.print("INFO", "Executing `place` command")
         success = object_place(self.spot_interface, semantic_class=command.object_class)
+
+        if success:
+            # Update object holding state
+            # TODO: don't hard-code the object to hold
+            feedback.set_robot_holding_state(False, "O43")
+
         feedback.print("INFO", "Finished `place` command")
         return success
 

spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py

@@ -10,6 +10,7 @@
 import tf2_ros
 import yaml
 from cv_bridge import CvBridge
+from heracles_ros_interfaces.srv import UpdateHoldingState
 from nav_msgs.msg import Path
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
@@ -229,6 +230,11 @@ def register_publishers(self, node):
             10,
         )
 
+        self.holding_client = node.create_client(
+            UpdateHoldingState,
+            "update_holding_state"
+        )
+
         # TODO(aaron): Once we switch logging to python logger,
         # should move into init
         self.logger.info(f"Logging to: {self.output_dir}")
@@ -239,6 +245,20 @@ def register_publishers(self, node):
         with open(log_fn, "w") as fo:
             fo.write("time,event\n")
 
+    def set_robot_holding_state(self, is_holding: bool, object_id: str, timeout=5):
+        req = UpdateHoldingState.Request()
+        req.is_holding = is_holding
+        req.id = object_id
+
+        future = self.holding_client.call_async(req)
+        start = time.time()
+        while not future.done():
+            if time.time() - start > timeout:
+                self.logger.error("UpdateHoldingState call timed out")
+                return False
+
+        return future.result().success
+
     def pick_confirmation_callback(self, msg):
         if msg.data:
             self.logger.info("Detection is valid. Continuing pick action!")