From 9d9d35517f1051bf0d4d7152fe314375583d8a8c Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Sun, 23 Nov 2025 22:38:34 -0500 Subject: [PATCH 1/5] hard-coded heracles hold objects --- spot_tools/src/spot_executor/spot_executor.py | 11 ++++++++++ .../src/spot_tools_ros/spot_executor_ros.py | 20 +++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/spot_tools/src/spot_executor/spot_executor.py b/spot_tools/src/spot_executor/spot_executor.py index 8bee53f..5408631 100644 --- a/spot_tools/src/spot_executor/spot_executor.py +++ b/spot_tools/src/spot_executor/spot_executor.py @@ -294,6 +294,11 @@ 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 @@ -301,6 +306,12 @@ def execute_pick(self, command, feedback): 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 diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index dcc381a..0281846 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/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!") From 05b5abfb177d1d3ba821d647707502c3d3161b02 Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Thu, 27 Nov 2025 14:33:11 -0500 Subject: [PATCH 2/5] fixed lint --- spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index 0281846..8b7dc0b 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -231,8 +231,7 @@ def register_publishers(self, node): ) self.holding_client = node.create_client( - UpdateHoldingState, - "update_holding_state" + UpdateHoldingState, "update_holding_state" ) # TODO(aaron): Once we switch logging to python logger, From 8e9550ef4cab665a0bf49d972db8b725ce4f9e83 Mon Sep 17 00:00:00 2001 From: Aaron Ray Date: Thu, 4 Dec 2025 20:25:16 +0000 Subject: [PATCH 3/5] Support object index (symbol) as part of action messages --- .../robot_executor_interface/action_descriptions.py | 3 +++ .../action_descriptions_ros.py | 11 +++++++++-- .../robot_executor_msgs/msg/ActionMsg.msg | 1 + 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/action_descriptions.py b/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/action_descriptions.py index 5950998..8376aca 100644 --- a/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/action_descriptions.py +++ b/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 diff --git a/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py b/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py index 7204fb8..5f238a9 100644 --- a/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py +++ b/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, + obejct_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 diff --git a/robot_executor_interface/robot_executor_msgs/msg/ActionMsg.msg b/robot_executor_interface/robot_executor_msgs/msg/ActionMsg.msg index 92cd8b7..9d494ac 100644 --- a/robot_executor_interface/robot_executor_msgs/msg/ActionMsg.msg +++ b/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 From c11ae0fcbf404bb61187e3e3f64ae668a4b8a8f6 Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Thu, 18 Dec 2025 00:05:58 -0500 Subject: [PATCH 4/5] confirmed working custom object pick and place and added non-ros temporary code for set_robot_holding_state --- .../robot_executor_interface_ros/action_descriptions_ros.py | 2 +- spot_tools/src/spot_executor/executor_feedback_pyplot.py | 5 +++++ spot_tools/src/spot_executor/spot_executor.py | 5 ++--- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py b/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py index 5f238a9..306b316 100644 --- a/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py +++ b/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py @@ -289,7 +289,7 @@ def place_from_msg(msg): object_class=msg.object_class, robot_point=robot_point, object_point=object_point, - obejct_id=msg.object_id, + object_id=msg.object_id, ) diff --git a/spot_tools/src/spot_executor/executor_feedback_pyplot.py b/spot_tools/src/spot_executor/executor_feedback_pyplot.py index ccdf1ad..aa577af 100644 --- a/spot_tools/src/spot_executor/executor_feedback_pyplot.py +++ b/spot_tools/src/spot_executor/executor_feedback_pyplot.py @@ -81,3 +81,8 @@ 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 diff --git a/spot_tools/src/spot_executor/spot_executor.py b/spot_tools/src/spot_executor/spot_executor.py index 5408631..e952249 100644 --- a/spot_tools/src/spot_executor/spot_executor.py +++ b/spot_tools/src/spot_executor/spot_executor.py @@ -297,7 +297,7 @@ def execute_pick(self, command, feedback): if success: # Update object holding state # TODO: don't hard-code the object to hold - feedback.set_robot_holding_state(True, "O43") + feedback.set_robot_holding_state(True, command.object_id.upper()) feedback.print("INFO", "Finished `pick` command") feedback.print("INFO", f"Pick skill success: {success}") @@ -309,8 +309,7 @@ def execute_place(self, command, feedback): if success: # Update object holding state - # TODO: don't hard-code the object to hold - feedback.set_robot_holding_state(False, "O43") + feedback.set_robot_holding_state(False, command.object_id.upper()) feedback.print("INFO", "Finished `place` command") return success From 253eff95a4c2c9f28f352f5bef0d371c2768812e Mon Sep 17 00:00:00 2001 From: Toya Takahashi Date: Thu, 18 Dec 2025 00:06:26 -0500 Subject: [PATCH 5/5] fix lint --- spot_tools/src/spot_executor/executor_feedback_pyplot.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/spot_tools/src/spot_executor/executor_feedback_pyplot.py b/spot_tools/src/spot_executor/executor_feedback_pyplot.py index aa577af..73174b0 100644 --- a/spot_tools/src/spot_executor/executor_feedback_pyplot.py +++ b/spot_tools/src/spot_executor/executor_feedback_pyplot.py @@ -84,5 +84,7 @@ def bounding_box_detection_feedback( 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})") + print( + f"[HOLDING STATE] Robot {action} object '{object_id}' (is_holding={is_holding})" + ) return True