Add Heracles DB update service client

robot_executor_interface/robot_executor_interface/src/robot_executor_interface/action_descriptions.py

@@ -21,6 +21,7 @@ class Gaze:
     frame: str
     robot_point: np.ndarray
     gaze_point: np.ndarray
+    object_id: str
     stow_after: bool = False
 
 
@@ -30,6 +31,7 @@ class Pick:
     object_class: str
     robot_point: np.ndarray
     object_point: np.ndarray
+    object_id: str
 
 
 @dataclass
@@ -38,3 +40,4 @@ class Place:
     object_class: str
     robot_point: np.ndarray
     object_point: np.ndarray
+    object_id: str

robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py

@@ -153,12 +153,14 @@ def gaze_from_msg(msg):
     robot_point = np.array([msg.robot_point.x, msg.robot_point.y, msg.robot_point.z])
     gaze_point = np.array([msg.gaze_point.x, msg.gaze_point.y, msg.gaze_point.z])
     stow_after = msg.place_frame == "STOW"
+    object_id = msg.object_id
 
     return Gaze(
         frame=msg.gaze_frame,
         robot_point=robot_point,
         gaze_point=gaze_point,
         stow_after=stow_after,
+        object_id=object_id,
     )
 
 
@@ -167,8 +169,6 @@ def _(action: Gaze):
     msg = ActionMsg()
     msg.action_type = msg.GAZE
 
-    print(action.robot_point[0])
-    print(type(action.robot_point[0]))
     msg.robot_point.x = action.robot_point[0]
     msg.robot_point.y = action.robot_point[1]
     msg.robot_point.z = action.robot_point[2]
@@ -179,6 +179,8 @@ def _(action: Gaze):
 
     msg.place_frame = "STOW" if action.stow_after else "NO_STOW"
 
+    msg.object_id = action.object_id
+
     return msg
 
 
@@ -223,6 +225,7 @@ def pick_from_msg(msg):
         object_class=msg.object_class,
         robot_point=robot_point,
         object_point=object_point,
+        object_id=msg.object_id,
     )
 
 
@@ -240,6 +243,7 @@ def _(action: Pick):
     msg.object_point.z = action.object_point[2]
 
     msg.object_class = action.object_class
+    msg.object_id = action.object_id
 
     return msg
 
@@ -285,6 +289,7 @@ def place_from_msg(msg):
         object_class=msg.object_class,
         robot_point=robot_point,
         object_point=object_point,
+        object_id=msg.object_id,
     )
 
 
@@ -301,6 +306,8 @@ def _(action: Place):
     msg.object_point.y = action.object_point[1]
     msg.object_point.z = action.object_point[2]
 
+    msg.object_id = action.object_id
+
     return msg
 
 

robot_executor_interface/robot_executor_msgs/msg/ActionMsg.msg

@@ -24,6 +24,7 @@ geometry_msgs/Point gaze_point
 string pick_frame
 string object_class
 geometry_msgs/Point object_point
+string object_id
 
 # Definition for PLACE
 string place_frame

spot_tools/src/spot_executor/executor_feedback_pyplot.py

@@ -81,3 +81,10 @@ def bounding_box_detection_feedback(
         cv2.imshow("Most Confident Output", annotated_img)
         cv2.waitKey(0)
         cv2.destroyAllWindows()
+
+    def set_robot_holding_state(self, is_holding: bool, object_id: str, timeout=5):
+        action = "picked up" if is_holding else "placed down"
+        print(
+            f"[HOLDING STATE] Robot {action} object '{object_id}' (is_holding={is_holding})"
+        )
+        return True

spot_tools/src/spot_executor/spot_executor.py

@@ -294,13 +294,23 @@ 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, command.object_id.upper())
+
         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
+            feedback.set_robot_holding_state(False, command.object_id.upper())
+
         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,10 @@ 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 +244,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!")