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..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 @@ -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 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 diff --git a/spot_tools/src/spot_executor/executor_feedback_pyplot.py b/spot_tools/src/spot_executor/executor_feedback_pyplot.py index ccdf1ad..73174b0 100644 --- a/spot_tools/src/spot_executor/executor_feedback_pyplot.py +++ b/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 diff --git a/spot_tools/src/spot_executor/spot_executor.py b/spot_tools/src/spot_executor/spot_executor.py index 8bee53f..e952249 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, command.object_id.upper()) + feedback.print("INFO", "Finished `pick` command") feedback.print("INFO", f"Pick skill success: {success}") return success @@ -301,6 +306,11 @@ 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 + feedback.set_robot_holding_state(False, command.object_id.upper()) + 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..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 @@ -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!")