From adfb0b174c484685220634f58c3e559065a939f4 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 18:48:13 +0000 Subject: [PATCH 01/66] refactored code into modules --- spot_wrapper/graph_nav_util.py | 130 -- spot_wrapper/spot_arm.py | 512 +++++++ spot_wrapper/spot_check.py | 260 ++++ spot_wrapper/spot_config.py | 66 + spot_wrapper/spot_docking.py | 60 + spot_wrapper/spot_eap.py | 73 + spot_wrapper/spot_graph_nav.py | 796 ++++++++++ spot_wrapper/spot_images.py | 273 ++++ spot_wrapper/spot_world_objects.py | 68 + spot_wrapper/tests/test_graph_nav_util.py | 222 +++ spot_wrapper/wrapper.py | 1695 ++++----------------- 11 files changed, 2607 insertions(+), 1548 deletions(-) delete mode 100644 spot_wrapper/graph_nav_util.py create mode 100644 spot_wrapper/spot_arm.py create mode 100644 spot_wrapper/spot_check.py create mode 100644 spot_wrapper/spot_config.py create mode 100644 spot_wrapper/spot_docking.py create mode 100644 spot_wrapper/spot_eap.py create mode 100644 spot_wrapper/spot_graph_nav.py create mode 100644 spot_wrapper/spot_images.py create mode 100644 spot_wrapper/spot_world_objects.py create mode 100755 spot_wrapper/tests/test_graph_nav_util.py diff --git a/spot_wrapper/graph_nav_util.py b/spot_wrapper/graph_nav_util.py deleted file mode 100644 index e35357e4..00000000 --- a/spot_wrapper/graph_nav_util.py +++ /dev/null @@ -1,130 +0,0 @@ -# Copyright (c) 2020 Boston Dynamics, Inc. All rights reserved. -# -# Downloading, reproducing, distributing or otherwise using the SDK Software -# is subject to the terms and conditions of the Boston Dynamics Software -# Development Kit License (20191101-BDSDK-SL). - -"""Graph nav utility functions""" - - -def id_to_short_code(id): - """Convert a unique id to a 2 letter short code.""" - tokens = id.split("-") - if len(tokens) > 2: - return "%c%c" % (tokens[0][0], tokens[1][0]) - return None - - -def pretty_print_waypoints( - waypoint_id, waypoint_name, short_code_to_count, localization_id, logger -): - short_code = id_to_short_code(waypoint_id) - if short_code is None or short_code_to_count[short_code] != 1: - short_code = " " # If the short code is not valid/unique, don't show it. - - logger.info( - "%s Waypoint name: %s id: %s short code: %s" - % ( - "->" if localization_id == waypoint_id else " ", - waypoint_name, - waypoint_id, - short_code, - ) - ) - - -def find_unique_waypoint_id(short_code, graph, name_to_id, logger): - """Convert either a 2 letter short code or an annotation name into the associated unique id.""" - if len(short_code) != 2: - # Not a short code, check if it is an annotation name (instead of the waypoint id). - if short_code in name_to_id: - # Short code is an waypoint's annotation name. Check if it is paired with a unique waypoint id. - if name_to_id[short_code] is not None: - # Has an associated waypoint id! - return name_to_id[short_code] - else: - logger.error( - "The waypoint name %s is used for multiple different unique waypoints. Please use" - + "the waypoint id." % (short_code) - ) - return None - # Also not an waypoint annotation name, so we will operate under the assumption that it is a - # unique waypoint id. - return short_code - - ret = short_code - for waypoint in graph.waypoints: - if short_code == id_to_short_code(waypoint.id): - if ret != short_code: - return short_code # Multiple waypoints with same short code. - ret = waypoint.id - return ret - - -def update_waypoints_and_edges(graph, localization_id, logger): - """Update and print waypoint ids and edge ids.""" - name_to_id = dict() - edges = dict() - - short_code_to_count = {} - waypoint_to_timestamp = [] - for waypoint in graph.waypoints: - # Determine the timestamp that this waypoint was created at. - timestamp = -1.0 - try: - timestamp = ( - waypoint.annotations.creation_time.seconds - + waypoint.annotations.creation_time.nanos / 1e9 - ) - except: - # Must be operating on an older graph nav map, since the creation_time is not - # available within the waypoint annotations message. - pass - waypoint_to_timestamp.append( - (waypoint.id, timestamp, waypoint.annotations.name) - ) - - # Determine how many waypoints have the same short code. - short_code = id_to_short_code(waypoint.id) - if short_code not in short_code_to_count: - short_code_to_count[short_code] = 0 - short_code_to_count[short_code] += 1 - - # Add the annotation name/id into the current dictionary. - waypoint_name = waypoint.annotations.name - if waypoint_name: - if waypoint_name in name_to_id: - # Waypoint name is used for multiple different waypoints, so set the waypoint id - # in this dictionary to None to avoid confusion between two different waypoints. - name_to_id[waypoint_name] = None - else: - # First time we have seen this waypoint annotation name. Add it into the dictionary - # with the respective waypoint unique id. - name_to_id[waypoint_name] = waypoint.id - - # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, - # fallback to sorting by annotation name. - waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) - - # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from - # when the waypoint was created. - logger.info("%d waypoints:" % len(graph.waypoints)) - for waypoint in waypoint_to_timestamp: - pretty_print_waypoints( - waypoint[0], waypoint[2], short_code_to_count, localization_id, logger - ) - - for edge in graph.edges: - if edge.id.to_waypoint in edges: - if edge.id.from_waypoint not in edges[edge.id.to_waypoint]: - edges[edge.id.to_waypoint].append(edge.id.from_waypoint) - else: - edges[edge.id.to_waypoint] = [edge.id.from_waypoint] - logger.info( - "(Edge) from waypoint id: ", - edge.id.from_waypoint, - " and to waypoint id: ", - edge.id.to_waypoint, - ) - - return name_to_id, edges diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py new file mode 100644 index 00000000..a06e3fab --- /dev/null +++ b/spot_wrapper/spot_arm.py @@ -0,0 +1,512 @@ +import time +import typing +import logging + +from bosdyn.util import seconds_to_duration +from bosdyn.client.frame_helpers import ( + ODOM_FRAME_NAME, + GRAV_ALIGNED_BODY_FRAME_NAME, + get_a_tform_b, +) +from bosdyn.client import robot_command, math_helpers +from bosdyn.client.robot import Robot +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client.robot_command import ( + RobotCommandBuilder, + RobotCommandClient, + block_until_arm_arrives, +) +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.api import robot_command_pb2 +from bosdyn.api import arm_command_pb2 +from bosdyn.api import synchronized_command_pb2 +from bosdyn.api import geometry_pb2 +from bosdyn.api import trajectory_pb2 +from bosdyn.api import manipulation_api_pb2 +from google.protobuf.duration_pb2 import Duration + +from geometry_msgs.msg import Pose +from spot_msgs.srv import HandPose, HandPoseResponse, HandPoseRequest +from spot_msgs.srv import ( + ArmForceTrajectory, + ArmForceTrajectoryResponse, + ArmForceTrajectoryRequest, +) + + +class SpotArm: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + """ + Constructor for SpotArm class. + :param robot: Robot object + :param logger: Logger object + :param robot_params: Dictionary of robot parameters + - robot_params['is_standing']: True if robot is standing, False otherwise + :param robot_clients: Dictionary of robot clients + - robot_clients['robot_command_client']: RobotCommandClient object + - robot_clients['robot_command_method']: RobotCommand method + """ + self._robot = robot + self._logger = logger + self._robot_params = robot_params + self._robot_command_client: RobotCommandClient = robot_clients[ + "robot_command_client" + ] + self._manipulation_client: ManipulationApiClient = robot_clients[ + "manipulation_client" + ] + self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] + self._robot_command: typing.Callable = robot_clients["robot_command_method"] + + def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: + if not self._robot.has_arm(): + return False, "Spot with an arm is required for this service" + + try: + self._logger.info("Spot is powering on within the timeout of 20 secs") + self._robot.power_on(timeout_sec=20) + assert self._robot.is_powered_on(), "Spot failed to power on" + self._logger.info("Spot is powered on") + except Exception as e: + return ( + False, + "Exception occured while Spot or its arm were trying to power on", + ) + + if not self._robot_params["is_standing"]: + robot_command.blocking_stand( + command_client=self._robot_command_client, timeout_sec=10 + ) + self._logger.info("Spot is standing") + else: + self._logger.info("Spot is already standing") + + return True, "Spot has an arm, is powered on, and standing" + + def arm_stow(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Stow Arm + stow = RobotCommandBuilder.arm_stow_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(stow) + self._logger.info("Command stow issued") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while trying to stow" + + return True, "Stow arm success" + + def arm_unstow(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Unstow Arm + unstow = RobotCommandBuilder.arm_ready_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(unstow) + self._logger.info("Command unstow issued") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while trying to unstow" + + return True, "Unstow arm success" + + def arm_carry(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Get Arm in carry mode + carry = RobotCommandBuilder.arm_carry_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(carry) + self._logger.info("Command carry issued") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while carry mode was issued" + + return True, "Carry mode success" + + def make_arm_trajectory_command( + self, arm_joint_trajectory: arm_command_pb2.ArmJointTrajectory + ) -> robot_command_pb2.RobotCommand: + """Helper function to create a RobotCommand from an ArmJointTrajectory. + Copy from 'spot-sdk/python/examples/arm_joint_move/arm_joint_move.py'""" + + joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request( + trajectory=arm_joint_trajectory + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_joint_move_command=joint_move_command + ) + sync_arm = synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + arm_sync_robot_cmd = robot_command_pb2.RobotCommand( + synchronized_command=sync_arm + ) + return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd) + + def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: + # All perspectives are given when looking at the robot from behind after the unstow service is called + # Joint1: 0.0 arm points to the front. positive: turn left, negative: turn right) + # RANGE: -3.14 -> 3.14 + # Joint2: 0.0 arm points to the front. positive: move down, negative move up + # RANGE: 0.4 -> -3.13 ( + # Joint3: 0.0 arm straight. moves the arm down + # RANGE: 0.0 -> 3.1415 + # Joint4: 0.0 middle position. negative: moves ccw, positive moves cw + # RANGE: -2.79253 -> 2.79253 + # # Joint5: 0.0 gripper points to the front. positive moves the gripper down + # RANGE: -1.8326 -> 1.8326 + # Joint6: 0.0 Gripper is not rolled, positive is ccw + # RANGE: -2.87 -> 2.87 + # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0] + if abs(joint_targets[0]) > 3.14: + msg = "Joint 1 has to be between -3.14 and 3.14" + self._logger.warn(msg) + return False, msg + elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13: + msg = "Joint 2 has to be between -3.13 and 0.4" + self._logger.warn(msg) + return False, msg + elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0: + msg = "Joint 3 has to be between 0.0 and 3.14" + self._logger.warn(msg) + return False, msg + elif abs(joint_targets[3]) > 2.79253: + msg = "Joint 4 has to be between -2.79253 and 2.79253" + self._logger.warn(msg) + return False, msg + elif abs(joint_targets[4]) > 1.8326: + msg = "Joint 5 has to be between -1.8326 and 1.8326" + self._logger.warn(msg) + return False, msg + elif abs(joint_targets[5]) > 2.87: + msg = "Joint 6 has to be between -2.87 and 2.87" + self._logger.warn(msg) + return False, msg + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + trajectory_point = ( + RobotCommandBuilder.create_arm_joint_trajectory_point( + joint_targets[0], + joint_targets[1], + joint_targets[2], + joint_targets[3], + joint_targets[4], + joint_targets[5], + ) + ) + arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory( + points=[trajectory_point] + ) + arm_command = self.make_arm_trajectory_command(arm_joint_trajectory) + + # Send the request + cmd_id = self._robot_command_client.robot_command(arm_command) + + # Query for feedback to determine how long it will take + feedback_resp = self._robot_command_client.robot_command_feedback( + cmd_id + ) + joint_move_feedback = ( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback + ) + time_to_goal: Duration = joint_move_feedback.time_to_goal + time_to_goal_in_seconds: float = time_to_goal.seconds + ( + float(time_to_goal.nanos) / float(10**9) + ) + time.sleep(time_to_goal_in_seconds) + return True, "Spot Arm moved successfully" + + except Exception as e: + return False, "Exception occured during arm movement: " + str(e) + + def force_trajectory( + self, data: ArmForceTrajectoryRequest + ) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + + def create_wrench_from_msg(forces, torques): + force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) + torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) + return geometry_pb2.Wrench(force=force, torque=torque) + + # Duration in seconds. + traj_duration = data.duration + + # first point on trajectory + wrench0 = create_wrench_from_msg(data.forces_pt0, data.torques_pt0) + t0 = seconds_to_duration(0) + traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( + wrench=wrench0, time_since_reference=t0 + ) + + # Second point on the trajectory + wrench1 = create_wrench_from_msg(data.forces_pt1, data.torques_pt1) + t1 = seconds_to_duration(traj_duration) + traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( + wrench=wrench1, time_since_reference=t1 + ) + + # Build the trajectory + trajectory = trajectory_pb2.WrenchTrajectory( + points=[traj_point0, traj_point1] + ) + + # Build the trajectory request, putting all axes into force mode + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + root_frame_name=data.frame, + wrench_trajectory_in_task=trajectory, + x_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + y_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + z_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + rx_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + ry_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + rz_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command + ) + synchronized_command = ( + synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + ) + robot_command = robot_command_pb2.RobotCommand( + synchronized_command=synchronized_command + ) + + # Send the request + self._robot_command_client.robot_command(robot_command) + self._logger.info("Force trajectory command sent") + + time.sleep(float(traj_duration) + 1.0) + + except Exception as e: + return False, "Exception occured during arm movement" + + return True, "Moved arm successfully" + + def gripper_open(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Open gripper + command = RobotCommandBuilder.claw_gripper_open_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper open sent") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while gripper was moving" + + return True, "Open gripper success" + + def gripper_close(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Close gripper + command = RobotCommandBuilder.claw_gripper_close_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper close sent") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while gripper was moving" + + return True, "Closed gripper successfully" + + def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: + # takes an angle between 0 (closed) and 90 (fully opened) and opens the + # gripper at this angle + if gripper_ang > 90 or gripper_ang < 0: + return False, "Gripper angle must be between 0 and 90" + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # The open angle command does not take degrees but the limits + # defined in the urdf, that is why we have to interpolate + closed = 0.349066 + opened = -1.396263 + angle = gripper_ang / 90.0 * (opened - closed) + closed + command = RobotCommandBuilder.claw_gripper_open_angle_command(angle) + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper open angle sent") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while gripper was moving" + + return True, "Opened gripper successfully" + + def hand_pose(self, data: HandPoseRequest): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + pose_point = data.pose_point + # Move the arm to a spot in front of the robot given a pose for the gripper. + # Build a position to move the arm to (in meters, relative to the body frame origin.) + position = geometry_pb2.Vec3( + x=pose_point.position.x, + y=pose_point.position.y, + z=pose_point.position.z, + ) + + # # Rotation as a quaternion. + rotation = geometry_pb2.Quaternion( + w=pose_point.orientation.w, + x=pose_point.orientation.x, + y=pose_point.orientation.y, + z=pose_point.orientation.z, + ) + + seconds = data.duration + duration = seconds_to_duration(seconds) + + # Build the SE(3) pose of the desired hand position in the moving body frame. + hand_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) + hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose, time_since_reference=duration + ) + hand_trajectory = trajectory_pb2.SE3Trajectory( + points=[hand_pose_traj_point] + ) + + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + root_frame_name=data.frame, + pose_trajectory_in_task=hand_trajectory, + force_remain_near_current_joint_configuration=True, + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command + ) + synchronized_command = ( + synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + ) + + robot_command = robot_command_pb2.RobotCommand( + synchronized_command=synchronized_command + ) + + command = RobotCommandBuilder.build_synchro_command(robot_command) + + # Send the request + self._robot_command_client.robot_command(robot_command) + self._logger.info("Moving arm to position.") + + time.sleep(2.0) + + except Exception as e: + return ( + False, + "An error occured while trying to move arm \n Exception:" + str(e), + ) + + return True, "Moved arm successfully" + + def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): + try: + frm = str(frame) + pos = geometry_pb2.Vec3( + x=object_rt_frame[0], y=object_rt_frame[1], z=object_rt_frame[2] + ) + + grasp = manipulation_api_pb2.PickObject(frame_name=frm, object_rt_frame=pos) + + # Ask the robot to pick up the object + grasp_request = manipulation_api_pb2.ManipulationApiRequest( + pick_object=grasp + ) + # Send the request + cmd_response = self._manipulation_client.manipulation_api_command( + manipulation_api_request=grasp_request + ) + + # Get feedback from the robot + while True: + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_response.manipulation_cmd_id + ) + + # Send the request + response = self._manipulation_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) + + print( + "Current state: ", + manipulation_api_pb2.ManipulationFeedbackState.Name( + response.current_state + ), + ) + + if ( + response.current_state + == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED + or response.current_state + == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED + ): + break + + time.sleep(0.25) + + self._robot.logger.info("Finished grasp.") + + except Exception as e: + return False, "An error occured while trying to grasp from pose" + + return True, "Grasped successfully" diff --git a/spot_wrapper/spot_check.py b/spot_wrapper/spot_check.py new file mode 100644 index 00000000..992d3aa3 --- /dev/null +++ b/spot_wrapper/spot_check.py @@ -0,0 +1,260 @@ +import typing +import logging +import time + +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command +from bosdyn.client.spot_check import SpotCheckClient, run_spot_check +from bosdyn.client.spot_check import spot_check_pb2 +from bosdyn.api import header_pb2 +from google.protobuf.timestamp_pb2 import Timestamp +from bosdyn.client.lease import LeaseClient, LeaseWallet, Lease + + +class SpotCheck: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._spot_check_client: SpotCheckClient = robot_clients["spot_check_client"] + self._robot_command_client: robot_command.RobotCommandClient = robot_clients[ + "robot_command_client" + ] + self._lease_client: LeaseClient = robot_clients["lease_client"] + self._robot_params = robot_params + self._spot_check_resp = None + self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet + + @property + def spot_check_resp(self) -> spot_check_pb2.SpotCheckFeedbackResponse: + return self._spot_check_resp + + def _get_lease(self) -> Lease: + self._lease = self._lease_wallet.get_lease() + return self._lease + + def _feedback_error_check( + self, resp: spot_check_pb2.SpotCheckFeedbackResponse + ) -> typing.Tuple[bool, str]: + """Check for errors in the feedback response""" + + # Save results from Spot Check + self._spot_check_resp = resp + + # Check for common errors + if resp.header.error.code in (2, 3): + return False, str(resp.header.error.message) + + # Check for other errors + if ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_UNEXPECTED_POWER_CHANGE + ): + return False, "Unexpected power change" + elif ( + resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_IMU_CHECK + ): + return False, "Robot body is not flat on the ground" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_NOT_SITTING + ): + return False, "Robot body is not close to a sitting pose" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_LOADCELL_TIMEOUT + ): + return False, "Timeout during loadcell calibration" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_ON_FAILURE + ): + return False, "Error enabling motor power" + elif ( + resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ENDSTOP_TIMEOUT + ): + return False, "Timeout during endstop calibration" + elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FAILED_STAND: + return False, "Robot failed to stand" + elif ( + resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_CAMERA_TIMEOUT + ): + return False, "Timeout during camera check" + elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GROUND_CHECK: + return False, "Flat ground check failed" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_OFF_FAILURE + ): + return False, "Robot failed to power off" + elif ( + resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_REVERT_FAILURE + ): + return False, "Robot failed to revert calibration" + elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FGKC_FAILURE: + return False, "Robot failed to do flat ground kinematic calibration" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GRIPPER_CAL_TIMEOUT + ): + return False, "Timeout during gripper calibration" + + return True, "Successfully ran Spot Check" + + def _req_feedback(self) -> spot_check_pb2.SpotCheckFeedbackResponse: + start_time_seconds, start_time_ns = int(time.time()), int(time.time_ns() % 1e9) + req = spot_check_pb2.SpotCheckFeedbackRequest( + header=header_pb2.RequestHeader( + request_timestamp=Timestamp( + seconds=start_time_seconds, nanos=start_time_ns + ), + client_name="spot-check", + disable_rpc_logging=False, + ) + ) + resp: spot_check_pb2.SpotCheckFeedbackResponse = ( + self._spot_check_client.spot_check_feedback(req) + ) + + self._spot_check_resp = resp + + return resp + + def _spot_check_cmd(self, command: spot_check_pb2.SpotCheckCommandRequest): + """Send a Spot Check command""" + start_time_seconds, start_time_ns = int(time.time()), int(time.time_ns() % 1e9) + req = spot_check_pb2.SpotCheckCommandRequest( + header=header_pb2.RequestHeader( + request_timestamp=Timestamp( + seconds=start_time_seconds, nanos=start_time_ns + ), + client_name="spot-check", + disable_rpc_logging=False, + ), + lease=self._get_lease().lease_proto, + command=command, + ) + self._spot_check_client.spot_check_command(req) + + def stop_check(self) -> typing.Tuple[bool, str]: + """Stop the Spot Check + Note: This may cause the robot to enter a FaultState. Use only in emergencies. + """ + self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_ABORT) + + # Get feedback + resp = self._req_feedback() + + # Check for errors + success, status = self._feedback_error_check(resp) + + if success: + status = "Successfully stopped Spot Check" + self._logger.info(status) + else: + self._logger.error("Failed to stop Spot Check") + + return success, status + + def revert_calibration(self) -> typing.Tuple[bool, str]: + """Revert calibration for Spot Check""" + self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_REVERT_CAL) + + # Get feedback + resp = self._req_feedback() + + # Check for errors + success, status = self._feedback_error_check(resp) + + if success: + status = "Successfully reverted calibration" + self._logger.info(status) + else: + self._logger.error("Failed to revert calibration") + + return success, status + + def start_check(self) -> typing.Tuple[bool, str]: + """Start the Spot Check""" + # Make sure we're powered on and sitting + try: + self._robot.power_on() + if not self._robot_params["is_sitting"]: + robot_command.blocking_sit( + command_client=self._robot_command_client, timeout_sec=10 + ) + self._logger.info("Spot is sitting") + else: + self._logger.info("Spot is already sitting") + + self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_START) + + # Get feedback + resp = self._req_feedback() + + # Check for errors + success, status = self._feedback_error_check(resp) + + if success: + status = "Successfully started Spot Check" + self._logger.info(status) + else: + self._logger.error("Failed to start Spot Check") + return success, status + + except Exception as e: + return False, str(e) + + def blocking_check( + self, + timeout_sec: int = 360, + update_freq: float = 0.25, + verbose: bool = False, + ) -> typing.Tuple[bool, str]: + """Check the robot + Args: + timeout_sec: Timeout for the blocking check + update_freq: Frequency to update the check + verbose: Whether to print the check status + Returns: + Tuple of (success, message) + """ + try: + # Make sure we're powered on and sitting + self._robot.power_on() + if not self._robot_params["is_sitting"]: + robot_command.blocking_sit( + command_client=self._robot_command_client, timeout_sec=10 + ) + self._logger.info("Spot is sitting") + else: + self._logger.info("Spot is already sitting") + + # Check the robot and block for timeout_sec + self._logger.info("Blocking Spot Check is starting!") + resp: spot_check_pb2.SpotCheckFeedbackResponse = run_spot_check( + self._spot_check_client, + self._get_lease(), + timeout_sec, + update_freq, + verbose, + ) + + self._logger.info("Blocking Spot Check ran successfully!") + success, status = self._feedback_error_check(resp) + + return success, status + + except Exception as e: + self._logger.error("Exception thrown: {}".format(e)) + return False, str(e) + + def get_feedback(self) -> spot_check_pb2.SpotCheckFeedbackResponse: + """Get feedback from Spot Check""" + resp = self._req_feedback() + return resp[0], "Got only feedback from Spot Check" diff --git a/spot_wrapper/spot_config.py b/spot_wrapper/spot_config.py new file mode 100644 index 00000000..0aa325b4 --- /dev/null +++ b/spot_wrapper/spot_config.py @@ -0,0 +1,66 @@ +""" +Global variables used for configuration. These will not change at runtime. +""" +from collections import namedtuple + +SPOT_CLIENT_NAME = "ros_spot" +MAX_COMMAND_DURATION = 1e5 + +"""List of image sources for front image periodic query""" +front_image_sources = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "frontleft_depth", + "frontright_depth", +] + +"""List of image sources for side image periodic query""" +side_image_sources = [ + "left_fisheye_image", + "right_fisheye_image", + "left_depth", + "right_depth", +] + +"""List of image sources for rear image periodic query""" +rear_image_sources = ["back_fisheye_image", "back_depth"] + +"""Service name for getting pointcloud of VLP16 connected to Spot Core""" +VELODYNE_SERVICE_NAME = "velodyne-point-cloud" + +"""List of point cloud sources""" +point_cloud_sources = ["velodyne-point-cloud"] + +"""List of image sources for hand image periodic query""" +hand_image_sources = [ + "hand_image", + "hand_depth", + "hand_color_image", + "hand_depth_in_hand_color_frame", +] + +# TODO: Missing Hand images +CAMERA_IMAGE_SOURCES = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "left_fisheye_image", + "right_fisheye_image", + "back_fisheye_image", +] +DEPTH_IMAGE_SOURCES = [ + "frontleft_depth", + "frontright_depth", + "left_depth", + "right_depth", + "back_depth", +] +DEPTH_REGISTERED_IMAGE_SOURCES = [ + "frontleft_depth_in_visual_frame", + "frontright_depth_in_visual_frame", + "right_depth_in_visual_frame", + "left_depth_in_visual_frame", + "back_depth_in_visual_frame", +] +ImageBundle = namedtuple( + "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] +) diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py new file mode 100644 index 00000000..d1cd14d3 --- /dev/null +++ b/spot_wrapper/spot_docking.py @@ -0,0 +1,60 @@ +import typing +import logging + +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command +from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock +from bosdyn.api.docking import docking_pb2 + + +class SpotDocking: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._docking_client: DockingClient = robot_clients["docking_client"] + self._robot_command_client: robot_command.RobotCommandClient = robot_clients[ + "robot_command_client" + ] + self._robot_params = robot_params + + def dock(self, dock_id: int) -> typing.Tuple[bool, str]: + """Dock the robot to the docking station with fiducial ID [dock_id].""" + try: + # Make sure we're powered on and standing + self._robot.power_on() + if not self._robot_params["is_standing"]: + robot_command.blocking_stand( + command_client=self._robot_command_client, timeout_sec=10 + ) + self._logger.info("Spot is standing") + else: + self._logger.info("Spot is already standing") + # Dock the robot + self.last_docking_command = dock_id + blocking_dock_robot(self._robot, dock_id) + self.last_docking_command = None + return True, "Success" + except Exception as e: + return False, str(e) + + def undock(self, timeout: int = 20) -> typing.Tuple[bool, str]: + """Power motors on and undock the robot from the station.""" + try: + # Maker sure we're powered on + self._robot.power_on() + # Undock the robot + blocking_undock(self._robot, timeout) + return True, "Success" + except Exception as e: + return False, str(e) + + def get_docking_state(self, **kwargs) -> docking_pb2.DockState: + """Get docking state of robot.""" + state = self._docking_client.get_docking_state(**kwargs) + return state diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py new file mode 100644 index 00000000..b3347a75 --- /dev/null +++ b/spot_wrapper/spot_eap.py @@ -0,0 +1,73 @@ +import typing +import logging + +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.point_cloud import PointCloudClient, build_pc_request +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command + + +from .spot_config import * + + +class AsyncPointCloudService(AsyncPeriodicQuery): + """ + Class to get point cloud at regular intervals. get_point_cloud_from_sources_async query sent to the robot at + every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback, point_cloud_requests): + super(AsyncPointCloudService, self).__init__( + "robot_point_cloud_service", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + self._point_cloud_requests = point_cloud_requests + + def _start_query(self): + if self._callback and self._point_cloud_requests: + callback_future = self._client.get_point_cloud_async( + self._point_cloud_requests + ) + callback_future.add_done_callback(self._callback) + return callback_future + + +class SpotEAP: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._robot_params = robot_params + self._rates: typing.Dict[str, float] = robot_params["rates"] + self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + self._point_cloud_client: PointCloudClient = robot_clients["point_cloud_client"] + + self._point_cloud_requests = [] + for source in point_cloud_sources: + self._point_cloud_requests.append(build_pc_request(source)) + + self._point_cloud_task = AsyncPointCloudService( + self._point_cloud_client, + self._logger, + max(0.0, self._rates.get("point_cloud", 0.0)), + self._callbacks.get("lidar_points", None), + self._point_cloud_requests, + ) + + @property + def async_task(self) -> AsyncPeriodicQuery: + """Returns the async PointCloudService task for the robot""" + return self._point_cloud_task diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py new file mode 100644 index 00000000..305dfa33 --- /dev/null +++ b/spot_wrapper/spot_graph_nav.py @@ -0,0 +1,796 @@ +import typing +import logging +import math +import time +import os + +from bosdyn.client.robot import Robot +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client.lease import LeaseClient, LeaseWallet, LeaseKeepAlive +from bosdyn.client.graph_nav import GraphNavClient +from bosdyn.client.map_processing import MapProcessingServiceClient +from bosdyn.client.frame_helpers import get_odom_tform_body +from bosdyn.api.graph_nav import graph_nav_pb2 +from bosdyn.api.graph_nav import map_pb2 +from bosdyn.api.graph_nav import nav_pb2 +from bosdyn.api.graph_nav import map_processing_pb2 + +from google.protobuf import wrappers_pb2 + + +class SpotGraphNav: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._graph_nav_client: GraphNavClient = robot_clients["graph_nav_client"] + self._map_processing_client: MapProcessingServiceClient = robot_clients[ + "map_processing_client" + ] + self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] + self._lease_client: LeaseClient = robot_clients["lease_client"] + self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet + self._robot_params = robot_params + + # Store the most recent knowledge of the state of the robot based on rpc calls. + self._current_graph = None + self._current_edges = dict() # maps to_waypoint to list(from_waypoint) + self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot + self._current_edge_snapshots = dict() # maps id to edge snapshot + self._current_annotation_name_to_wp_id = dict() + + def list_graph(self) -> typing.List[str]: + """List waypoint ids of graph_nav + Args: + upload_path : Path to the root directory of the map. + """ + ids, eds = self._list_graph_waypoint_and_edge_ids() + + return [ + v + for k, v in sorted( + ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) + ) + ] + + def navigate_initial_localization( + self, + upload_path: str, + initial_localization_fiducial: bool = True, + initial_localization_waypoint: typing.Optional[str] = None, + ): + """Navigate with graphnav. + + Args: + upload_path : Path to the root directory of the map. + navigate_to : Waypont id string for where to goal + initial_localization_fiducial : Tells the initializer whether to use fiducials + initial_localization_waypoint : Waypoint id string of current robot position (optional) + """ + # Filepath for uploading a saved graph's and snapshots too. + if upload_path and upload_path[-1] == "/": + upload_filepath = upload_path[:-1] + else: + upload_filepath = upload_path + + # Boolean indicating the robot's power state. + power_state = self._robot_state_client.get_robot_state().power_state + self._started_powered_on = power_state.motor_power_state == power_state.STATE_ON + self._powered_on = self._started_powered_on + + # Claim lease, power on robot, start graphnav. + self._lease = self._lease_wallet.get_lease() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + if upload_filepath: + self._clear_graph() + self._upload_graph_and_snapshots(upload_filepath) + else: + self._download_current_graph() + self._logger.info( + "Re-using existing graph on robot. Check that the correct graph is loaded!" + ) + if initial_localization_fiducial: + self._set_initial_localization_fiducial() + if initial_localization_waypoint: + self._set_initial_localization_waypoint([initial_localization_waypoint]) + self._list_graph_waypoint_and_edge_ids() + self._get_localization_state() + + return True, "Localization done!" + + def navigate_to_existing_waypoint(self, waypoint_id: str): + """Navigate to an existing waypoint. + Args: + waypoint_id : Waypoint id string for where to go + """ + self._get_localization_state() + resp = self._navigate_to(waypoint_id) + return resp + + def navigate_through_route(self, waypoint_ids: typing.List[str]): + """ + Args: + waypoint_ids: List[str] of waypoints to follow + """ + self._get_localization_state() + self._logger.info(f"Waypoint ids: {','.join(waypoint_ids)}") + resp = self._navigate_route(waypoint_ids) + return resp + + def download_navigation_graph(self, download_path: str) -> typing.List[str]: + """Download the navigation graph. + Args: + download_path : Path to the root directory of the map. + """ + self._download_filepath = download_path + self._download_full_graph() + return self.list_graph() + + def navigation_close_loops( + self, close_fiducial_loops: bool, close_odometry_loops: bool + ) -> typing.Tuple[bool, str]: + return self._auto_close_loops(close_fiducial_loops, close_odometry_loops) + + def optmize_anchoring(self) -> typing.Tuple[bool, str]: + return self._optimize_anchoring() + + ## Copied from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py + # Copyright (c) 2020 Boston Dynamics, Inc. All rights reserved. + # + # Downloading, reproducing, distributing or otherwise using the SDK Software + # is subject to the terms and conditions of the Boston Dynamics Software + # Development Kit License (20191101-BDSDK-SL). + def _get_localization_state(self, *args): + """Get the current localization and state of the robot.""" + state = self._graph_nav_client.get_localization_state() + self._logger.info(f"Got localization: \n{str(state.localization)}") + odom_tform_body = get_odom_tform_body( + state.robot_kinematics.transforms_snapshot + ) + self._logger.info( + f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}" + ) + + def _set_initial_localization_fiducial(self, *args): + """Trigger localization when near a fiducial.""" + robot_state = self._robot_state_client.get_robot_state() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() + # Create an empty instance for initial localization since we are asking it to localize + # based on the nearest fiducial. + localization = nav_pb2.Localization() + self._graph_nav_client.set_localization( + initial_guess_localization=localization, + ko_tform_body=current_odom_tform_body, + ) + + def _set_initial_localization_waypoint(self, *args): + """Trigger localization to a waypoint.""" + # Take the first argument as the localization waypoint. + if len(args) < 1: + # If no waypoint id is given as input, then return without initializing. + self._logger.error("No waypoint specified to initialize to.") + return + destination_waypoint = self._find_unique_waypoint_id( + args[0][0], + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not destination_waypoint: + self._logger.error("Failed to find waypoint id.") + return + + robot_state = self._robot_state_client.get_robot_state() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() + # Create an initial localization to the specified waypoint as the identity. + localization = nav_pb2.Localization() + localization.waypoint_id = destination_waypoint + localization.waypoint_tform_body.rotation.w = 1.0 + self._graph_nav_client.set_localization( + initial_guess_localization=localization, + # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). + max_distance=0.2, + max_yaw=20.0 * math.pi / 180.0, + fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NO_FIDUCIAL, + ko_tform_body=current_odom_tform_body, + ) + + def _download_current_graph(self): + graph = self._graph_nav_client.download_graph() + if graph is None: + self._logger.error("Empty graph.") + return + self._current_graph = graph + return graph + + def _download_full_graph(self, *args): + """Download the graph and snapshots from the robot.""" + graph = self._graph_nav_client.download_graph() + if graph is None: + self._logger.info("Failed to download the graph.") + return + self._write_full_graph(graph) + self._logger.info( + "Graph downloaded with {} waypoints and {} edges".format( + len(graph.waypoints), len(graph.edges) + ) + ) + # Download the waypoint and edge snapshots. + self._download_and_write_waypoint_snapshots(graph.waypoints) + self._download_and_write_edge_snapshots(graph.edges) + + def _write_full_graph(self, graph): + """Download the graph from robot to the specified, local filepath location.""" + graph_bytes = graph.SerializeToString() + self._write_bytes(self._download_filepath, "/graph", graph_bytes) + + def _download_and_write_waypoint_snapshots(self, waypoints): + """Download the waypoint snapshots from robot to the specified, local filepath location.""" + num_waypoint_snapshots_downloaded = 0 + for waypoint in waypoints: + if len(waypoint.snapshot_id) == 0: + continue + try: + waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot( + waypoint.snapshot_id + ) + except Exception: + # Failure in downloading waypoint snapshot. Continue to next snapshot. + self._logger.error( + "Failed to download waypoint snapshot: " + waypoint.snapshot_id + ) + continue + self._write_bytes( + self._download_filepath + "/waypoint_snapshots", + "/" + waypoint.snapshot_id, + waypoint_snapshot.SerializeToString(), + ) + num_waypoint_snapshots_downloaded += 1 + self._logger.info( + "Downloaded {} of the total {} waypoint snapshots.".format( + num_waypoint_snapshots_downloaded, len(waypoints) + ) + ) + + def _download_and_write_edge_snapshots(self, edges): + """Download the edge snapshots from robot to the specified, local filepath location.""" + num_edge_snapshots_downloaded = 0 + num_to_download = 0 + for edge in edges: + if len(edge.snapshot_id) == 0: + continue + num_to_download += 1 + try: + edge_snapshot = self._graph_nav_client.download_edge_snapshot( + edge.snapshot_id + ) + except Exception: + # Failure in downloading edge snapshot. Continue to next snapshot. + self._logger.error( + "Failed to download edge snapshot: " + edge.snapshot_id + ) + continue + self._write_bytes( + self._download_filepath + "/edge_snapshots", + "/" + edge.snapshot_id, + edge_snapshot.SerializeToString(), + ) + num_edge_snapshots_downloaded += 1 + self._logger.info( + "Downloaded {} of the total {} edge snapshots.".format( + num_edge_snapshots_downloaded, num_to_download + ) + ) + + def _write_bytes(self, filepath: str, filename: str, data): + """Write data to a file.""" + os.makedirs(filepath, exist_ok=True) + with open(filepath + filename, "wb+") as f: + f.write(data) + f.close() + + def _list_graph_waypoint_and_edge_ids(self, *args): + """List the waypoint ids and edge ids of the graph currently on the robot.""" + + # Download current graph + graph = self._download_current_graph() + + localization_id = ( + self._graph_nav_client.get_localization_state().localization.waypoint_id + ) + + # Update and print waypoints and edges + ( + self._current_annotation_name_to_wp_id, + self._current_edges, + ) = self._update_waypoints_and_edges(graph, localization_id, self._logger) + return self._current_annotation_name_to_wp_id, self._current_edges + + def _upload_graph_and_snapshots(self, upload_filepath: str): + """Upload the graph and snapshots to the robot.""" + self._logger.info("Loading the graph from disk into local storage...") + with open(upload_filepath + "/graph", "rb") as graph_file: + # Load the graph from disk. + data = graph_file.read() + self._current_graph = map_pb2.Graph() + self._current_graph.ParseFromString(data) + self._logger.info( + "Loaded graph has {} waypoints and {} edges".format( + len(self._current_graph.waypoints), len(self._current_graph.edges) + ) + ) + for waypoint in self._current_graph.waypoints: + # Load the waypoint snapshots from disk. + with open( + upload_filepath + "/waypoint_snapshots/{}".format(waypoint.snapshot_id), + "rb", + ) as snapshot_file: + waypoint_snapshot = map_pb2.WaypointSnapshot() + waypoint_snapshot.ParseFromString(snapshot_file.read()) + self._current_waypoint_snapshots[ + waypoint_snapshot.id + ] = waypoint_snapshot + for edge in self._current_graph.edges: + # Load the edge snapshots from disk. + self._logger.info(f"Trying edge: {edge.snapshot_id}") + if not edge.snapshot_id: + continue + with open( + upload_filepath + "/edge_snapshots/{}".format(edge.snapshot_id), "rb" + ) as snapshot_file: + edge_snapshot = map_pb2.EdgeSnapshot() + edge_snapshot.ParseFromString(snapshot_file.read()) + self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot + # Upload the graph to the robot. + self._logger.info("Uploading the graph and snapshots to the robot...") + self._graph_nav_client.upload_graph( + lease=self._lease.lease_proto, graph=self._current_graph + ) + # Upload the snapshots to the robot. + for waypoint_snapshot in self._current_waypoint_snapshots.values(): + self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) + self._logger.info("Uploaded {}".format(waypoint_snapshot.id)) + for edge_snapshot in self._current_edge_snapshots.values(): + self._graph_nav_client.upload_edge_snapshot(edge_snapshot) + self._logger.info("Uploaded {}".format(edge_snapshot.id)) + + # The upload is complete! Check that the robot is localized to the graph, + # and it if is not, prompt the user to localize the robot before attempting + # any navigation commands. + localization_state = self._graph_nav_client.get_localization_state() + if not localization_state.localization.waypoint_id: + # The robot is not localized to the newly uploaded graph. + self._logger.info( + "Upload complete! The robot is currently not localized to the map; please localize the robot using a fiducial before attempting a navigation command." + ) + + def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: + """Navigate to a specific waypoint.""" + self._lease = self._lease_wallet.get_lease() + destination_waypoint = self._find_unique_waypoint_id( + waypoint_id, + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not destination_waypoint: + self._logger.error( + "Failed to find the appropriate unique waypoint id for the navigation command." + ) + return ( + False, + "Failed to find the appropriate unique waypoint id for the navigation command.", + ) + + # Stop the lease keepalive and create a new sublease for graphnav. + self._lease = self._lease_wallet.advance() + sublease = self._lease.create_sublease() + self._lease_keepalive.shutdown() + + # Navigate to the destination waypoint. + is_finished = False + nav_to_cmd_id = -1 + while not is_finished: + # Issue the navigation command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + nav_to_cmd_id = self._graph_nav_client.navigate_to( + destination_waypoint, 1.0, leases=[sublease.lease_proto] + ) + time.sleep(0.5) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the navigation command is complete. + is_finished = self._check_success(nav_to_cmd_id) + + self._lease = self._lease_wallet.advance() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + + status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): + return True, "Successfully completed the navigation commands!" + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: + return ( + False, + "Robot got lost when navigating the route, the robot will now sit down.", + ) + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: + return ( + False, + "Robot got stuck when navigating the route, the robot will now sit down.", + ) + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): + return False, "Robot is impaired." + else: + return False, "Navigation command is not complete yet." + + def _navigate_route( + self, waypoint_ids: typing.List[str] + ) -> typing.Tuple[bool, str]: + """Navigate through a specific route of waypoints. + Note that each waypoint must have an edge between them, aka be adjacent. + """ + for i in range(len(waypoint_ids)): + waypoint_ids[i] = self._find_unique_waypoint_id( + waypoint_ids[i], + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not waypoint_ids[i]: + self._logger.error( + "navigate_route: Failed to find the unique waypoint id." + ) + return False, "Failed to find the unique waypoint id." + + edge_ids_list = [] + # Attempt to find edges in the current graph that match the ordered waypoint pairs. + # These are necessary to create a valid route. + for i in range(len(waypoint_ids) - 1): + start_wp = waypoint_ids[i] + end_wp = waypoint_ids[i + 1] + edge_id = self._match_edge(self._current_edges, start_wp, end_wp) + if edge_id is not None: + edge_ids_list.append(edge_id) + else: + self._logger.error( + f"Failed to find an edge between waypoints: {start_wp} and {end_wp}" + ) + return ( + False, + f"Failed to find an edge between waypoints: {start_wp} and {end_wp}", + ) + + # Stop the lease keepalive and create a new sublease for graphnav. + self._lease = self._lease_wallet.advance() + sublease = self._lease.create_sublease() + self._lease_keepalive.shutdown() + + # Navigate a specific route. + route = self._graph_nav_client.build_route(waypoint_ids, edge_ids_list) + is_finished = False + while not is_finished: + # Issue the route command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + nav_route_command_id = self._graph_nav_client.navigate_route( + route, cmd_duration=1.0, leases=[sublease.lease_proto] + ) + time.sleep(0.5) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the route is complete. + is_finished = self._check_success(nav_route_command_id) + + self._lease = self._lease_wallet.advance() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + + return True, "Finished navigating route!" + + def _clear_graph(self, *args) -> bool: + """Clear the state of the map on the robot, removing all waypoints and edges.""" + return self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) + + def _check_success(self, command_id: int = -1) -> bool: + """Use a navigation command id to get feedback from the robot and sit when command succeeds.""" + if command_id == -1: + # No command, so we have not status to check. + return False + status = self._graph_nav_client.navigation_feedback(command_id) + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): + # Successfully completed the navigation commands! + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: + self._logger.error( + "Robot got lost when navigating the route, the robot will now sit down." + ) + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: + self._logger.error( + "Robot got stuck when navigating the route, the robot will now sit down." + ) + return True + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): + self._logger.error("Robot is impaired.") + return True + else: + # Navigation command is not complete yet. + return False + + def _match_edge( + self, + current_edges: typing.Dict[str, typing.List[str]], + waypoint1: str, + waypoint2: str, + ) -> typing.Optional[map_pb2.Edge.Id]: + """Find an edge in the graph that is between two waypoint ids.""" + # Return the correct edge id as soon as it's found. + for edge_to_id in current_edges: + for edge_from_id in current_edges[edge_to_id]: + if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): + # This edge matches the pair of waypoints! Add it the edge list and continue. + return map_pb2.Edge.Id( + from_waypoint=waypoint2, to_waypoint=waypoint1 + ) + elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): + # This edge matches the pair of waypoints! Add it the edge list and continue. + return map_pb2.Edge.Id( + from_waypoint=waypoint1, to_waypoint=waypoint2 + ) + return None + + def _auto_close_loops( + self, close_fiducial_loops: bool, close_odometry_loops: bool, *args + ): + """Automatically find and close all loops in the graph.""" + response: map_processing_pb2.ProcessTopologyResponse = ( + self._map_processing_client.process_topology( + params=map_processing_pb2.ProcessTopologyRequest.Params( + do_fiducial_loop_closure=wrappers_pb2.BoolValue( + value=close_fiducial_loops + ), + do_odometry_loop_closure=wrappers_pb2.BoolValue( + value=close_odometry_loops + ), + ), + modify_map_on_server=True, + ) + ) + self._logger.info( + "Created {} new edge(s).".format(len(response.new_subgraph.edges)) + ) + if response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_OK: + return True, "Successfully closed loops." + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS + ): + return False, "Missing waypoint snapshots." + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH + ): + return False, "Invalid graph." + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING + ): + return False, "Map modified during processing." + else: + return False, "Unknown error during map processing." + + def _optimize_anchoring(self, *args): + """Call anchoring optimization on the server, producing a globally optimal reference frame for waypoints to be expressed in.""" + response: map_processing_pb2.ProcessAnchoringResponse = ( + self._map_processing_client.process_anchoring( + params=map_processing_pb2.ProcessAnchoringRequest.Params(), + modify_anchoring_on_server=True, + stream_intermediate_results=False, + ) + ) + if response.status == map_processing_pb2.ProcessAnchoringResponse.STATUS_OK: + self._logger.info( + "Optimized anchoring after {} iteration(s).".format(response.iteration) + ) + return True, "Successfully optimized anchoring." + else: + self._logger.error("Error optimizing {}".format(response)) + if ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS + ): + return False, "Missing waypoint snapshots." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAPH + ): + return False, "Invalid graph." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_OPTIMIZATION_FAILURE + ): + return False, "Optimization failure." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_PARAMS + ): + return False, "Invalid parameters." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_CONSTRAINT_VIOLATION + ): + return False, "Constraint violation." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_ITERATIONS + ): + return False, "Max iterations, timeout." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_TIME + ): + return False, "Max time reached, timeout." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_HINTS + ): + return False, "Invalid hints." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING + ): + return False, "Map modified during processing." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT + ): + return False, "Invalid gravity alignment." + else: + return False, "Unknown error during anchoring optimization." + + def _id_to_short_code(self, id: str): + """Convert a unique id to a 2 letter short code.""" + tokens = id.split("-") + if len(tokens) > 2: + return "%c%c" % (tokens[0][0], tokens[1][0]) + return None + + def _pretty_print_waypoints( + self, + waypoint_id: str, + waypoint_name: str, + short_code_to_count: typing.Dict[str, int], + localization_id: str, + logger: logging.Logger, + ): + short_code = self._id_to_short_code(waypoint_id) + if short_code is None or short_code_to_count[short_code] != 1: + # If the short code is not valid/unique, don't show it. + short_code = " " + + logger.info( + "%s Waypoint name: %s id: %s short code: %s" + % ( + "->" if localization_id == waypoint_id else " ", + waypoint_name, + waypoint_id, + short_code, + ) + ) + + def _find_unique_waypoint_id( + self, + short_code: str, + graph: map_pb2.Graph, + name_to_id: typing.Dict[str, str], + logger: logging.Logger, + ): + """Convert either a 2 letter short code or an annotation name into the associated unique id.""" + if len(short_code) != 2: + # Not a short code, check if it is an annotation name (instead of the waypoint id). + if short_code in name_to_id: + # Short code is an waypoint's annotation name. Check if it is paired with a unique waypoint id. + if name_to_id[short_code] is not None: + # Has an associated waypoint id! + return name_to_id[short_code] + else: + logger.error( + "The waypoint name %s is used for multiple different unique waypoints. Please use" + + "the waypoint id." % (short_code) + ) + return None + # Also not an waypoint annotation name, so we will operate under the assumption that it is a + # unique waypoint id. + return short_code + + ret = short_code + for waypoint in graph.waypoints: + if short_code == self._id_to_short_code(waypoint.id): + if ret != short_code: + return short_code # Multiple waypoints with same short code. + ret = waypoint.id + return ret + + def _update_waypoints_and_edges( + self, graph: map_pb2.Graph, localization_id: str, logger: logging.Logger + ) -> typing.Tuple[typing.Dict[str, str], typing.Dict[str, str]]: + """Update and print waypoint ids and edge ids.""" + name_to_id = dict() + edges = dict() + + short_code_to_count = {} + waypoint_to_timestamp = [] + for waypoint in graph.waypoints: + # Determine the timestamp that this waypoint was created at. + timestamp = -1.0 + try: + timestamp = ( + waypoint.annotations.creation_time.seconds + + waypoint.annotations.creation_time.nanos / 1e9 + ) + except: + # Must be operating on an older graph nav map, since the creation_time is not + # available within the waypoint annotations message. + pass + waypoint_to_timestamp.append( + (waypoint.id, timestamp, waypoint.annotations.name) + ) + + # Determine how many waypoints have the same short code. + short_code = self._id_to_short_code(waypoint.id) + if short_code not in short_code_to_count: + short_code_to_count[short_code] = 0 + short_code_to_count[short_code] += 1 + + # Add the annotation name/id into the current dictionary. + waypoint_name = waypoint.annotations.name + if waypoint_name: + if waypoint_name in name_to_id: + # Waypoint name is used for multiple different waypoints, so set the waypoint id + # in this dictionary to None to avoid confusion between two different waypoints. + name_to_id[waypoint_name] = None + else: + # First time we have seen this waypoint annotation name. Add it into the dictionary + # with the respective waypoint unique id. + name_to_id[waypoint_name] = waypoint.id + + # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, + # fallback to sorting by annotation name. + waypoint_to_timestamp = sorted( + waypoint_to_timestamp, key=lambda x: (x[1], x[2]) + ) + + # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from + # when the waypoint was created. + logger.info("%d waypoints:" % len(graph.waypoints)) + for waypoint in waypoint_to_timestamp: + self._pretty_print_waypoints( + waypoint[0], waypoint[2], short_code_to_count, localization_id, logger + ) + + for edge in graph.edges: + if edge.id.to_waypoint in edges: + if edge.id.from_waypoint not in edges[edge.id.to_waypoint]: + edges[edge.id.to_waypoint].append(edge.id.from_waypoint) + else: + edges[edge.id.to_waypoint] = [edge.id.from_waypoint] + logger.info( + f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}" + ) + + return name_to_id, edges diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py new file mode 100644 index 00000000..d8444a6e --- /dev/null +++ b/spot_wrapper/spot_images.py @@ -0,0 +1,273 @@ +import typing +import logging + +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command +from bosdyn.client.image import ( + ImageClient, + build_image_request, + UnsupportedPixelFormatRequestedError, +) +from bosdyn.api import image_pb2 + +from .spot_config import * + + +class AsyncImageService(AsyncPeriodicQuery): + """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback, image_requests): + super(AsyncImageService, self).__init__( + "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + self._image_requests = image_requests + + def _start_query(self): + if self._callback: + callback_future = self._client.get_image_async(self._image_requests) + callback_future.add_done_callback(self._callback) + return callback_future + + +class SpotImages: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._robot_params = robot_params + self._rates: typing.Dict[str, float] = robot_params["rates"] + self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + self._image_client: ImageClient = robot_clients["image_client"] + + self._front_image_requests = [] + for source in front_image_sources: + self._front_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._side_image_requests = [] + for source in side_image_sources: + self._side_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._rear_image_requests = [] + for source in rear_image_sources: + self._rear_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._hand_image_requests = [] + for source in hand_image_sources: + self._hand_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._front_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("front_image", 0.0)), + self._callbacks.get("front_image", None), + self._front_image_requests, + ) + self._side_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("side_image", 0.0)), + self._callbacks.get("side_image", None), + self._side_image_requests, + ) + self._rear_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("rear_image", 0.0)), + self._callbacks.get("rear_image", None), + self._rear_image_requests, + ) + self._hand_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("hand_image", 0.0)), + self._callbacks.get("hand_image", None), + self._hand_image_requests, + ) + + self.camera_task_name_to_task_mapping: typing.Dict[str, AsyncImageService] = { + "hand_image": self._hand_image_task, + "side_image": self._side_image_task, + "rear_image": self._rear_image_task, + "front_image": self._front_image_task, + } + + ############################################ + # TODO: Sort out double publishing of images + self._camera_image_requests = [] + for camera_source in CAMERA_IMAGE_SOURCES: + self._camera_image_requests.append( + build_image_request( + camera_source, + image_format=image_pb2.Image.FORMAT_JPEG, + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ) + + self._depth_image_requests = [] + for camera_source in DEPTH_IMAGE_SOURCES: + self._depth_image_requests.append( + build_image_request( + camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + ) + ) + + self._depth_registered_image_requests = [] + for camera_source in DEPTH_REGISTERED_IMAGE_SOURCES: + self._depth_registered_image_requests.append( + build_image_request( + camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + ) + ) + + @property + def front_image_task(self) -> AsyncImageService: + return self._front_image_task + + @property + def side_image_task(self) -> AsyncImageService: + return self._side_image_task + + @property + def rear_image_task(self) -> AsyncImageService: + return self._rear_image_task + + @property + def hand_image_task(self) -> AsyncImageService: + return self._hand_image_task + + def update_image_tasks(self, image_name: str): + """Adds an async tasks to retrieve images from the specified image source""" + task_to_add = self.camera_task_name_to_task_mapping[image_name] + + if task_to_add == self._hand_image_task and not self._robot.has_arm(): + self._logger.warning( + "Robot has no arm, therefore the arm image task can not be added" + ) + return + + if task_to_add in self._async_tasks._tasks: + self._logger.warning( + f"Task {image_name} already in async task list, will not be added again" + ) + return + + self._async_tasks.add_task(self.camera_task_name_to_task_mapping[image_name]) + + def get_frontleft_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "frontleft_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_frontright_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "frontright_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_left_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "left_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_right_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "right_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_back_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "back_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_images( + self, image_requests: typing.List[image_pb2.ImageRequest] + ) -> typing.Optional[ImageBundle]: + try: + image_responses = self._image_client.get_image(image_requests) + except UnsupportedPixelFormatRequestedError as e: + return None + return ImageBundle( + frontleft=image_responses[0], + frontright=image_responses[1], + left=image_responses[2], + right=image_responses[3], + back=image_responses[4], + ) + + def get_camera_images(self) -> ImageBundle: + return self.get_images(self._camera_image_requests) + + def get_depth_images(self) -> ImageBundle: + return self.get_images(self._depth_image_requests) + + def get_depth_registered_images(self) -> ImageBundle: + return self.get_images(self._depth_registered_image_requests) diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py new file mode 100644 index 00000000..067801d2 --- /dev/null +++ b/spot_wrapper/spot_world_objects.py @@ -0,0 +1,68 @@ +import typing +import logging + +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command +from bosdyn.client.world_object import WorldObjectClient + +from .spot_config import * + + +class AsyncWorldObjects(AsyncPeriodicQuery): + """Class to get world objects. list_world_objects_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback): + super(AsyncWorldObjects, self).__init__( + "world-objects", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + + def _start_query(self): + if self._callback: + callback_future = self._client.list_world_objects_async() + callback_future.add_done_callback(self._callback) + return callback_future + + +class SpotWorldObjects: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._robot_params = robot_params + self._rates: typing.Dict[str, float] = robot_params["rates"] + self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + self._world_objects_client: WorldObjectClient = robot_clients[ + "world_objects_client" + ] + + self._world_objects_task = AsyncWorldObjects( + self._world_objects_client, + self._logger, + self._rates.get("world_objects", 10.0), + self._callbacks.get("world_objects", None), + ) + + @property + def async_task(self): + return self._world_objects_task + + def list_world_objects(self, object_types, time_start_point): + return self._world_objects_client.list_world_objects( + object_types, time_start_point + ) diff --git a/spot_wrapper/tests/test_graph_nav_util.py b/spot_wrapper/tests/test_graph_nav_util.py new file mode 100755 index 00000000..1dc78556 --- /dev/null +++ b/spot_wrapper/tests/test_graph_nav_util.py @@ -0,0 +1,222 @@ +#!/usr/bin/env python3 +PKG = "graph_nav_util" +NAME = "test_graph_nav_util" +SUITE = "test_graph_nav_util.TestSuiteGraphNavUtil" + +import unittest +import logging + +from bosdyn.api.graph_nav import map_pb2 + +from spot_wrapper.spot_graph_nav import SpotGraphNav + + +class MockSpotGraphNav(SpotGraphNav): + def __init__(self) -> None: + pass + + +# Create a mock SpotGraphNav object to access utility methods +graph_nav_util = MockSpotGraphNav() + + +class TestGraphNavUtilShortCode(unittest.TestCase): + def test_id_to_short_code(self): + self.assertEqual( + graph_nav_util._id_to_short_code("ebony-pug-mUzxLNq.TkGlVIxga+UKAQ=="), "ep" + ) + self.assertEqual( + graph_nav_util._id_to_short_code("erose-simian-sug9xpxhCxgft7Mtbhr98A=="), + "es", + ) + + +class TestGraphNavUtilFindUniqueWaypointId(unittest.TestCase): + def setUp(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + self.graph = map_pb2.Graph() + self.name_to_id = {"ABCDE": "Node1"} + + def test_short_code(self): + # Test normal short code + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ), + "AC", + "AC!=AC, normal short code", + ) + # Test annotation name that is known + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "ABCDE", self.graph, self.name_to_id, self.logger + ), + "Node1", + "ABCDE!=Node1, known annotation name", + ) + # Test annotation name that is unknown + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "ABCDEF", self.graph, self.name_to_id, self.logger + ), + "ABCDEF", + "ABCDEF!=ABCDEF, unknown annotation name", + ) + + def test_short_code_with_graph(self): + # Test short code that is in graph + self.graph.waypoints.add(id="AB-CD-EF") + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ), + "AB-CD-EF", + "AC!=AB-CD-EF, short code in graph", + ) + # Test short code that is not in graph + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "AD", self.graph, self.name_to_id, self.logger + ), + "AD", + "AD!=AD, short code not in graph", + ) + # Test multiple waypoints with the same short code + self.graph.waypoints.add(id="AB-CD-EF-1") + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ), + "AC", + "AC!=AC, multiple waypoints with same short code", + ) + + +class TestGraphNavUtilUpdateWaypointsEdges(unittest.TestCase): + def setUp(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + + def test_empty_graph(self): + # Test empty graph + self.graph = map_pb2.Graph() + self.localization_id = "" + graph_nav_util._update_waypoints_and_edges( + self.graph, self.localization_id, self.logger + ) + self.assertEqual( + len(self.graph.waypoints), 0, "Empty graph should have 0 waypoints" + ) + self.assertEqual(len(self.graph.edges), 0, "Empty graph should have 0 edges") + + def test_one_waypoint(self): + # Test graph with 1 waypoint + self.localization_id = "" + self.graph = map_pb2.Graph() + new_waypoint = map_pb2.Waypoint() + new_waypoint.id = "ABCDE" + new_waypoint.annotations.name = "Node1" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( + self.graph, self.localization_id, self.logger + ) + self.assertEqual( + len(self.graph.waypoints), 1, "Graph with 1 waypoint should have 1 waypoint" + ) + self.assertEqual( + len(self.graph.edges), 0, "Graph with 1 waypoint should have 0 edges" + ) + self.assertEqual(len(self.edges), 0, "Edges should have 0 entries") + self.assertEqual(len(self.name_to_id), 1, "Name to id should have 1 entry") + self.assertEqual( + self.name_to_id["Node1"], "ABCDE", "Name to id should have 1 entry" + ) + + def test_two_waypoints_with_edge(self): + # Test graph with 2 waypoints and an edge between them + self.localization_id = "" + self.graph = map_pb2.Graph() + new_waypoint = map_pb2.Waypoint() + new_waypoint.id = "ABCDE" + new_waypoint.annotations.name = "Node1" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + new_waypoint.id = "DE" + new_waypoint.annotations.name = "Node2" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + new_edge = map_pb2.Edge.Id(from_waypoint="ABCDE", to_waypoint="DE") + + self.graph.edges.add(id=new_edge) + self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( + self.graph, self.localization_id, self.logger + ) + self.assertEqual( + len(self.graph.waypoints), + 2, + "Graph with 2 waypoints should have 2 waypoints", + ) + self.assertEqual( + len(self.graph.edges), 1, "Graph with 2 waypoints should have 1 edge" + ) + self.assertEqual(len(self.edges), 1, "Edges should have 1 entry") + self.assertEqual( + self.edges["DE"][0], "ABCDE", "Edges should point to the correct waypoint" + ) + self.assertEqual(len(self.name_to_id), 2, "Name to id should have 2 entries") + self.assertEqual(self.name_to_id["Node1"], "ABCDE", "Name to id entry, ABCDE") + self.assertEqual(self.name_to_id["Node2"], "DE", "Name to id entry, DE") + + def test_two_waypoints_with_edge_and_localization(self): + # Test graph with 2 waypoints and an edge between them. Mainly affects the pretty print. + self.localization_id = "ABCDE" + self.graph = map_pb2.Graph() + new_waypoint = map_pb2.Waypoint() + new_waypoint.id = "ABCDE" + new_waypoint.annotations.name = "Node1" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + new_waypoint.id = "DE" + new_waypoint.annotations.name = "Node2" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + new_edge = map_pb2.Edge.Id(from_waypoint="ABCDE", to_waypoint="DE") + + self.graph.edges.add(id=new_edge) + self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( + self.graph, self.localization_id, self.logger + ) + self.assertEqual( + len(self.graph.waypoints), + 2, + "Graph with 2 waypoints should have 2 waypoints", + ) + self.assertEqual( + len(self.graph.edges), 1, "Graph with 2 waypoints should have 1 edge" + ) + self.assertEqual(len(self.edges), 1, "Edges should have 1 entry") + self.assertEqual( + self.edges["DE"][0], "ABCDE", "Edges should point to the correct waypoint" + ) + self.assertEqual(len(self.name_to_id), 2, "Name to id should have 2 entries") + self.assertEqual(self.name_to_id["Node1"], "ABCDE", "Name to id entry, ABCDE") + self.assertEqual(self.name_to_id["Node2"], "DE", "Name to id entry, DE") + + +class TestSuiteGraphNavUtil(unittest.TestSuite): + def __init__(self) -> None: + super(TestSuiteGraphNavUtil, self).__init__() + + self.loader = unittest.TestLoader() + self.addTest(self.loader.loadTestsFromTestCase(TestGraphNavUtilShortCode)) + self.addTest( + self.loader.loadTestsFromTestCase(TestGraphNavUtilFindUniqueWaypointId) + ) + self.addTest( + self.loader.loadTestsFromTestCase(TestGraphNavUtilUpdateWaypointsEdges) + ) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9b019101..ba85d8ca 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -4,7 +4,6 @@ import time import traceback import typing -from collections import namedtuple import bosdyn.client.auth from bosdyn.api import arm_command_pb2 @@ -15,6 +14,10 @@ from bosdyn.api import robot_state_pb2 from bosdyn.api import synchronized_command_pb2 from bosdyn.api import trajectory_pb2 +from bosdyn.api import world_object_pb2 +from bosdyn.api import lease_pb2 +from bosdyn.api import point_cloud_pb2 +from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 from bosdyn.api.graph_nav import graph_nav_pb2 from bosdyn.api.graph_nav import map_pb2 from bosdyn.api.graph_nav import nav_pb2 @@ -22,6 +25,7 @@ from bosdyn.client import frame_helpers from bosdyn.client import math_helpers from bosdyn.client import robot_command +from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock from bosdyn.client.estop import ( @@ -32,6 +36,7 @@ ) from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.client.graph_nav import GraphNavClient +from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.image import ( ImageClient, build_image_request, @@ -44,75 +49,22 @@ from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder from bosdyn.client.robot_state import RobotStateClient from bosdyn.client.world_object import WorldObjectClient +from bosdyn.client.spot_check import SpotCheckClient from bosdyn.geometry import EulerZXY from bosdyn.util import seconds_to_duration from google.protobuf.duration_pb2 import Duration -MAX_COMMAND_DURATION = 1e5 - -### Release -from . import graph_nav_util - -### Debug -# import graph_nav_util - from bosdyn.api import basic_command_pb2 from google.protobuf.timestamp_pb2 import Timestamp -front_image_sources = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "frontleft_depth", - "frontright_depth", -] -"""List of image sources for front image periodic query""" -side_image_sources = [ - "left_fisheye_image", - "right_fisheye_image", - "left_depth", - "right_depth", -] -"""List of image sources for side image periodic query""" -rear_image_sources = ["back_fisheye_image", "back_depth"] -"""List of image sources for rear image periodic query""" -VELODYNE_SERVICE_NAME = "velodyne-point-cloud" -"""Service name for getting pointcloud of VLP16 connected to Spot Core""" -point_cloud_sources = ["velodyne-point-cloud"] -"""List of point cloud sources""" -hand_image_sources = [ - "hand_image", - "hand_depth", - "hand_color_image", - "hand_depth_in_hand_color_frame", -] -"""List of image sources for hand image periodic query""" - - -# TODO: Missing Hand images -CAMERA_IMAGE_SOURCES = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "left_fisheye_image", - "right_fisheye_image", - "back_fisheye_image", -] -DEPTH_IMAGE_SOURCES = [ - "frontleft_depth", - "frontright_depth", - "left_depth", - "right_depth", - "back_depth", -] -DEPTH_REGISTERED_IMAGE_SOURCES = [ - "frontleft_depth_in_visual_frame", - "frontright_depth_in_visual_frame", - "right_depth_in_visual_frame", - "left_depth_in_visual_frame", - "back_depth_in_visual_frame", -] -ImageBundle = namedtuple( - "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] -) +from .spot_arm import SpotArm +from .spot_world_objects import SpotWorldObjects +from .spot_eap import SpotEAP +from .spot_docking import SpotDocking +from .spot_graph_nav import SpotGraphNav +from .spot_check import SpotCheck +from .spot_images import SpotImages +from .spot_config import * class AsyncRobotState(AsyncPeriodicQuery): @@ -216,36 +168,6 @@ def _start_query(self): return callback_future -class AsyncPointCloudService(AsyncPeriodicQuery): - """ - Class to get point cloud at regular intervals. get_point_cloud_from_sources_async query sent to the robot at - every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback, point_cloud_requests): - super(AsyncPointCloudService, self).__init__( - "robot_point_cloud_service", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - self._point_cloud_requests = point_cloud_requests - - def _start_query(self): - if self._callback and self._point_cloud_requests: - callback_future = self._client.get_point_cloud_async( - self._point_cloud_requests - ) - callback_future.add_done_callback(self._callback) - return callback_future - - class AsyncIdle(AsyncPeriodicQuery): """Class to check if the robot is moving, and if not, command a stand with the set mobility parameters @@ -418,31 +340,6 @@ def _start_query(self): pass -class AsyncWorldObjects(AsyncPeriodicQuery): - """Class to get world objects. list_world_objects_async query sent to the robot at every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback): - super(AsyncWorldObjects, self).__init__( - "world-objects", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - - def _start_query(self): - if self._callback: - callback_future = self._client.list_world_objects_async() - callback_future.add_done_callback(self._callback) - return callback_future - - def try_claim(func=None, *, power_on=False): """ Decorator which tries to acquire the lease before executing the wrapped function @@ -478,8 +375,6 @@ def wrapper_try_claim(self, *args, **kwargs): class SpotWrapper: """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically""" - SPOT_CLIENT_NAME = "ros_spot" - def __init__( self, username: str, @@ -489,8 +384,8 @@ def __init__( logger: logging.Logger, start_estop: bool = True, estop_timeout: float = 9.0, - rates: typing.Optional[typing.Dict] = None, - callbacks: typing.Optional[typing.Dict] = None, + rates: typing.Dict[str, float] = {}, + callbacks: typing.Dict[str, typing.Callable] = {}, use_take_lease: bool = False, get_lease_on_action: bool = False, continually_try_stand: bool = True, @@ -518,6 +413,8 @@ def __init__( self._password = password self._hostname = hostname self._robot_name = robot_name + self._rates = rates + self._callbacks = callbacks self._use_take_lease = use_take_lease self._get_lease_on_action = get_lease_on_action self._continually_try_stand = continually_try_stand @@ -525,14 +422,6 @@ def __init__( if robot_name is not None: self._frame_prefix = robot_name + "/" self._logger = logger - if rates is None: - self._rates = {} - else: - self._rates = rates - if callbacks is None: - self._callbacks = {} - else: - self._callbacks = callbacks self._estop_timeout = estop_timeout self._start_estop = start_estop self._keep_alive = True @@ -553,64 +442,18 @@ def __init__( self._last_trajectory_command_precise = None self._last_velocity_command_time = None self._last_docking_command = None - - self._front_image_requests = [] - for source in front_image_sources: - self._front_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._side_image_requests = [] - for source in side_image_sources: - self._side_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._rear_image_requests = [] - for source in rear_image_sources: - self._rear_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._point_cloud_requests = [] - for source in point_cloud_sources: - self._point_cloud_requests.append(build_pc_request(source)) - - self._hand_image_requests = [] - for source in hand_image_sources: - self._hand_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._camera_image_requests = [] - for camera_source in CAMERA_IMAGE_SOURCES: - self._camera_image_requests.append( - build_image_request( - camera_source, - image_format=image_pb2.Image.FORMAT_JPEG, - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ) - - self._depth_image_requests = [] - for camera_source in DEPTH_IMAGE_SOURCES: - self._depth_image_requests.append( - build_image_request( - camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 - ) - ) - - self._depth_registered_image_requests = [] - for camera_source in DEPTH_REGISTERED_IMAGE_SOURCES: - self._depth_registered_image_requests.append( - build_image_request( - camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 - ) - ) + self._robot_params = { + "is_standing": False, + "is_sitting": True, + "is_moving": False, + "robot_id": None, + "estop_timeout": self._estop_timeout, + "rates": self._rates, + "callbacks": self._callbacks, + } try: - self._sdk = create_standard_sdk(self.SPOT_CLIENT_NAME) + self._sdk = create_standard_sdk(SPOT_CLIENT_NAME) except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False @@ -648,255 +491,303 @@ def __init__( self._point_cloud_client = None self._logger.warning("No point cloud services are available.") - if self._robot: - # Clients - self._logger.info("Creating clients...") - initialised = False - while not initialised: - try: - self._robot_state_client = self._robot.ensure_client( - RobotStateClient.default_service_name - ) - self._world_objects_client = self._robot.ensure_client( - WorldObjectClient.default_service_name - ) - self._robot_command_client = self._robot.ensure_client( - RobotCommandClient.default_service_name - ) - self._graph_nav_client = self._robot.ensure_client( - GraphNavClient.default_service_name - ) - self._power_client = self._robot.ensure_client( - PowerClient.default_service_name - ) - self._lease_client = self._robot.ensure_client( - LeaseClient.default_service_name - ) - self._lease_wallet = self._lease_client.lease_wallet - self._image_client = self._robot.ensure_client( - ImageClient.default_service_name - ) - self._estop_client = self._robot.ensure_client( - EstopClient.default_service_name - ) - self._docking_client = self._robot.ensure_client( - DockingClient.default_service_name + if not self._robot: + self._logger.error("Failed to create robot object") + self._valid = False + return + + # Clients + self._logger.info("Creating clients...") + initialised = False + while not initialised: + try: + self._robot_state_client = self._robot.ensure_client( + RobotStateClient.default_service_name + ) + self._world_objects_client = self._robot.ensure_client( + WorldObjectClient.default_service_name + ) + self._robot_command_client = self._robot.ensure_client( + RobotCommandClient.default_service_name + ) + self._graph_nav_client = self._robot.ensure_client( + GraphNavClient.default_service_name + ) + self._map_processing_client = self._robot.ensure_client( + MapProcessingServiceClient.default_service_name + ) + self._power_client = self._robot.ensure_client( + PowerClient.default_service_name + ) + self._lease_client = self._robot.ensure_client( + LeaseClient.default_service_name + ) + self._lease_wallet = self._lease_client.lease_wallet + self._image_client = self._robot.ensure_client( + ImageClient.default_service_name + ) + self._estop_client = self._robot.ensure_client( + EstopClient.default_service_name + ) + self._docking_client = self._robot.ensure_client( + DockingClient.default_service_name + ) + self._spot_check_client = self._robot.ensure_client( + SpotCheckClient.default_service_name + ) + if self._robot.has_arm(): + self._manipulation_client = self._robot.ensure_client( + ManipulationApiClient.default_service_name ) - if self._robot.has_arm(): - self._manipulation_client = self._robot.ensure_client( - ManipulationApiClient.default_service_name - ) - initialised = True - except Exception as e: - sleep_secs = 15 - self._logger.warning( - "Unable to create client service: {}. This usually means the robot hasn't " - "finished booting yet. Will wait {} seconds and try again.".format( - e, sleep_secs - ) + + self._robot_clients = { + "robot_state_client": self._robot_state_client, + "robot_command_client": self._robot_command_client, + "graph_nav_client": self._graph_nav_client, + "map_processing_client": self._map_processing_client, + "power_client": self._power_client, + "lease_client": self._lease_client, + "image_client": self._image_client, + "estop_client": self._estop_client, + "docking_client": self._docking_client, + "spot_check_client": self._spot_check_client, + "robot_command_method": self._robot_command, + "world_objects_client": self._world_objects_client, + } + if self._point_cloud_client: + self._robot_clients["point_cloud_client"] = self._point_cloud_client + + initialised = True + except Exception as e: + sleep_secs = 15 + self._logger.warning( + "Unable to create client service: {}. This usually means the robot hasn't " + "finished booting yet. Will wait {} seconds and try again.".format( + e, sleep_secs ) - time.sleep(sleep_secs) - - # Store the most recent knowledge of the state of the robot based on rpc calls. - self._current_graph = None - self._current_edges = dict() # maps to_waypoint to list(from_waypoint) - self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot - self._current_edge_snapshots = dict() # maps id to edge snapshot - self._current_annotation_name_to_wp_id = dict() - - # Async Tasks - self._async_task_list = [] - self._robot_state_task = AsyncRobotState( - self._robot_state_client, - self._logger, - max(0.0, self._rates.get("robot_state", 0.0)), - self._callbacks.get("robot_state", None), - ) - self._robot_metrics_task = AsyncMetrics( - self._robot_state_client, - self._logger, - max(0.0, self._rates.get("metrics", 0.0)), - self._callbacks.get("metrics", None), - ) - self._lease_task = AsyncLease( - self._lease_client, - self._logger, - max(0.0, self._rates.get("lease", 0.0)), - self._callbacks.get("lease", None), - ) - self._front_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("front_image", 0.0)), - self._callbacks.get("front_image", None), - self._front_image_requests, - ) - self._side_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("side_image", 0.0)), - self._callbacks.get("side_image", None), - self._side_image_requests, - ) - self._rear_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("rear_image", 0.0)), - self._callbacks.get("rear_image", None), - self._rear_image_requests, - ) - self._hand_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("hand_image", 0.0)), - self._callbacks.get("hand_image", None), - self._hand_image_requests, - ) + ) + time.sleep(sleep_secs) - self._idle_task = AsyncIdle( - self._robot_command_client, self._logger, 10.0, self - ) - self._estop_monitor = AsyncEStopMonitor( - self._estop_client, self._logger, 20.0, self - ) - self._world_objects_task = AsyncWorldObjects( - self._world_objects_client, - self._logger, - 10.0, - self._callbacks.get("world_objects", None), - ) + # Core Async Tasks + self._async_task_list = [] + self._robot_state_task = AsyncRobotState( + self._robot_state_client, + self._logger, + max(0.0, self._rates.get("robot_state", 0.0)), + self._callbacks.get("robot_state", None), + ) + self._robot_metrics_task = AsyncMetrics( + self._robot_state_client, + self._logger, + max(0.0, self._rates.get("metrics", 0.0)), + self._callbacks.get("metrics", None), + ) + self._lease_task = AsyncLease( + self._lease_client, + self._logger, + max(0.0, self._rates.get("lease", 0.0)), + self._callbacks.get("lease", None), + ) + self._idle_task = AsyncIdle( + self._robot_command_client, self._logger, 10.0, self + ) + self._estop_monitor = AsyncEStopMonitor( + self._estop_client, self._logger, 20.0, self + ) - self._estop_endpoint = None - self._estop_keepalive = None + self._estop_endpoint = None + self._estop_keepalive = None + self._robot_id = None + self._lease = None + + robot_tasks = [ + self._robot_state_task, + self._robot_metrics_task, + self._lease_task, + self._idle_task, + self._estop_monitor, + ] - robot_tasks = [ - self._robot_state_task, - self._robot_metrics_task, - self._lease_task, - self._front_image_task, - self._idle_task, - self._estop_monitor, - self._world_objects_task, - ] - - if self._point_cloud_client: - self._point_cloud_task = AsyncPointCloudService( - self._point_cloud_client, - self._logger, - max(0.0, self._rates.get("point_cloud", 0.0)), - self._callbacks.get("lidar_points", None), - self._point_cloud_requests, - ) - robot_tasks.append(self._point_cloud_task) + # Create component objects for different functionality + self.spot_image = SpotImages( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + robot_tasks.append(self.spot_image.front_image_task) + robot_tasks.append(self.spot_image.side_image_task) + robot_tasks.append(self.spot_image.rear_image_task) + + if self._robot.has_arm(): + robot_tasks.append(self._hand_image_task) + self._spot_arm = SpotArm( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + else: + self._spot_arm = None + + self._spot_docking = SpotDocking( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_graph_nav = SpotGraphNav( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_check = SpotCheck( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_image = SpotImages( + self._robot, self._logger, self._robot_params, self._robot_clients + ) - self._async_tasks = AsyncTasks(robot_tasks) + if self._point_cloud_client: + self._spot_eap = SpotEAP( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._point_cloud_task = self._spot_eap.async_task + robot_tasks.append(self._point_cloud_task) + else: + self._spot_eap = None - self.camera_task_name_to_task_mapping = { - "hand_image": self._hand_image_task, - "side_image": self._side_image_task, - "rear_image": self._rear_image_task, - "front_image": self._front_image_task, - } + self._spot_world_objects = SpotWorldObjects( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._world_objects_task = self._spot_world_objects.async_task + robot_tasks.append(self._world_objects_task) - self._robot_id = None - self._lease = None + self._async_tasks = AsyncTasks(robot_tasks) @property - def robot_name(self): + def robot_name(self) -> str: return self._robot_name @property - def frame_prefix(self): + def frame_prefix(self) -> str: return self._frame_prefix @property - def logger(self): + def spot_images(self) -> SpotImages: + """Return SpotImages instance""" + return self._spot_image + + @property + def spot_arm(self) -> SpotArm: + """Return SpotArm instance""" + if not self._robot.has_arm(): + raise Exception("SpotArm is not available on this robot") + else: + return self._spot_arm + + @property + def spot_eap_lidar(self) -> SpotEAP: + """Return SpotEAP instance""" + return self._spot_eap + + @property + def spot_world_objects(self) -> SpotWorldObjects: + """Return SpotWorldObjects instance""" + return self._spot_world_objects + + @property + def spot_docking(self) -> SpotDocking: + """Return SpotDocking instance""" + return self._spot_docking + + @property + def spot_graph_nav(self) -> SpotGraphNav: + """Return SpotGraphNav instance""" + return self._spot_graph_nav + + @property + def spot_check(self) -> SpotCheck: + """Return SpotCheck instance""" + return self._spot_check + + @property + def logger(self) -> logging.Logger: """Return logger instance of the SpotWrapper""" return self._logger @property - def is_valid(self): + def is_valid(self) -> bool: """Return boolean indicating if the wrapper initialized successfully""" return self._valid @property - def id(self): + def id(self) -> str: """Return robot's ID""" return self._robot_id @property - def robot_state(self): + def robot_state(self) -> robot_state_pb2.RobotState: """Return latest proto from the _robot_state_task""" return self._robot_state_task.proto @property - def metrics(self): + def metrics(self) -> robot_state_pb2.RobotMetrics: """Return latest proto from the _robot_metrics_task""" return self._robot_metrics_task.proto @property - def lease(self): + def lease(self) -> typing.List[lease_pb2.LeaseResource]: """Return latest proto from the _lease_task""" return self._lease_task.proto @property - def world_objects(self): + def world_objects(self) -> world_object_pb2.ListWorldObjectResponse: """Return most recent proto from _world_objects_task""" - return self._world_objects_task.proto + return self.spot_world_objects.async_task.proto @property - def front_images(self): + def front_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _front_image_task""" - return self._front_image_task.proto + return self.spot_image.front_image_task.proto @property - def side_images(self): + def side_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _side_image_task""" - return self._side_image_task.proto + return self.spot_image.side_image_task.proto @property - def rear_images(self): + def rear_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _rear_image_task""" - return self._rear_image_task.proto + return self.spot_image.rear_image_task.proto @property - def hand_images(self): + def hand_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _hand_image_task""" return self._hand_image_task.proto @property - def point_clouds(self): + def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: """Return latest proto from the _point_cloud_task""" - return self._point_cloud_task.proto + return self.spot_eap_lidar.async_task.proto @property - def is_standing(self): + def is_standing(self) -> bool: """Return boolean of standing state""" return self._is_standing @property - def is_sitting(self): + def is_sitting(self) -> bool: """Return boolean of standing state""" return self._is_sitting @property - def is_moving(self): + def is_moving(self) -> bool: """Return boolean of walking state""" return self._is_moving @property - def near_goal(self): + def near_goal(self) -> bool: return self._near_goal @property - def at_goal(self): + def at_goal(self) -> bool: return self._at_goal - def is_estopped(self, timeout=None): + def is_estopped(self, timeout=None) -> bool: return self._robot.is_estopped(timeout=timeout) @property - def time_skew(self): + def time_skew(self) -> Timestamp: """Return the time skew between local and spot time""" return self._robot.time_sync.endpoint.clock_skew @@ -907,7 +798,7 @@ def resetMobilityParams(self): """ self._mobility_params = RobotCommandBuilder.mobility_params() - def robotToLocalTime(self, timestamp): + def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: """Takes a timestamp and an estimated skew and return seconds and nano seconds in local time Args: @@ -1034,7 +925,12 @@ def disconnect(self): self.releaseLease() self.releaseEStop() - def _robot_command(self, command_proto, end_time_secs=None, timesync_endpoint=None): + def _robot_command( + self, + command_proto: robot_command_pb2.RobotCommand, + end_time_secs: typing.Optional[float] = None, + timesync_endpoint: typing.Optional[TimeSyncEndpoint] = None, + ) -> typing.Tuple[bool, str, typing.Optional[str]]: """Generic blocking function for sending commands to robots. Args: @@ -1170,7 +1066,7 @@ def power_on(self): return True, "Was already powered on" - def set_mobility_params(self, mobility_params): + def set_mobility_params(self, mobility_params: spot_command_pb2.MobilityParams): """Set Params for mobility and movement Args: @@ -1178,17 +1074,18 @@ def set_mobility_params(self, mobility_params): """ self._mobility_params = mobility_params - def get_mobility_params(self): + def get_mobility_params(self) -> spot_command_pb2.MobilityParams: """Get mobility params""" return self._mobility_params - def list_world_objects(self, object_types, time_start_point): - return self._world_objects_client.list_world_objects( - object_types, time_start_point - ) - @try_claim - def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125): + def velocity_cmd( + self, + v_x: float, + v_y: float, + v_rot: float, + cmd_duration: float = 0.7, + ) -> typing.Tuple[bool, str]: """Send a velocity motion command to the robot. Args: @@ -1211,14 +1108,14 @@ def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125): @try_claim def trajectory_cmd( self, - goal_x, - goal_y, - goal_heading, - cmd_duration, - frame_name="odom", - precise_position=False, - mobility_params=None, - ): + goal_x: float, + goal_y: float, + goal_heading: float, + cmd_duration: float, + frame_name: str = "odom", + precise_position: bool = False, + mobility_params: spot_command_pb2.MobilityParams = None, + ) -> typing.Tuple[bool, str]: """Send a trajectory motion command to the robot. Args: @@ -1282,7 +1179,9 @@ def trajectory_cmd( self._last_trajectory_command = response[2] return response[0], response[1] - def robot_command(self, robot_command): + def robot_command( + self, robot_command: robot_command_pb2.RobotCommand + ) -> typing.Tuple[bool, str]: end_time = time.time() + MAX_COMMAND_DURATION return self._robot_command( robot_command, @@ -1290,1053 +1189,13 @@ def robot_command(self, robot_command): timesync_endpoint=self._robot.time_sync.endpoint, ) - def get_robot_command_feedback(self, cmd_id): + def get_robot_command_feedback( + self, cmd_id: int + ) -> robot_command_pb2.RobotCommandFeedbackResponse: return self._robot_command_client.robot_command_feedback(cmd_id) - def list_graph(self, upload_path): - """List waypoint ids of garph_nav - Args: - upload_path : Path to the root directory of the map. - """ - ids, eds = self._list_graph_waypoint_and_edge_ids() - # skip waypoint_ for v2.2.1, skip waypiont for < v2.2 - return [ - v - for k, v in sorted( - ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) - ) - ] - - @try_claim - def navigate_to( - self, - upload_path, - navigate_to, - initial_localization_fiducial=True, - initial_localization_waypoint=None, - ): - """navigate with graph nav. - - Args: - upload_path : Path to the root directory of the map. - navigate_to : Waypont id string for where to goal - initial_localization_fiducial : Tells the initializer whether to use fiducials - initial_localization_waypoint : Waypoint id string of current robot position (optional) - """ - # Filepath for uploading a saved graph's and snapshots too. - if upload_path[-1] == "/": - upload_filepath = upload_path[:-1] - else: - upload_filepath = upload_path - - # Boolean indicating the robot's power state. - power_state = self._robot_state_client.get_robot_state().power_state - self._started_powered_on = power_state.motor_power_state == power_state.STATE_ON - self._powered_on = self._started_powered_on - - # FIX ME somehow,,,, if the robot is stand, need to sit the robot before starting garph nav - if self.is_standing and not self.is_moving: - self.sit() - - # TODO verify estop / claim / power_on - self._clear_graph() - self._upload_graph_and_snapshots(upload_filepath) - if initial_localization_fiducial: - self._set_initial_localization_fiducial() - if initial_localization_waypoint: - self._set_initial_localization_waypoint([initial_localization_waypoint]) - self._list_graph_waypoint_and_edge_ids() - self._get_localization_state() - resp = self._navigate_to([navigate_to]) - - return resp - - # Arm ############################################ - @try_claim - def ensure_arm_power_and_stand(self): - if not self._robot.has_arm(): - return False, "Spot with an arm is required for this service" - - try: - if not self.check_is_powered_on(): - self._logger.info("Spot is powering on within the timeout of 20 secs") - self._robot.power_on(timeout_sec=20) - assert self._robot.is_powered_on(), "Spot failed to power on" - self._logger.info("Spot is powered on") - except Exception as e: - return ( - False, - "Exception occured while Spot or its arm were trying to power on", - ) - - if not self._is_standing: - robot_command.blocking_stand( - command_client=self._robot_command_client, timeout_sec=10.0 - ) - self._logger.info("Spot is standing") - else: - self._logger.info("Spot is already standing") - - return True, "Spot has an arm, is powered on, and standing" - - @try_claim - def arm_stow(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Stow Arm - stow = RobotCommandBuilder.arm_stow_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(stow) - self._logger.info("Command stow issued") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while trying to stow" - - return True, "Stow arm success" - - @try_claim - def arm_unstow(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Unstow Arm - unstow = RobotCommandBuilder.arm_ready_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(unstow) - self._logger.info("Command unstow issued") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while trying to unstow" - - return True, "Unstow arm success" - - @try_claim - def arm_carry(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Get Arm in carry mode - carry = RobotCommandBuilder.arm_carry_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(carry) - self._logger.info("Command carry issued") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while carry mode was issued" - - return True, "Carry mode success" - - def make_arm_trajectory_command(self, arm_joint_trajectory): - """Helper function to create a RobotCommand from an ArmJointTrajectory. - Copy from 'spot-sdk/python/examples/arm_joint_move/arm_joint_move.py'""" - - joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request( - trajectory=arm_joint_trajectory - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_joint_move_command=joint_move_command - ) - sync_arm = synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - arm_sync_robot_cmd = robot_command_pb2.RobotCommand( - synchronized_command=sync_arm - ) - return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd) - - @try_claim - def arm_joint_move(self, joint_targets): - # All perspectives are given when looking at the robot from behind after the unstow service is called - # Joint1: 0.0 arm points to the front. positive: turn left, negative: turn right) - # RANGE: -3.14 -> 3.14 - # Joint2: 0.0 arm points to the front. positive: move down, negative move up - # RANGE: 0.4 -> -3.13 ( - # Joint3: 0.0 arm straight. moves the arm down - # RANGE: 0.0 -> 3.1415 - # Joint4: 0.0 middle position. negative: moves ccw, positive moves cw - # RANGE: -2.79253 -> 2.79253 - # # Joint5: 0.0 gripper points to the front. positive moves the gripper down - # RANGE: -1.8326 -> 1.8326 - # Joint6: 0.0 Gripper is not rolled, positive is ccw - # RANGE: -2.87 -> 2.87 - # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0] - if abs(joint_targets[0]) > 3.14: - msg = "Joint 1 has to be between -3.14 and 3.14" - self._logger.warning(msg) - return False, msg - elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13: - msg = "Joint 2 has to be between -3.13 and 0.4" - self._logger.warning(msg) - return False, msg - elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0: - msg = "Joint 3 has to be between 0.0 and 3.14" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[3]) > 2.79253: - msg = "Joint 4 has to be between -2.79253 and 2.79253" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[4]) > 1.8326: - msg = "Joint 5 has to be between -1.8326 and 1.8326" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[5]) > 2.87: - msg = "Joint 6 has to be between -2.87 and 2.87" - self._logger.warning(msg) - return False, msg - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - trajectory_point = ( - RobotCommandBuilder.create_arm_joint_trajectory_point( - joint_targets[0], - joint_targets[1], - joint_targets[2], - joint_targets[3], - joint_targets[4], - joint_targets[5], - ) - ) - arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory( - points=[trajectory_point] - ) - arm_command = self.make_arm_trajectory_command(arm_joint_trajectory) - - # Send the request - cmd_id = self._robot_command_client.robot_command(arm_command) - - # Query for feedback to determine how long it will take - feedback_resp = self._robot_command_client.robot_command_feedback( - cmd_id - ) - joint_move_feedback = ( - feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback - ) - time_to_goal: Duration = joint_move_feedback.time_to_goal - time_to_goal_in_seconds: float = time_to_goal.seconds + ( - float(time_to_goal.nanos) / float(10**9) - ) - time.sleep(time_to_goal_in_seconds) - return True, "Spot Arm moved successfully" - - except Exception as e: - return False, "Exception occured during arm movement: " + str(e) - - @try_claim - def force_trajectory(self, data): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - - def create_wrench_from_msg(forces, torques): - force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) - torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) - return geometry_pb2.Wrench(force=force, torque=torque) - - # Duration in seconds. - traj_duration = data.duration - - # first point on trajectory - wrench0 = create_wrench_from_msg(data.forces_pt0, data.torques_pt0) - t0 = seconds_to_duration(0) - traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( - wrench=wrench0, time_since_reference=t0 - ) - - # Second point on the trajectory - wrench1 = create_wrench_from_msg(data.forces_pt1, data.torques_pt1) - t1 = seconds_to_duration(traj_duration) - traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( - wrench=wrench1, time_since_reference=t1 - ) - - # Build the trajectory - trajectory = trajectory_pb2.WrenchTrajectory( - points=[traj_point0, traj_point1] - ) - - # Build the trajectory request, putting all axes into force mode - arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( - root_frame_name=data.frame, - wrench_trajectory_in_task=trajectory, - x_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - y_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - z_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - rx_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - ry_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - rz_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_cartesian_command=arm_cartesian_command - ) - synchronized_command = ( - synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - ) - robot_command = robot_command_pb2.RobotCommand( - synchronized_command=synchronized_command - ) - - # Send the request - self._robot_command_client.robot_command(robot_command) - self._logger.info("Force trajectory command sent") - - time.sleep(float(traj_duration) + 1.0) - - except Exception as e: - return False, "Exception occured during arm movement" - - return True, "Moved arm successfully" - - @try_claim - def gripper_open(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Open gripper - command = RobotCommandBuilder.claw_gripper_open_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) - self._logger.info("Command gripper open sent") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while gripper was moving" - - return True, "Open gripper success" - - @try_claim - def gripper_close(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Close gripper - command = RobotCommandBuilder.claw_gripper_close_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) - self._logger.info("Command gripper close sent") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while gripper was moving" - - return True, "Closed gripper successfully" - - @try_claim - def gripper_angle_open(self, gripper_ang): - # takes an angle between 0 (closed) and 90 (fully opened) and opens the - # gripper at this angle - if gripper_ang > 90 or gripper_ang < 0: - return False, "Gripper angle must be between 0 and 90" - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # The open angle command does not take degrees but the limits - # defined in the urdf, that is why we have to interpolate - closed = 0.349066 - opened = -1.396263 - angle = gripper_ang / 90.0 * (opened - closed) + closed - command = RobotCommandBuilder.claw_gripper_open_angle_command(angle) - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) - self._logger.info("Command gripper open angle sent") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while gripper was moving" - - return True, "Opened gripper successfully" - - @try_claim - def hand_pose(self, data): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - pose_point = data.pose_point - # Move the arm to a spot in front of the robot given a pose for the gripper. - # Build a position to move the arm to (in meters, relative to the body frame origin.) - position = geometry_pb2.Vec3( - x=pose_point.position.x, - y=pose_point.position.y, - z=pose_point.position.z, - ) - - # # Rotation as a quaternion. - rotation = geometry_pb2.Quaternion( - w=pose_point.orientation.w, - x=pose_point.orientation.x, - y=pose_point.orientation.y, - z=pose_point.orientation.z, - ) - - seconds = data.duration - duration = seconds_to_duration(seconds) - - # Build the SE(3) pose of the desired hand position in the moving body frame. - hand_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) - hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint( - pose=hand_pose, time_since_reference=duration - ) - hand_trajectory = trajectory_pb2.SE3Trajectory( - points=[hand_pose_traj_point] - ) - - arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( - root_frame_name=data.frame, - pose_trajectory_in_task=hand_trajectory, - force_remain_near_current_joint_configuration=True, - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_cartesian_command=arm_cartesian_command - ) - synchronized_command = ( - synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - ) - - robot_command = robot_command_pb2.RobotCommand( - synchronized_command=synchronized_command - ) - - command = RobotCommandBuilder.build_synchro_command(robot_command) - - # Send the request - self._robot_command_client.robot_command(robot_command) - self._logger.info("Moving arm to position.") - - time.sleep(2.0) - - except Exception as e: - return ( - False, - "An error occured while trying to move arm \n Exception:" + str(e), - ) - - return True, "Moved arm successfully" - - @try_claim - def grasp_3d(self, frame, object_rt_frame): - try: - frm = str(frame) - pos = geometry_pb2.Vec3( - x=object_rt_frame[0], y=object_rt_frame[1], z=object_rt_frame[2] - ) - - grasp = manipulation_api_pb2.PickObject(frame_name=frm, object_rt_frame=pos) - - # Ask the robot to pick up the object - grasp_request = manipulation_api_pb2.ManipulationApiRequest( - pick_object=grasp - ) - # Send the request - cmd_response = self._manipulation_client.manipulation_api_command( - manipulation_api_request=grasp_request - ) - - # Get feedback from the robot - while True: - feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( - manipulation_cmd_id=cmd_response.manipulation_cmd_id - ) - - # Send the request - response = self._manipulation_client.manipulation_api_feedback_command( - manipulation_api_feedback_request=feedback_request - ) - - print( - "Current state: ", - manipulation_api_pb2.ManipulationFeedbackState.Name( - response.current_state - ), - ) - - if ( - response.current_state - == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED - or response.current_state - == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED - ): - break - - time.sleep(0.25) - - self._robot.logger.info("Finished grasp.") - - except Exception as e: - return False, "An error occured while trying to grasp from pose" - - return True, "Grasped successfully" - - ################################################################### - - ## copy from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py - def _get_localization_state(self, *args): - """Get the current localization and state of the robot.""" - state = self._graph_nav_client.get_localization_state() - self._logger.info("Got localization: \n%s" % str(state.localization)) - odom_tform_body = get_odom_tform_body( - state.robot_kinematics.transforms_snapshot - ) - self._logger.info( - "Got robot state in kinematic odometry frame: \n%s" % str(odom_tform_body) - ) - - def _set_initial_localization_fiducial(self, *args): - """Trigger localization when near a fiducial.""" - robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() - # Create an empty instance for initial localization since we are asking it to localize - # based on the nearest fiducial. - localization = nav_pb2.Localization() - self._graph_nav_client.set_localization( - initial_guess_localization=localization, - ko_tform_body=current_odom_tform_body, - ) - - def _set_initial_localization_waypoint(self, *args): - """Trigger localization to a waypoint.""" - # Take the first argument as the localization waypoint. - if len(args) < 1: - # If no waypoint id is given as input, then return without initializing. - self._logger.error("No waypoint specified to initialize to.") - return - destination_waypoint = graph_nav_util.find_unique_waypoint_id( - args[0][0], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not destination_waypoint: - # Failed to find the unique waypoint id. - return - - robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() - # Create an initial localization to the specified waypoint as the identity. - localization = nav_pb2.Localization() - localization.waypoint_id = destination_waypoint - localization.waypoint_tform_body.rotation.w = 1.0 - self._graph_nav_client.set_localization( - initial_guess_localization=localization, - # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). - max_distance=0.2, - max_yaw=20.0 * math.pi / 180.0, - fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NO_FIDUCIAL, - ko_tform_body=current_odom_tform_body, - ) - - def _list_graph_waypoint_and_edge_ids(self, *args): - """List the waypoint ids and edge ids of the graph currently on the robot.""" - - # Download current graph - graph = self._graph_nav_client.download_graph() - if graph is None: - self._logger.error("Empty graph.") - return - self._current_graph = graph - - localization_id = ( - self._graph_nav_client.get_localization_state().localization.waypoint_id - ) - - # Update and print waypoints and edges - ( - self._current_annotation_name_to_wp_id, - self._current_edges, - ) = graph_nav_util.update_waypoints_and_edges( - graph, localization_id, self._logger - ) - return self._current_annotation_name_to_wp_id, self._current_edges - - def _upload_graph_and_snapshots(self, upload_filepath): - """Upload the graph and snapshots to the robot.""" - self._logger.info("Loading the graph from disk into local storage...") - with open(upload_filepath + "/graph", "rb") as graph_file: - # Load the graph from disk. - data = graph_file.read() - self._current_graph = map_pb2.Graph() - self._current_graph.ParseFromString(data) - self._logger.info( - "Loaded graph has {} waypoints and {} edges".format( - len(self._current_graph.waypoints), len(self._current_graph.edges) - ) - ) - for waypoint in self._current_graph.waypoints: - # Load the waypoint snapshots from disk. - with open( - upload_filepath + "/waypoint_snapshots/{}".format(waypoint.snapshot_id), - "rb", - ) as snapshot_file: - waypoint_snapshot = map_pb2.WaypointSnapshot() - waypoint_snapshot.ParseFromString(snapshot_file.read()) - self._current_waypoint_snapshots[ - waypoint_snapshot.id - ] = waypoint_snapshot - for edge in self._current_graph.edges: - # Load the edge snapshots from disk. - with open( - upload_filepath + "/edge_snapshots/{}".format(edge.snapshot_id), "rb" - ) as snapshot_file: - edge_snapshot = map_pb2.EdgeSnapshot() - edge_snapshot.ParseFromString(snapshot_file.read()) - self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot - # Upload the graph to the robot. - self._logger.info("Uploading the graph and snapshots to the robot...") - self._graph_nav_client.upload_graph( - lease=self._lease.lease_proto, graph=self._current_graph - ) - # Upload the snapshots to the robot. - for waypoint_snapshot in self._current_waypoint_snapshots.values(): - self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) - self._logger.info("Uploaded {}".format(waypoint_snapshot.id)) - for edge_snapshot in self._current_edge_snapshots.values(): - self._graph_nav_client.upload_edge_snapshot(edge_snapshot) - self._logger.info("Uploaded {}".format(edge_snapshot.id)) - - # The upload is complete! Check that the robot is localized to the graph, - # and it if is not, prompt the user to localize the robot before attempting - # any navigation commands. - localization_state = self._graph_nav_client.get_localization_state() - if not localization_state.localization.waypoint_id: - # The robot is not localized to the newly uploaded graph. - self._logger.info( - "Upload complete! The robot is currently not localized to the map; please localize", - "the robot using commands (2) or (3) before attempting a navigation command.", - ) - - @try_claim - def _navigate_to(self, *args): - """Navigate to a specific waypoint.""" - # Take the first argument as the destination waypoint. - if len(args) < 1: - # If no waypoint id is given as input, then return without requesting navigation. - self._logger.info("No waypoint provided as a destination for navigate to.") - return - - self._lease = self._lease_wallet.get_lease() - destination_waypoint = graph_nav_util.find_unique_waypoint_id( - args[0][0], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not destination_waypoint: - # Failed to find the appropriate unique waypoint id for the navigation command. - return - if not self.toggle_power(should_power_on=True): - self._logger.info( - "Failed to power on the robot, and cannot complete navigate to request." - ) - return - - # Stop the lease keepalive and create a new sublease for graph nav. - self._lease = self._lease_wallet.advance() - sublease = self._lease.create_sublease() - self._lease_keepalive.shutdown() - - # Navigate to the destination waypoint. - is_finished = False - nav_to_cmd_id = -1 - while not is_finished: - # Issue the navigation command about twice a second such that it is easy to terminate the - # navigation command (with estop or killing the program). - nav_to_cmd_id = self._graph_nav_client.navigate_to( - destination_waypoint, 1.0, leases=[sublease.lease_proto] - ) - time.sleep(0.5) # Sleep for half a second to allow for command execution. - # Poll the robot for feedback to determine if the navigation command is complete. Then sit - # the robot down once it is finished. - is_finished = self._check_success(nav_to_cmd_id) - - self._lease = self._lease_wallet.advance() - self._lease_keepalive = LeaseKeepAlive(self._lease_client) - - # Update the lease and power off the robot if appropriate. - if self._powered_on and not self._started_powered_on: - # Sit the robot down + power off after the navigation command is complete. - self.toggle_power(should_power_on=False) - - status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): - return True, "Successfully completed the navigation commands!" - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - return ( - False, - "Robot got lost when navigating the route, the robot will now sit down.", - ) - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - return ( - False, - "Robot got stuck when navigating the route, the robot will now sit down.", - ) - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): - return False, "Robot is impaired." - else: - return False, "Navigation command is not complete yet." - - @try_claim - def _navigate_route(self, *args): - """Navigate through a specific route of waypoints.""" - if len(args) < 1: - # If no waypoint ids are given as input, then return without requesting navigation. - self._logger.error("No waypoints provided for navigate route.") - return - waypoint_ids = args[0] - for i in range(len(waypoint_ids)): - waypoint_ids[i] = graph_nav_util.find_unique_waypoint_id( - waypoint_ids[i], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not waypoint_ids[i]: - # Failed to find the unique waypoint id. - return - - edge_ids_list = [] - all_edges_found = True - # Attempt to find edges in the current graph that match the ordered waypoint pairs. - # These are necessary to create a valid route. - for i in range(len(waypoint_ids) - 1): - start_wp = waypoint_ids[i] - end_wp = waypoint_ids[i + 1] - edge_id = self._match_edge(self._current_edges, start_wp, end_wp) - if edge_id is not None: - edge_ids_list.append(edge_id) - else: - all_edges_found = False - self._logger.error( - "Failed to find an edge between waypoints: ", - start_wp, - " and ", - end_wp, - ) - self._logger.error( - "List the graph's waypoints and edges to ensure pairs of waypoints has an edge." - ) - break - - self._lease = self._lease_wallet.get_lease() - if all_edges_found: - if not self.toggle_power(should_power_on=True): - self._logger.error( - "Failed to power on the robot, and cannot complete navigate route request." - ) - return - - # Stop the lease keepalive and create a new sublease for graph nav. - self._lease = self._lease_wallet.advance() - sublease = self._lease.create_sublease() - self._lease_keepalive.shutdown() - - # Navigate a specific route. - route = self._graph_nav_client.build_route(waypoint_ids, edge_ids_list) - is_finished = False - while not is_finished: - # Issue the route command about twice a second such that it is easy to terminate the - # navigation command (with estop or killing the program). - nav_route_command_id = self._graph_nav_client.navigate_route( - route, cmd_duration=1.0, leases=[sublease.lease_proto] - ) - time.sleep( - 0.5 - ) # Sleep for half a second to allow for command execution. - # Poll the robot for feedback to determine if the route is complete. Then sit - # the robot down once it is finished. - is_finished = self._check_success(nav_route_command_id) - - self._lease = self._lease_wallet.advance() - self._lease_keepalive = LeaseKeepAlive(self._lease_client) - - # Update the lease and power off the robot if appropriate. - if self._powered_on and not self._started_powered_on: - # Sit the robot down + power off after the navigation command is complete. - self.toggle_power(should_power_on=False) - - def _clear_graph(self, *args): - """Clear the state of the map on the robot, removing all waypoints and edges.""" - return self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) - - @try_claim - def toggle_power(self, should_power_on): - """Power the robot on/off dependent on the current power state.""" - is_powered_on = self.check_is_powered_on() - if not is_powered_on and should_power_on: - # Power on the robot up before navigating when it is in a powered-off state. - power_on(self._power_client) - motors_on = False - while not motors_on: - future = self._robot_state_client.get_robot_state_async() - state_response = future.result( - timeout=10 - ) # 10 second timeout for waiting for the state response. - if ( - state_response.power_state.motor_power_state - == robot_state_pb2.PowerState.STATE_ON - ): - motors_on = True - else: - # Motors are not yet fully powered on. - time.sleep(0.25) - elif is_powered_on and not should_power_on: - # Safe power off (robot will sit then power down) when it is in a - # powered-on state. - safe_power_off(self._robot_command_client, self._robot_state_client) - else: - # Return the current power state without change. - return is_powered_on - # Update the locally stored power state. - self.check_is_powered_on() - return self._powered_on - def check_is_powered_on(self): """Determine if the robot is powered on or off.""" power_state = self._robot_state_client.get_robot_state().power_state self._powered_on = power_state.motor_power_state == power_state.STATE_ON return self._powered_on - - def _check_success(self, command_id=-1): - """Use a navigation command id to get feedback from the robot and sit when command succeeds.""" - if command_id == -1: - # No command, so we have not status to check. - return False - status = self._graph_nav_client.navigation_feedback(command_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): - # Successfully completed the navigation commands! - return True - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - self._logger.error( - "Robot got lost when navigating the route, the robot will now sit down." - ) - return True - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - self._logger.error( - "Robot got stuck when navigating the route, the robot will now sit down." - ) - return True - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): - self._logger.error("Robot is impaired.") - return True - else: - # Navigation command is not complete yet. - return False - - def _match_edge(self, current_edges, waypoint1, waypoint2): - """Find an edge in the graph that is between two waypoint ids.""" - # Return the correct edge id as soon as it's found. - for edge_to_id in current_edges: - for edge_from_id in current_edges[edge_to_id]: - if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): - # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint2, to_waypoint=waypoint1 - ) - elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): - # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint1, to_waypoint=waypoint2 - ) - return None - - @try_claim - def dock(self, dock_id): - """Dock the robot to the docking station with fiducial ID [dock_id].""" - try: - # Make sure we're powered on and standing - self._robot.power_on() - self.stand() - # Dock the robot - self._last_docking_command = dock_id - blocking_dock_robot(self._robot, dock_id) - self._last_docking_command = None - # Necessary to reset this as docking often causes the last stand command to go into an unknown state - self._last_stand_command = None - return True, "Success" - except Exception as e: - return False, str(e) - - @try_claim - def undock(self, timeout=20): - """Power motors on and undock the robot from the station.""" - try: - # Maker sure we're powered on - self._robot.power_on() - # Undock the robot - blocking_undock(self._robot, timeout) - return True, "Success" - except Exception as e: - return False, str(e) - - def get_docking_state(self, **kwargs): - """Get docking state of robot.""" - state = self._docking_client.get_docking_state(**kwargs) - return state - - def update_image_tasks(self, image_name): - """Adds an async tasks to retrieve images from the specified image source""" - - task_to_add = self.camera_task_name_to_task_mapping[image_name] - - if task_to_add == self._hand_image_task and not self._robot.has_arm(): - self._logger.warning( - "Robot has no arm, therefore the arm image task can not be added" - ) - return - - if task_to_add in self._async_tasks._tasks: - self._logger.warning( - f"Task {image_name} already in async task list, will not be added again" - ) - return - - self._async_tasks.add_task(self.camera_task_name_to_task_mapping[image_name]) - - def get_frontleft_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "frontleft_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_frontright_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "frontright_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_left_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "left_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_right_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "right_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_back_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "back_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_images( - self, image_requests: typing.List[image_pb2.ImageRequest] - ) -> typing.Optional[ImageBundle]: - try: - image_responses = self._image_client.get_image(image_requests) - except UnsupportedPixelFormatRequestedError as e: - return None - return ImageBundle( - frontleft=image_responses[0], - frontright=image_responses[1], - left=image_responses[2], - right=image_responses[3], - back=image_responses[4], - ) - - def get_camera_images(self) -> ImageBundle: - return self.get_images(self._camera_image_requests) - - def get_depth_images(self) -> ImageBundle: - return self.get_images(self._depth_image_requests) - - def get_depth_registered_images(self) -> ImageBundle: - return self.get_images(self._depth_registered_image_requests) From 76f4899c5c8b58c8bcf2b2ba2ddaf47ec7d3164a Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 19:25:11 +0000 Subject: [PATCH 02/66] refactored image service --- spot_wrapper/spot_arm.py | 86 +++++++++++++++++++------ spot_wrapper/spot_config.py | 25 +------- spot_wrapper/spot_images.py | 125 ------------------------------------ spot_wrapper/wrapper.py | 27 ++------ 4 files changed, 75 insertions(+), 188 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index a06e3fab..0d75b73f 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -3,19 +3,12 @@ import logging from bosdyn.util import seconds_to_duration -from bosdyn.client.frame_helpers import ( - ODOM_FRAME_NAME, - GRAV_ALIGNED_BODY_FRAME_NAME, - get_a_tform_b, -) -from bosdyn.client import robot_command, math_helpers +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client import robot_command from bosdyn.client.robot import Robot from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.robot_command import ( - RobotCommandBuilder, - RobotCommandClient, - block_until_arm_arrives, -) +from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.image import ImageClient, build_image_request from bosdyn.client.manipulation_api_client import ManipulationApiClient from bosdyn.api import robot_command_pb2 from bosdyn.api import arm_command_pb2 @@ -23,15 +16,40 @@ from bosdyn.api import geometry_pb2 from bosdyn.api import trajectory_pb2 from bosdyn.api import manipulation_api_pb2 +from bosdyn.api import image_pb2 from google.protobuf.duration_pb2 import Duration -from geometry_msgs.msg import Pose -from spot_msgs.srv import HandPose, HandPoseResponse, HandPoseRequest -from spot_msgs.srv import ( - ArmForceTrajectory, - ArmForceTrajectoryResponse, - ArmForceTrajectoryRequest, -) + +from spot_msgs.srv import HandPoseRequest +from spot_msgs.srv import ArmForceTrajectoryRequest + +from .spot_config import * + + +class AsyncImageService(AsyncPeriodicQuery): + """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback, image_requests): + super(AsyncImageService, self).__init__( + "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + self._image_requests = image_requests + + def _start_query(self): + if self._callback: + callback_future = self._client.get_image_async(self._image_requests) + callback_future.add_done_callback(self._callback) + return callback_future class SpotArm: @@ -62,7 +80,37 @@ def __init__( "manipulation_client" ] self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] + self._image_client: ImageClient = robot_clients["image_client"] self._robot_command: typing.Callable = robot_clients["robot_command_method"] + self._rates: typing.Dict[str, float] = robot_params["rates"] + self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + + self._hand_image_task: AsyncImageService = None + self.initialize_hand_image_service() + + def initialize_hand_image_service(self): + self._hand_image_requests = [] + + for source in HAND_IMAGE_SOURCES: + self._hand_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._hand_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("hand_image", 0.0)), + self._callbacks.get("hand_image", None), + self._hand_image_requests, + ) + + self.camera_task_name_to_task_mapping: typing.Dict[str, AsyncImageService] = { + "hand_image": self._hand_image_task, + } + + @property + def hand_image_task(self): + return self._hand_image_task def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: if not self._robot.has_arm(): @@ -388,7 +436,7 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: return True, "Opened gripper successfully" - def hand_pose(self, data: HandPoseRequest): + def hand_pose(self, data: HandPoseRequest) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() if not success: diff --git a/spot_wrapper/spot_config.py b/spot_wrapper/spot_config.py index 0aa325b4..0cd7c921 100644 --- a/spot_wrapper/spot_config.py +++ b/spot_wrapper/spot_config.py @@ -6,40 +6,21 @@ SPOT_CLIENT_NAME = "ros_spot" MAX_COMMAND_DURATION = 1e5 -"""List of image sources for front image periodic query""" -front_image_sources = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "frontleft_depth", - "frontright_depth", -] - -"""List of image sources for side image periodic query""" -side_image_sources = [ - "left_fisheye_image", - "right_fisheye_image", - "left_depth", - "right_depth", -] - -"""List of image sources for rear image periodic query""" -rear_image_sources = ["back_fisheye_image", "back_depth"] - """Service name for getting pointcloud of VLP16 connected to Spot Core""" VELODYNE_SERVICE_NAME = "velodyne-point-cloud" """List of point cloud sources""" point_cloud_sources = ["velodyne-point-cloud"] -"""List of image sources for hand image periodic query""" -hand_image_sources = [ +"""List of hand image sources for asynchronous periodic query""" +HAND_IMAGE_SOURCES = [ "hand_image", "hand_depth", "hand_color_image", "hand_depth_in_hand_color_frame", ] -# TODO: Missing Hand images +"""List of body image sources for periodic query""" CAMERA_IMAGE_SOURCES = [ "frontleft_fisheye_image", "frontright_fisheye_image", diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index d8444a6e..ffdcb940 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,9 +1,7 @@ import typing import logging -from bosdyn.client.async_tasks import AsyncPeriodicQuery from bosdyn.client.robot import Robot -from bosdyn.client import robot_command from bosdyn.client.image import ( ImageClient, build_image_request, @@ -14,32 +12,6 @@ from .spot_config import * -class AsyncImageService(AsyncPeriodicQuery): - """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback, image_requests): - super(AsyncImageService, self).__init__( - "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - self._image_requests = image_requests - - def _start_query(self): - if self._callback: - callback_future = self._client.get_image_async(self._image_requests) - callback_future.add_done_callback(self._callback) - return callback_future - - class SpotImages: def __init__( self, @@ -51,72 +23,9 @@ def __init__( self._robot = robot self._logger = logger self._robot_params = robot_params - self._rates: typing.Dict[str, float] = robot_params["rates"] - self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] self._image_client: ImageClient = robot_clients["image_client"] - self._front_image_requests = [] - for source in front_image_sources: - self._front_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._side_image_requests = [] - for source in side_image_sources: - self._side_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._rear_image_requests = [] - for source in rear_image_sources: - self._rear_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._hand_image_requests = [] - for source in hand_image_sources: - self._hand_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._front_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("front_image", 0.0)), - self._callbacks.get("front_image", None), - self._front_image_requests, - ) - self._side_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("side_image", 0.0)), - self._callbacks.get("side_image", None), - self._side_image_requests, - ) - self._rear_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("rear_image", 0.0)), - self._callbacks.get("rear_image", None), - self._rear_image_requests, - ) - self._hand_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("hand_image", 0.0)), - self._callbacks.get("hand_image", None), - self._hand_image_requests, - ) - - self.camera_task_name_to_task_mapping: typing.Dict[str, AsyncImageService] = { - "hand_image": self._hand_image_task, - "side_image": self._side_image_task, - "rear_image": self._rear_image_task, - "front_image": self._front_image_task, - } - ############################################ - # TODO: Sort out double publishing of images self._camera_image_requests = [] for camera_source in CAMERA_IMAGE_SOURCES: self._camera_image_requests.append( @@ -144,40 +53,6 @@ def __init__( ) ) - @property - def front_image_task(self) -> AsyncImageService: - return self._front_image_task - - @property - def side_image_task(self) -> AsyncImageService: - return self._side_image_task - - @property - def rear_image_task(self) -> AsyncImageService: - return self._rear_image_task - - @property - def hand_image_task(self) -> AsyncImageService: - return self._hand_image_task - - def update_image_tasks(self, image_name: str): - """Adds an async tasks to retrieve images from the specified image source""" - task_to_add = self.camera_task_name_to_task_mapping[image_name] - - if task_to_add == self._hand_image_task and not self._robot.has_arm(): - self._logger.warning( - "Robot has no arm, therefore the arm image task can not be added" - ) - return - - if task_to_add in self._async_tasks._tasks: - self._logger.warning( - f"Task {image_name} already in async task list, will not be added again" - ) - return - - self._async_tasks.add_task(self.camera_task_name_to_task_mapping[image_name]) - def get_frontleft_rgb_image(self) -> image_pb2.Image: try: return self._image_client.get_image( diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index ba85d8ca..6273c3da 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -612,15 +612,13 @@ def __init__( self.spot_image = SpotImages( self._robot, self._logger, self._robot_params, self._robot_clients ) - robot_tasks.append(self.spot_image.front_image_task) - robot_tasks.append(self.spot_image.side_image_task) - robot_tasks.append(self.spot_image.rear_image_task) if self._robot.has_arm(): - robot_tasks.append(self._hand_image_task) self._spot_arm = SpotArm( self._robot, self._logger, self._robot_params, self._robot_clients ) + self._hand_image_task = self._spot_arm.hand_image_task + robot_tasks.append(self._hand_image_task) else: self._spot_arm = None @@ -633,7 +631,7 @@ def __init__( self._spot_check = SpotCheck( self._robot, self._logger, self._robot_params, self._robot_clients ) - self._spot_image = SpotImages( + self._spot_images = SpotImages( self._robot, self._logger, self._robot_params, self._robot_clients ) @@ -665,7 +663,7 @@ def frame_prefix(self) -> str: @property def spot_images(self) -> SpotImages: """Return SpotImages instance""" - return self._spot_image + return self._spot_images @property def spot_arm(self) -> SpotArm: @@ -735,25 +733,10 @@ def world_objects(self) -> world_object_pb2.ListWorldObjectResponse: """Return most recent proto from _world_objects_task""" return self.spot_world_objects.async_task.proto - @property - def front_images(self) -> typing.List[image_pb2.ImageResponse]: - """Return latest proto from the _front_image_task""" - return self.spot_image.front_image_task.proto - - @property - def side_images(self) -> typing.List[image_pb2.ImageResponse]: - """Return latest proto from the _side_image_task""" - return self.spot_image.side_image_task.proto - - @property - def rear_images(self) -> typing.List[image_pb2.ImageResponse]: - """Return latest proto from the _rear_image_task""" - return self.spot_image.rear_image_task.proto - @property def hand_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _hand_image_task""" - return self._hand_image_task.proto + return self.spot_arm.hand_image_task.proto @property def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: From f026a3e729154d3e42d656817813e2fb12d36ff7 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 19:27:53 +0000 Subject: [PATCH 03/66] cleanup imports --- spot_wrapper/spot_arm.py | 10 ++-------- spot_wrapper/spot_eap.py | 1 - spot_wrapper/spot_world_objects.py | 1 - spot_wrapper/wrapper.py | 23 +++-------------------- 4 files changed, 5 insertions(+), 30 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 0d75b73f..fce2ae29 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -19,10 +19,6 @@ from bosdyn.api import image_pb2 from google.protobuf.duration_pb2 import Duration - -from spot_msgs.srv import HandPoseRequest -from spot_msgs.srv import ArmForceTrajectoryRequest - from .spot_config import * @@ -297,9 +293,7 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: except Exception as e: return False, "Exception occured during arm movement: " + str(e) - def force_trajectory( - self, data: ArmForceTrajectoryRequest - ) -> typing.Tuple[bool, str]: + def force_trajectory(self, data) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -436,7 +430,7 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: return True, "Opened gripper successfully" - def hand_pose(self, data: HandPoseRequest) -> typing.Tuple[bool, str]: + def hand_pose(self, data) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() if not success: diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py index b3347a75..1d05718d 100644 --- a/spot_wrapper/spot_eap.py +++ b/spot_wrapper/spot_eap.py @@ -6,7 +6,6 @@ from bosdyn.client.robot import Robot from bosdyn.client import robot_command - from .spot_config import * diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py index 067801d2..90bbaa06 100644 --- a/spot_wrapper/spot_world_objects.py +++ b/spot_wrapper/spot_world_objects.py @@ -3,7 +3,6 @@ from bosdyn.client.async_tasks import AsyncPeriodicQuery from bosdyn.client.robot import Robot -from bosdyn.client import robot_command from bosdyn.client.world_object import WorldObjectClient from .spot_config import * diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 6273c3da..df91bfca 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1,58 +1,41 @@ import functools import logging -import math import time import traceback import typing import bosdyn.client.auth -from bosdyn.api import arm_command_pb2 -from bosdyn.api import geometry_pb2 from bosdyn.api import image_pb2 -from bosdyn.api import manipulation_api_pb2 from bosdyn.api import robot_command_pb2 from bosdyn.api import robot_state_pb2 -from bosdyn.api import synchronized_command_pb2 -from bosdyn.api import trajectory_pb2 from bosdyn.api import world_object_pb2 from bosdyn.api import lease_pb2 from bosdyn.api import point_cloud_pb2 from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 -from bosdyn.api.graph_nav import graph_nav_pb2 -from bosdyn.api.graph_nav import map_pb2 -from bosdyn.api.graph_nav import nav_pb2 from bosdyn.client import create_standard_sdk, ResponseError, RpcError from bosdyn.client import frame_helpers from bosdyn.client import math_helpers from bosdyn.client import robot_command from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks -from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock +from bosdyn.client.docking import DockingClient from bosdyn.client.estop import ( EstopClient, EstopEndpoint, EstopKeepAlive, MotorsOnError, ) -from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.client.graph_nav import GraphNavClient from bosdyn.client.map_processing import MapProcessingServiceClient -from bosdyn.client.image import ( - ImageClient, - build_image_request, - UnsupportedPixelFormatRequestedError, -) +from bosdyn.client.image import ImageClient from bosdyn.client.lease import LeaseClient, LeaseKeepAlive from bosdyn.client.manipulation_api_client import ManipulationApiClient -from bosdyn.client.point_cloud import build_pc_request -from bosdyn.client.power import safe_power_off, PowerClient, power_on +from bosdyn.client.power import PowerClient from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder from bosdyn.client.robot_state import RobotStateClient from bosdyn.client.world_object import WorldObjectClient from bosdyn.client.spot_check import SpotCheckClient from bosdyn.geometry import EulerZXY -from bosdyn.util import seconds_to_duration -from google.protobuf.duration_pb2 import Duration from bosdyn.api import basic_command_pb2 from google.protobuf.timestamp_pb2 import Timestamp From 88ae203fd8dec90c3e7bc373ab079c8af9af109b Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 19:36:39 +0000 Subject: [PATCH 04/66] add unit tests for some graph_nav_util functions --- .github/workflows/graph_nav_util_test.yml | 27 +++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 .github/workflows/graph_nav_util_test.yml diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml new file mode 100644 index 00000000..cd6bd556 --- /dev/null +++ b/.github/workflows/graph_nav_util_test.yml @@ -0,0 +1,27 @@ +name: graph_nav_util unit tests + +# Run on every push +on: push + +jobs: + test: + runs-on: ubuntu-latest + defaults: + run: + shell: bash + + steps: + - uses: actions/checkout@v3 + with: + path: $GITHUB_WORKSPACE + + - name: Set up Python + uses: actions/setup-python@v2 + with: + python-version: "3.8" + + - name: Install dependencies + run: pip install -r requirements.txt + + - name: Run tests + run: python3 -m unittest discover $GITHUB_WORKSPACE/spot_wrapper/spot_wrapper/tests From be3507833be1358f7d70ba237c5a25b2ff43f0ab Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:20:35 +0000 Subject: [PATCH 05/66] pip install requirements --- .github/workflows/graph_nav_util_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index cd6bd556..25172e7d 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -21,7 +21,7 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r requirements.txt + run: pip install -r $GITHUB_WORKSPACE/spot_wrapper/requirements.txt - name: Run tests run: python3 -m unittest discover $GITHUB_WORKSPACE/spot_wrapper/spot_wrapper/tests From 86a604fe4584d0902148ad67ecf574035291ca18 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:20:45 +0000 Subject: [PATCH 06/66] typing changes --- spot_wrapper/spot_images.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index ffdcb940..992347e8 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -53,7 +53,7 @@ def __init__( ) ) - def get_frontleft_rgb_image(self) -> image_pb2.Image: + def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ @@ -67,7 +67,7 @@ def get_frontleft_rgb_image(self) -> image_pb2.Image: except UnsupportedPixelFormatRequestedError as e: return None - def get_frontright_rgb_image(self) -> image_pb2.Image: + def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ @@ -81,7 +81,7 @@ def get_frontright_rgb_image(self) -> image_pb2.Image: except UnsupportedPixelFormatRequestedError as e: return None - def get_left_rgb_image(self) -> image_pb2.Image: + def get_left_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ @@ -95,7 +95,7 @@ def get_left_rgb_image(self) -> image_pb2.Image: except UnsupportedPixelFormatRequestedError as e: return None - def get_right_rgb_image(self) -> image_pb2.Image: + def get_right_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ @@ -109,7 +109,7 @@ def get_right_rgb_image(self) -> image_pb2.Image: except UnsupportedPixelFormatRequestedError as e: return None - def get_back_rgb_image(self) -> image_pb2.Image: + def get_back_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ From f4e9e43767926c3829a73b57d7438a78d7aa28db Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:22:49 +0000 Subject: [PATCH 07/66] absolute paths --- .github/workflows/graph_nav_util_test.yml | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 25172e7d..5d8f7496 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -11,9 +11,13 @@ jobs: shell: bash steps: - - uses: actions/checkout@v3 + - name: Make directory + run: mkdir -p /ros/spot_wrapper + + - name: Checkout repository + uses: actions/checkout@v3 with: - path: $GITHUB_WORKSPACE + path: /ros/spot_wrapper - name: Set up Python uses: actions/setup-python@v2 @@ -21,7 +25,7 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r $GITHUB_WORKSPACE/spot_wrapper/requirements.txt + run: pip install -r /ros/spot_wrapper/requirements.txt - name: Run tests - run: python3 -m unittest discover $GITHUB_WORKSPACE/spot_wrapper/spot_wrapper/tests + run: python3 -m unittest discover /ros/spot_wrapper/spot_wrapper/tests From 03c59970c9df7efc798f691c37f9d3aec9d22dde Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:23:53 +0000 Subject: [PATCH 08/66] clone to specific directory --- .github/workflows/graph_nav_util_test.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 5d8f7496..281e69a3 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -12,12 +12,12 @@ jobs: steps: - name: Make directory - run: mkdir -p /ros/spot_wrapper + run: mkdir -p ~/spot_wrapper - name: Checkout repository uses: actions/checkout@v3 with: - path: /ros/spot_wrapper + path: ~/spot_wrapper - name: Set up Python uses: actions/setup-python@v2 @@ -25,7 +25,7 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r /ros/spot_wrapper/requirements.txt + run: pip install -r ~/spot_wrapper/requirements.txt - name: Run tests - run: python3 -m unittest discover /ros/spot_wrapper/spot_wrapper/tests + run: python3 -m unittest discover ~/spot_wrapper/spot_wrapper/tests From 89213d8f9f3b4c069cbfcdbc251a872178d80530 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:25:45 +0000 Subject: [PATCH 09/66] check file structure for CI --- .github/workflows/graph_nav_util_test.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 281e69a3..9dedaaf5 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -19,6 +19,9 @@ jobs: with: path: ~/spot_wrapper + - name: Check files + run: ls -l ~/spot_wrapper + - name: Set up Python uses: actions/setup-python@v2 with: From 1ffab673e4c3463d7011baee48aee68db4e95d13 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:27:04 +0000 Subject: [PATCH 10/66] check files --- .github/workflows/graph_nav_util_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 9dedaaf5..c94b937b 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -20,7 +20,7 @@ jobs: path: ~/spot_wrapper - name: Check files - run: ls -l ~/spot_wrapper + run: ls ~/spot_wrapper - name: Set up Python uses: actions/setup-python@v2 From 860c796d038caaa41179f246338b5448206e5d2b Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:29:40 +0000 Subject: [PATCH 11/66] check files --- .github/workflows/graph_nav_util_test.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index c94b937b..7090a1c0 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -17,10 +17,10 @@ jobs: - name: Checkout repository uses: actions/checkout@v3 with: - path: ~/spot_wrapper + path: spot_wrapper - name: Check files - run: ls ~/spot_wrapper + run: ls spot_wrapper - name: Set up Python uses: actions/setup-python@v2 @@ -28,7 +28,7 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r ~/spot_wrapper/requirements.txt + run: pip install -r spot_wrapper/requirements.txt - name: Run tests - run: python3 -m unittest discover ~/spot_wrapper/spot_wrapper/tests + run: python3 -m unittest discover spot_wrapper/spot_wrapper/tests From 0097875fad09372ac62e1ff96a6f1397a0ea5c72 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:31:50 +0000 Subject: [PATCH 12/66] add test script --- setup.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/setup.py b/setup.py index 65b80317..6d9c5646 100644 --- a/setup.py +++ b/setup.py @@ -5,5 +5,8 @@ version="1.0.0", description="Wrapper for Boston Dynamics Spot SDK", packages=["spot_wrapper"], + scripts=[ + "tests/test_graph_nav_util.py", + ], install_requires=["bosdyn-client", "bosdyn-api", "bosdyn-mission", "bosdyn-core"], ) From 5f043537e0093bdd70e508bb8387604767d170a2 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:32:49 +0000 Subject: [PATCH 13/66] install spot wrapper --- .github/workflows/graph_nav_util_test.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 7090a1c0..678288bd 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -28,7 +28,9 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r spot_wrapper/requirements.txt + run: | + pip install -r spot_wrapper/requirements.txt + pip install -e spot_wrapper - name: Run tests run: python3 -m unittest discover spot_wrapper/spot_wrapper/tests From 7a9811b3b90fde6746f4832451861544a267bf3b Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:33:56 +0000 Subject: [PATCH 14/66] install script --- setup.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/setup.py b/setup.py index 6d9c5646..65b80317 100644 --- a/setup.py +++ b/setup.py @@ -5,8 +5,5 @@ version="1.0.0", description="Wrapper for Boston Dynamics Spot SDK", packages=["spot_wrapper"], - scripts=[ - "tests/test_graph_nav_util.py", - ], install_requires=["bosdyn-client", "bosdyn-api", "bosdyn-mission", "bosdyn-core"], ) From 15addcdddcc6d2b949a7a4d38e46818d5b58d3c6 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:36:40 +0000 Subject: [PATCH 15/66] remove asyncimageservice --- spot_wrapper/wrapper.py | 26 -------------------------- 1 file changed, 26 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index df91bfca..9e7a9c67 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -125,32 +125,6 @@ def _start_query(self): return callback_future -class AsyncImageService(AsyncPeriodicQuery): - """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback, image_requests): - super(AsyncImageService, self).__init__( - "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - self._image_requests = image_requests - - def _start_query(self): - if self._callback: - callback_future = self._client.get_image_async(self._image_requests) - callback_future.add_done_callback(self._callback) - return callback_future - - class AsyncIdle(AsyncPeriodicQuery): """Class to check if the robot is moving, and if not, command a stand with the set mobility parameters From 3fa6430ea9dffedb7c386540efd2096777b4e14a Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Wed, 19 Apr 2023 09:40:40 +0000 Subject: [PATCH 16/66] use robot_params to share state variables --- spot_wrapper/wrapper.py | 64 +++++++++++++++++++++-------------------- 1 file changed, 33 insertions(+), 31 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9e7a9c67..39e79db0 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -149,24 +149,24 @@ def _start_query(self): status = ( response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status ) - self._spot_wrapper._is_sitting = False + self._spot_wrapper._robot_params["is_sitting"] = False if status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING: - self._spot_wrapper._is_standing = True + self._spot_wrapper._robot_params["is_standing"] = True self._spot_wrapper._last_stand_command = None elif ( status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS ): - self._spot_wrapper._is_standing = False + self._spot_wrapper._robot_params["is_standing"] = False else: self._logger.warning("Stand command in unknown state") - self._spot_wrapper._is_standing = False + self._spot_wrapper._robot_params["is_standing"] = False except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_stand_command = None if self._spot_wrapper._last_sit_command != None: try: - self._spot_wrapper._is_standing = False + self._spot_wrapper._robot_params["is_standing"] = False response = self._client.robot_command_feedback( self._spot_wrapper._last_sit_command ) @@ -174,10 +174,10 @@ def _start_query(self): response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status == basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING ): - self._spot_wrapper._is_sitting = True + self._spot_wrapper._robot_params["is_sitting"] = True self._spot_wrapper._last_sit_command = None else: - self._spot_wrapper._is_sitting = False + self._spot_wrapper._robot_params["is_sitting"] = False except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_sit_command = None @@ -209,7 +209,7 @@ def _start_query(self): and not self._spot_wrapper._last_trajectory_command_precise ) ): - self._spot_wrapper._at_goal = True + self._spot_wrapper._robot_params["at_goal"] = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None elif ( @@ -222,7 +222,7 @@ def _start_query(self): == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL ): is_moving = True - self._spot_wrapper._near_goal = True + self._spot_wrapper._robot_params["near_goal"] = True elif ( status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN @@ -240,14 +240,14 @@ def _start_query(self): self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_trajectory_command = None - self._spot_wrapper._is_moving = is_moving + self._spot_wrapper._robot_params["is_moving"] = is_moving # We must check if any command currently has a non-None value for its id. If we don't do this, this stand # command can cause other commands to be interrupted before they get to start if ( - self._spot_wrapper.is_standing + self._spot_wrapper._robot_params["is_standing"] and self._spot_wrapper._continually_try_stand - and not self._spot_wrapper.is_moving + and not self._spot_wrapper._robot_params["is_moving"] and self._spot_wrapper._last_trajectory_command is not None and self._spot_wrapper._last_stand_command is not None and self._spot_wrapper._last_velocity_command_time is not None @@ -399,15 +399,6 @@ def __init__( self._last_trajectory_command_precise = None self._last_velocity_command_time = None self._last_docking_command = None - self._robot_params = { - "is_standing": False, - "is_sitting": True, - "is_moving": False, - "robot_id": None, - "estop_timeout": self._estop_timeout, - "rates": self._rates, - "callbacks": self._callbacks, - } try: self._sdk = create_standard_sdk(SPOT_CLIENT_NAME) @@ -566,6 +557,17 @@ def __init__( ] # Create component objects for different functionality + self._robot_params = { + "is_standing": self._is_standing, + "is_sitting": self._is_sitting, + "is_moving": self._is_moving, + "at_goal": self._at_goal, + "near_goal": self._near_goal, + "robot_id": self._robot_id, + "estop_timeout": self._estop_timeout, + "rates": self._rates, + "callbacks": self._callbacks, + } self.spot_image = SpotImages( self._robot, self._logger, self._robot_params, self._robot_clients ) @@ -668,7 +670,7 @@ def is_valid(self) -> bool: @property def id(self) -> str: """Return robot's ID""" - return self._robot_id + return self._robot_params["robot_id"] @property def robot_state(self) -> robot_state_pb2.RobotState: @@ -703,25 +705,25 @@ def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: @property def is_standing(self) -> bool: """Return boolean of standing state""" - return self._is_standing + return self._robot_params["is_standing"] @property def is_sitting(self) -> bool: """Return boolean of standing state""" - return self._is_sitting + return self._robot_params["is_sitting"] @property def is_moving(self) -> bool: """Return boolean of walking state""" - return self._is_moving + return self._robot_params["is_moving"] @property def near_goal(self) -> bool: - return self._near_goal + return self._robot_params["near_goal"] @property def at_goal(self) -> bool: - return self._at_goal + return self._robot_params["at_goal"] def is_estopped(self, timeout=None) -> bool: return self._robot.is_estopped(timeout=timeout) @@ -772,7 +774,7 @@ def claim(self): return True, "We already claimed the lease" try: - self._robot_id = self._robot.get_id() + self._robot_params["robot_id"] = self._robot.get_id() self.getLease() if self._start_estop and not self.check_is_powered_on(): # If we are requested to start the estop, and the robot is not already powered on, then we reset the @@ -965,7 +967,7 @@ def battery_change_pose(self, dir_hint: int = 1): Args: dir_hint: 1 rolls to the right side of the robot, 2 to the left """ - if self._is_sitting: + if self._robot_params["is_sitting"]: response = self._robot_command( RobotCommandBuilder.battery_change_pose_command(dir_hint) ) @@ -1072,8 +1074,8 @@ def trajectory_cmd( """ if mobility_params is None: mobility_params = self._mobility_params - self._at_goal = False - self._near_goal = False + self._robot_params["at_goal"] = False + self._robot_params["near_goal"] = False self._last_trajectory_command_precise = precise_position self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration From 55c68a1b575599a56ee39f5134819c86f81288c1 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Wed, 19 Apr 2023 23:41:38 +0200 Subject: [PATCH 17/66] Pytest replacement (#1) * use pytest instead * add pytest dependency * fail test on purpose * remove failing test --- .github/workflows/graph_nav_util_test.yml | 19 ++- requirements.txt | 1 + spot_wrapper/tests/test_graph_nav_util.py | 164 +++++++++------------- 3 files changed, 82 insertions(+), 102 deletions(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 678288bd..08636ac9 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -33,4 +33,21 @@ jobs: pip install -e spot_wrapper - name: Run tests - run: python3 -m unittest discover spot_wrapper/spot_wrapper/tests + run: pytest --junit-xml=test-results.xml + + - name: Surface failing tests + if: always() + uses: pmeier/pytest-summary-gha@main + with: + # A list of JUnit XML files, directories containing the former, and wildcard patterns to process. + path: test-results.xml + + # Add a summary of the results at the top of the report + summary: true + + # Select which results should be included in the report. + # Follows the same syntax as `pytest -r` + display-options: fEX + + # Fail the workflow if no JUnit XML was found. + fail-on-empty: true diff --git a/requirements.txt b/requirements.txt index 200f1390..76c87a8b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,4 @@ bosdyn_core==3.2.2.post1 protobuf==4.22.1 setuptools==45.2.0 +pytest==7.3.1 \ No newline at end of file diff --git a/spot_wrapper/tests/test_graph_nav_util.py b/spot_wrapper/tests/test_graph_nav_util.py index 1dc78556..18ba04d2 100755 --- a/spot_wrapper/tests/test_graph_nav_util.py +++ b/spot_wrapper/tests/test_graph_nav_util.py @@ -1,9 +1,5 @@ #!/usr/bin/env python3 -PKG = "graph_nav_util" -NAME = "test_graph_nav_util" -SUITE = "test_graph_nav_util.TestSuiteGraphNavUtil" - -import unittest +import pytest import logging from bosdyn.api.graph_nav import map_pb2 @@ -20,95 +16,93 @@ def __init__(self) -> None: graph_nav_util = MockSpotGraphNav() -class TestGraphNavUtilShortCode(unittest.TestCase): +class TestGraphNavUtilShortCode: def test_id_to_short_code(self): - self.assertEqual( - graph_nav_util._id_to_short_code("ebony-pug-mUzxLNq.TkGlVIxga+UKAQ=="), "ep" + assert ( + graph_nav_util._id_to_short_code("ebony-pug-mUzxLNq.TkGlVIxga+UKAQ==") + == "ep" ) - self.assertEqual( - graph_nav_util._id_to_short_code("erose-simian-sug9xpxhCxgft7Mtbhr98A=="), - "es", + assert ( + graph_nav_util._id_to_short_code("erose-simian-sug9xpxhCxgft7Mtbhr98A==") + == "es" ) -class TestGraphNavUtilFindUniqueWaypointId(unittest.TestCase): - def setUp(self): +class TestGraphNavUtilFindUniqueWaypointId: + def test_short_code(self): + # Set up self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) self.graph = map_pb2.Graph() self.name_to_id = {"ABCDE": "Node1"} - - def test_short_code(self): # Test normal short code - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "AC", self.graph, self.name_to_id, self.logger - ), - "AC", - "AC!=AC, normal short code", + ) + == "AC" ) # Test annotation name that is known - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "ABCDE", self.graph, self.name_to_id, self.logger - ), - "Node1", - "ABCDE!=Node1, known annotation name", + ) + == "Node1" ) # Test annotation name that is unknown - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "ABCDEF", self.graph, self.name_to_id, self.logger - ), - "ABCDEF", - "ABCDEF!=ABCDEF, unknown annotation name", + ) + == "ABCDEF" ) def test_short_code_with_graph(self): + # Set up + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + self.graph = map_pb2.Graph() + self.name_to_id = {"ABCDE": "Node1"} + # Test short code that is in graph self.graph.waypoints.add(id="AB-CD-EF") - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "AC", self.graph, self.name_to_id, self.logger - ), - "AB-CD-EF", - "AC!=AB-CD-EF, short code in graph", + ) + == "AB-CD-EF" ) # Test short code that is not in graph - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "AD", self.graph, self.name_to_id, self.logger - ), - "AD", - "AD!=AD, short code not in graph", + ) + == "AD" ) # Test multiple waypoints with the same short code self.graph.waypoints.add(id="AB-CD-EF-1") - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "AC", self.graph, self.name_to_id, self.logger - ), - "AC", - "AC!=AC, multiple waypoints with same short code", + ) + == "AC" ) -class TestGraphNavUtilUpdateWaypointsEdges(unittest.TestCase): - def setUp(self): +class TestGraphNavUtilUpdateWaypointsEdges: + def test_empty_graph(self): self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) - def test_empty_graph(self): # Test empty graph self.graph = map_pb2.Graph() self.localization_id = "" graph_nav_util._update_waypoints_and_edges( self.graph, self.localization_id, self.logger ) - self.assertEqual( - len(self.graph.waypoints), 0, "Empty graph should have 0 waypoints" - ) - self.assertEqual(len(self.graph.edges), 0, "Empty graph should have 0 edges") + assert len(self.graph.waypoints) == 0 + assert len(self.graph.edges) == 0 def test_one_waypoint(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + # Test graph with 1 waypoint self.localization_id = "" self.graph = map_pb2.Graph() @@ -121,19 +115,15 @@ def test_one_waypoint(self): self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( self.graph, self.localization_id, self.logger ) - self.assertEqual( - len(self.graph.waypoints), 1, "Graph with 1 waypoint should have 1 waypoint" - ) - self.assertEqual( - len(self.graph.edges), 0, "Graph with 1 waypoint should have 0 edges" - ) - self.assertEqual(len(self.edges), 0, "Edges should have 0 entries") - self.assertEqual(len(self.name_to_id), 1, "Name to id should have 1 entry") - self.assertEqual( - self.name_to_id["Node1"], "ABCDE", "Name to id should have 1 entry" - ) + assert len(self.graph.waypoints) == 1 + assert len(self.graph.edges) == 0 + assert len(self.edges) == 0 + assert len(self.name_to_id) == 1 + assert self.name_to_id["Node1"] == "ABCDE" def test_two_waypoints_with_edge(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + # Test graph with 2 waypoints and an edge between them self.localization_id = "" self.graph = map_pb2.Graph() @@ -154,23 +144,17 @@ def test_two_waypoints_with_edge(self): self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( self.graph, self.localization_id, self.logger ) - self.assertEqual( - len(self.graph.waypoints), - 2, - "Graph with 2 waypoints should have 2 waypoints", - ) - self.assertEqual( - len(self.graph.edges), 1, "Graph with 2 waypoints should have 1 edge" - ) - self.assertEqual(len(self.edges), 1, "Edges should have 1 entry") - self.assertEqual( - self.edges["DE"][0], "ABCDE", "Edges should point to the correct waypoint" - ) - self.assertEqual(len(self.name_to_id), 2, "Name to id should have 2 entries") - self.assertEqual(self.name_to_id["Node1"], "ABCDE", "Name to id entry, ABCDE") - self.assertEqual(self.name_to_id["Node2"], "DE", "Name to id entry, DE") + assert len(self.graph.waypoints) == 2 + assert len(self.graph.edges) == 1 + assert len(self.edges) == 1 + assert self.edges["DE"][0] == "ABCDE" + assert len(self.name_to_id) == 2 + assert self.name_to_id["Node1"] == "ABCDE" + assert self.name_to_id["Node2"] == "DE" def test_two_waypoints_with_edge_and_localization(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + # Test graph with 2 waypoints and an edge between them. Mainly affects the pretty print. self.localization_id = "ABCDE" self.graph = map_pb2.Graph() @@ -191,32 +175,10 @@ def test_two_waypoints_with_edge_and_localization(self): self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( self.graph, self.localization_id, self.logger ) - self.assertEqual( - len(self.graph.waypoints), - 2, - "Graph with 2 waypoints should have 2 waypoints", - ) - self.assertEqual( - len(self.graph.edges), 1, "Graph with 2 waypoints should have 1 edge" - ) - self.assertEqual(len(self.edges), 1, "Edges should have 1 entry") - self.assertEqual( - self.edges["DE"][0], "ABCDE", "Edges should point to the correct waypoint" - ) - self.assertEqual(len(self.name_to_id), 2, "Name to id should have 2 entries") - self.assertEqual(self.name_to_id["Node1"], "ABCDE", "Name to id entry, ABCDE") - self.assertEqual(self.name_to_id["Node2"], "DE", "Name to id entry, DE") - - -class TestSuiteGraphNavUtil(unittest.TestSuite): - def __init__(self) -> None: - super(TestSuiteGraphNavUtil, self).__init__() - - self.loader = unittest.TestLoader() - self.addTest(self.loader.loadTestsFromTestCase(TestGraphNavUtilShortCode)) - self.addTest( - self.loader.loadTestsFromTestCase(TestGraphNavUtilFindUniqueWaypointId) - ) - self.addTest( - self.loader.loadTestsFromTestCase(TestGraphNavUtilUpdateWaypointsEdges) - ) + assert len(self.graph.waypoints) == 2 + assert len(self.graph.edges) == 1 + assert len(self.edges) == 1 + assert self.edges["DE"][0] == "ABCDE" + assert len(self.name_to_id) == 2 + assert self.name_to_id["Node1"] == "ABCDE" + assert self.name_to_id["Node2"] == "DE" From cf262e412632aeb0b8a89a21e676b1d12377b71c Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Thu, 20 Apr 2023 07:17:37 +0000 Subject: [PATCH 18/66] replace print() with self._logger.error() --- spot_wrapper/wrapper.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 39e79db0..abec1eb2 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -787,7 +787,7 @@ def claim(self): self._logger.error("Failed to initialize robot communication: %s", err) return False, str(err) except Exception as err: - print(traceback.format_exc(), flush=True) + self._logger.error(traceback.format_exc(), flush=True) return False, str(err) def updateTasks(self): @@ -795,7 +795,7 @@ def updateTasks(self): try: self._async_tasks.update() except Exception as e: - print(f"Update tasks failed with error: {str(e)}") + self._logger.error(f"Update tasks failed with error: {str(e)}") def resetEStop(self): """Get keepalive for eStop""" From 4928997bd682d374ebf435d595ee7891c6379dea Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Fri, 21 Apr 2023 13:18:21 +0000 Subject: [PATCH 19/66] image publishing works well --- spot_wrapper/spot_images.py | 29 ++++++++++++++--------------- spot_wrapper/wrapper.py | 4 +++- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 992347e8..b2291601 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -31,9 +31,7 @@ def __init__( self._camera_image_requests.append( build_image_request( camera_source, - image_format=image_pb2.Image.FORMAT_JPEG, - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW, ) ) @@ -41,7 +39,7 @@ def __init__( for camera_source in DEPTH_IMAGE_SOURCES: self._depth_image_requests.append( build_image_request( - camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + camera_source, image_format=image_pb2.Image.FORMAT_RAW ) ) @@ -49,7 +47,7 @@ def __init__( for camera_source in DEPTH_REGISTERED_IMAGE_SOURCES: self._depth_registered_image_requests.append( build_image_request( - camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + camera_source, image_format=image_pb2.Image.FORMAT_RAW ) ) @@ -59,12 +57,12 @@ def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "frontleft_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: @@ -73,12 +71,12 @@ def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "frontright_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_left_rgb_image(self) -> image_pb2.ImageResponse: @@ -87,12 +85,12 @@ def get_left_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "left_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_right_rgb_image(self) -> image_pb2.ImageResponse: @@ -101,12 +99,12 @@ def get_right_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "right_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_back_rgb_image(self) -> image_pb2.ImageResponse: @@ -115,12 +113,12 @@ def get_back_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "back_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_images( @@ -129,6 +127,7 @@ def get_images( try: image_responses = self._image_client.get_image(image_requests) except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None return ImageBundle( frontleft=image_responses[0], diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 39e79db0..8363f40c 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -501,6 +501,7 @@ def __init__( "spot_check_client": self._spot_check_client, "robot_command_method": self._robot_command, "world_objects_client": self._world_objects_client, + "manipulation_client": self._manipulation_client, } if self._point_cloud_client: self._robot_clients["point_cloud_client"] = self._point_cloud_client @@ -787,6 +788,7 @@ def claim(self): self._logger.error("Failed to initialize robot communication: %s", err) return False, str(err) except Exception as err: + self._logger.error("Failed to claim lease: %s", err) print(traceback.format_exc(), flush=True) return False, str(err) @@ -795,7 +797,7 @@ def updateTasks(self): try: self._async_tasks.update() except Exception as e: - print(f"Update tasks failed with error: {str(e)}") + self._logger.error(f"Update tasks failed with error: {str(e)}") def resetEStop(self): """Get keepalive for eStop""" From 0c2bb45922dfc7c79505a83ef97d26461fe320bb Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Fri, 21 Apr 2023 13:22:56 +0000 Subject: [PATCH 20/66] moved SPOT_CLIENT_NAME --- spot_wrapper/wrapper.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 8363f40c..12d72676 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -765,12 +765,12 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: return rtime - def claim(self): + def claim(self) -> typing.List[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" for resource in self.lease: if ( resource.resource == "all-leases" - and self.SPOT_CLIENT_NAME in resource.lease_owner.client_name + and SPOT_CLIENT_NAME in resource.lease_owner.client_name ): return True, "We already claimed the lease" @@ -802,7 +802,7 @@ def updateTasks(self): def resetEStop(self): """Get keepalive for eStop""" self._estop_endpoint = EstopEndpoint( - self._estop_client, self.SPOT_CLIENT_NAME, self._estop_timeout + self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout ) self._estop_endpoint.force_simple_setup() # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) From 6779aa5584221b12dfb5924403410f38f218a5c8 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Fri, 21 Apr 2023 13:27:01 +0000 Subject: [PATCH 21/66] static typing for claim() --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 12d72676..561c1459 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -765,7 +765,7 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: return rtime - def claim(self) -> typing.List[bool, str]: + def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" for resource in self.lease: if ( From 4881d38d63935da2f360713ba6a67f120226f2ad Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Fri, 21 Apr 2023 14:36:31 +0000 Subject: [PATCH 22/66] black formatting --- spot_wrapper/spot_images.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index b2291601..6012e543 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -57,7 +57,7 @@ def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "frontleft_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + image_format=image_pb2.Image.FORMAT_RAW, ) ] )[0] @@ -71,7 +71,7 @@ def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "frontright_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + image_format=image_pb2.Image.FORMAT_RAW, ) ] )[0] @@ -84,8 +84,7 @@ def get_left_rgb_image(self) -> image_pb2.ImageResponse: return self._image_client.get_image( [ build_image_request( - "left_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + "left_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] @@ -98,8 +97,7 @@ def get_right_rgb_image(self) -> image_pb2.ImageResponse: return self._image_client.get_image( [ build_image_request( - "right_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + "right_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] @@ -112,8 +110,7 @@ def get_back_rgb_image(self) -> image_pb2.ImageResponse: return self._image_client.get_image( [ build_image_request( - "back_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + "back_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] From 392da898d027fc716a17f1cd7ff487183cc192cb Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Sat, 22 Apr 2023 18:43:43 +0000 Subject: [PATCH 23/66] comments and passing error feedback --- spot_wrapper/spot_arm.py | 15 +++---- spot_wrapper/spot_check.py | 73 +++++++++------------------------- spot_wrapper/spot_graph_nav.py | 69 ++++++++------------------------ spot_wrapper/wrapper.py | 8 ++-- 4 files changed, 47 insertions(+), 118 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index fce2ae29..843a13fb 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -58,13 +58,14 @@ def __init__( ): """ Constructor for SpotArm class. - :param robot: Robot object - :param logger: Logger object - :param robot_params: Dictionary of robot parameters - - robot_params['is_standing']: True if robot is standing, False otherwise - :param robot_clients: Dictionary of robot clients - - robot_clients['robot_command_client']: RobotCommandClient object - - robot_clients['robot_command_method']: RobotCommand method + Args: + robot: Robot object + logger: Logger object + robot_params: Dictionary of robot parameters + - robot_params['is_standing']: True if robot is standing, False otherwise + robot_clients: Dictionary of robot clients + - robot_clients['robot_command_client']: RobotCommandClient object + - robot_clients['robot_command_method']: RobotCommand method """ self._robot = robot self._logger = logger diff --git a/spot_wrapper/spot_check.py b/spot_wrapper/spot_check.py index 992d3aa3..ddeab131 100644 --- a/spot_wrapper/spot_check.py +++ b/spot_wrapper/spot_check.py @@ -46,63 +46,28 @@ def _feedback_error_check( # Save results from Spot Check self._spot_check_resp = resp + errorcode_mapping = { + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_UNEXPECTED_POWER_CHANGE: "Unexpected power change", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_IMU_CHECK: "Robot body is not flat on the ground", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_NOT_SITTING: "Robot body is not close to a sitting pose", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_LOADCELL_TIMEOUT: "Timeout during loadcell calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_ON_FAILURE: "Error enabling motor power", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ENDSTOP_TIMEOUT: "Timeout during endstop calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FAILED_STAND: "Robot failed to stand", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_CAMERA_TIMEOUT: "Timeout during camera check", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GROUND_CHECK: "Flat ground check failed", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_OFF_FAILURE: "Robot failed to power off", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_REVERT_FAILURE: "Robot failed to revert calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FGKC_FAILURE: "Robot failed to do flat ground kinematic calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GRIPPER_CAL_TIMEOUT: "Timeout during gripper calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ARM_CHECK_COLLISION: "Arm motion would cause collisions", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ARM_CHECK_TIMEOUT: "Timeout during arm joint check", + } # Check for common errors if resp.header.error.code in (2, 3): return False, str(resp.header.error.message) - - # Check for other errors - if ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_UNEXPECTED_POWER_CHANGE - ): - return False, "Unexpected power change" - elif ( - resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_IMU_CHECK - ): - return False, "Robot body is not flat on the ground" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_NOT_SITTING - ): - return False, "Robot body is not close to a sitting pose" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_LOADCELL_TIMEOUT - ): - return False, "Timeout during loadcell calibration" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_ON_FAILURE - ): - return False, "Error enabling motor power" - elif ( - resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ENDSTOP_TIMEOUT - ): - return False, "Timeout during endstop calibration" - elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FAILED_STAND: - return False, "Robot failed to stand" - elif ( - resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_CAMERA_TIMEOUT - ): - return False, "Timeout during camera check" - elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GROUND_CHECK: - return False, "Flat ground check failed" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_OFF_FAILURE - ): - return False, "Robot failed to power off" - elif ( - resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_REVERT_FAILURE - ): - return False, "Robot failed to revert calibration" - elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FGKC_FAILURE: - return False, "Robot failed to do flat ground kinematic calibration" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GRIPPER_CAL_TIMEOUT - ): - return False, "Timeout during gripper calibration" + if resp.error > 1: + return False, errorcode_mapping[resp.error] return True, "Successfully ran Spot Check" diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 305dfa33..daad4404 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -139,7 +139,7 @@ def navigation_close_loops( def optmize_anchoring(self) -> typing.Tuple[bool, str]: return self._optimize_anchoring() - ## Copied from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py + ## Copied from https://github.com/boston-dynamics/spot-sdk/blob/master/python/examples/graph_nav_command_line/recording_command_line.py and https://github.com/boston-dynamics/spot-sdk/blob/master/python/examples/graph_nav_command_line/graph_nav_command_line.py with minor modifications # Copyright (c) 2020 Boston Dynamics, Inc. All rights reserved. # # Downloading, reproducing, distributing or otherwise using the SDK Software @@ -610,58 +610,21 @@ def _optimize_anchoring(self, *args): return True, "Successfully optimized anchoring." else: self._logger.error("Error optimizing {}".format(response)) - if ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS - ): - return False, "Missing waypoint snapshots." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAPH - ): - return False, "Invalid graph." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_OPTIMIZATION_FAILURE - ): - return False, "Optimization failure." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_PARAMS - ): - return False, "Invalid parameters." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_CONSTRAINT_VIOLATION - ): - return False, "Constraint violation." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_ITERATIONS - ): - return False, "Max iterations, timeout." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_TIME - ): - return False, "Max time reached, timeout." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_HINTS - ): - return False, "Invalid hints." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING - ): - return False, "Map modified during processing." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT - ): - return False, "Invalid gravity alignment." - else: - return False, "Unknown error during anchoring optimization." + status_mapping = { + map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: "Missing waypoint snapshots.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAPH: "Invalid graph.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_OPTIMIZATION_FAILURE: "Optimization failure.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_PARAMS: "Invalid parameters.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_CONSTRAINT_VIOLATION: "Constraint violation.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_ITERATIONS: "Max iterations, timeout.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_TIME: "Max time reached, timeout.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_HINTS: "Invalid hints.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: "Map modified during processing.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT: "Invalid gravity alignment.", + } + if response.status in status_mapping: + return False, status_mapping[response.status] + return False, "Unknown error during anchoring optimization." def _id_to_short_code(self, id: str): """Convert a unique id to a 2 letter short code.""" diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 5722f641..dc83d99b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -341,8 +341,8 @@ def __init__( logger: logging.Logger, start_estop: bool = True, estop_timeout: float = 9.0, - rates: typing.Dict[str, float] = {}, - callbacks: typing.Dict[str, typing.Callable] = {}, + rates: typing.Optional[typing.Dict[str, float]] = None, + callbacks: typing.Optional[typing.Dict[str, typing.Callable]] = None, use_take_lease: bool = False, get_lease_on_action: bool = False, continually_try_stand: bool = True, @@ -370,8 +370,8 @@ def __init__( self._password = password self._hostname = hostname self._robot_name = robot_name - self._rates = rates - self._callbacks = callbacks + self._rates = rates or {} + self._callbacks = callbacks or {} self._use_take_lease = use_take_lease self._get_lease_on_action = get_lease_on_action self._continually_try_stand = continually_try_stand From 22d108496e0906e798e1324fabe113378cd8e806 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Sat, 22 Apr 2023 18:49:23 +0000 Subject: [PATCH 24/66] remove spot_config --- spot_wrapper/spot_arm.py | 8 ++++- spot_wrapper/spot_config.py | 47 ------------------------------ spot_wrapper/spot_eap.py | 3 +- spot_wrapper/spot_images.py | 27 ++++++++++++++++- spot_wrapper/spot_world_objects.py | 2 -- spot_wrapper/wrapper.py | 7 ++++- 6 files changed, 41 insertions(+), 53 deletions(-) delete mode 100644 spot_wrapper/spot_config.py diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 843a13fb..15e03a4b 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -19,7 +19,13 @@ from bosdyn.api import image_pb2 from google.protobuf.duration_pb2 import Duration -from .spot_config import * +"""List of hand image sources for asynchronous periodic query""" +HAND_IMAGE_SOURCES = [ + "hand_image", + "hand_depth", + "hand_color_image", + "hand_depth_in_hand_color_frame", +] class AsyncImageService(AsyncPeriodicQuery): diff --git a/spot_wrapper/spot_config.py b/spot_wrapper/spot_config.py deleted file mode 100644 index 0cd7c921..00000000 --- a/spot_wrapper/spot_config.py +++ /dev/null @@ -1,47 +0,0 @@ -""" -Global variables used for configuration. These will not change at runtime. -""" -from collections import namedtuple - -SPOT_CLIENT_NAME = "ros_spot" -MAX_COMMAND_DURATION = 1e5 - -"""Service name for getting pointcloud of VLP16 connected to Spot Core""" -VELODYNE_SERVICE_NAME = "velodyne-point-cloud" - -"""List of point cloud sources""" -point_cloud_sources = ["velodyne-point-cloud"] - -"""List of hand image sources for asynchronous periodic query""" -HAND_IMAGE_SOURCES = [ - "hand_image", - "hand_depth", - "hand_color_image", - "hand_depth_in_hand_color_frame", -] - -"""List of body image sources for periodic query""" -CAMERA_IMAGE_SOURCES = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "left_fisheye_image", - "right_fisheye_image", - "back_fisheye_image", -] -DEPTH_IMAGE_SOURCES = [ - "frontleft_depth", - "frontright_depth", - "left_depth", - "right_depth", - "back_depth", -] -DEPTH_REGISTERED_IMAGE_SOURCES = [ - "frontleft_depth_in_visual_frame", - "frontright_depth_in_visual_frame", - "right_depth_in_visual_frame", - "left_depth_in_visual_frame", - "back_depth_in_visual_frame", -] -ImageBundle = namedtuple( - "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] -) diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py index 1d05718d..05557655 100644 --- a/spot_wrapper/spot_eap.py +++ b/spot_wrapper/spot_eap.py @@ -6,7 +6,8 @@ from bosdyn.client.robot import Robot from bosdyn.client import robot_command -from .spot_config import * +"""List of point cloud sources""" +point_cloud_sources = ["velodyne-point-cloud"] class AsyncPointCloudService(AsyncPeriodicQuery): diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 6012e543..9d151f29 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,5 +1,6 @@ import typing import logging +from collections import namedtuple from bosdyn.client.robot import Robot from bosdyn.client.image import ( @@ -9,7 +10,31 @@ ) from bosdyn.api import image_pb2 -from .spot_config import * +"""List of body image sources for periodic query""" +CAMERA_IMAGE_SOURCES = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "left_fisheye_image", + "right_fisheye_image", + "back_fisheye_image", +] +DEPTH_IMAGE_SOURCES = [ + "frontleft_depth", + "frontright_depth", + "left_depth", + "right_depth", + "back_depth", +] +DEPTH_REGISTERED_IMAGE_SOURCES = [ + "frontleft_depth_in_visual_frame", + "frontright_depth_in_visual_frame", + "right_depth_in_visual_frame", + "left_depth_in_visual_frame", + "back_depth_in_visual_frame", +] +ImageBundle = namedtuple( + "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] +) class SpotImages: diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py index 90bbaa06..da7061a3 100644 --- a/spot_wrapper/spot_world_objects.py +++ b/spot_wrapper/spot_world_objects.py @@ -5,8 +5,6 @@ from bosdyn.client.robot import Robot from bosdyn.client.world_object import WorldObjectClient -from .spot_config import * - class AsyncWorldObjects(AsyncPeriodicQuery): """Class to get world objects. list_world_objects_async query sent to the robot at every tick. Callback registered to defined callback function. diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index dc83d99b..10d20eec 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -47,7 +47,12 @@ from .spot_graph_nav import SpotGraphNav from .spot_check import SpotCheck from .spot_images import SpotImages -from .spot_config import * + +SPOT_CLIENT_NAME = "ros_spot" +MAX_COMMAND_DURATION = 1e5 + +"""Service name for getting pointcloud of VLP16 connected to Spot Core""" +VELODYNE_SERVICE_NAME = "velodyne-point-cloud" class AsyncRobotState(AsyncPeriodicQuery): From 73d88bea581c140836885f423fe733c8e154bfb6 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 30 May 2023 13:55:51 +0100 Subject: [PATCH 25/66] use fstring for short code, move wrench from msg function to class body --- spot_wrapper/spot_arm.py | 29 ++++++++++++++--------------- spot_wrapper/spot_graph_nav.py | 2 +- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 15e03a4b..2a687833 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -38,7 +38,7 @@ class AsyncImageService(AsyncPeriodicQuery): callback: Callback function to call when the results of the query are available """ - def __init__(self, client, logger, rate, callback, image_requests): + def __init__(self, client, logger , rate, callback, image_requests): super(AsyncImageService, self).__init__( "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) ) @@ -237,27 +237,27 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0] if abs(joint_targets[0]) > 3.14: msg = "Joint 1 has to be between -3.14 and 3.14" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13: msg = "Joint 2 has to be between -3.13 and 0.4" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0: msg = "Joint 3 has to be between 0.0 and 3.14" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif abs(joint_targets[3]) > 2.79253: msg = "Joint 4 has to be between -2.79253 and 2.79253" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif abs(joint_targets[4]) > 1.8326: msg = "Joint 5 has to be between -1.8326 and 1.8326" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif abs(joint_targets[5]) > 2.87: msg = "Joint 6 has to be between -2.87 and 2.87" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg try: success, msg = self.ensure_arm_power_and_stand() @@ -300,6 +300,11 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: except Exception as e: return False, "Exception occured during arm movement: " + str(e) + def create_wrench_from_msg(self, forces, torques): + force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) + torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) + return geometry_pb2.Wrench(force=force, torque=torque) + def force_trajectory(self, data) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() @@ -307,24 +312,18 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: self._logger.info(msg) return False, msg else: - - def create_wrench_from_msg(forces, torques): - force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) - torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) - return geometry_pb2.Wrench(force=force, torque=torque) - # Duration in seconds. traj_duration = data.duration # first point on trajectory - wrench0 = create_wrench_from_msg(data.forces_pt0, data.torques_pt0) + wrench0 = self.create_wrench_from_msg(data.forces_pt0, data.torques_pt0) t0 = seconds_to_duration(0) traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench0, time_since_reference=t0 ) # Second point on the trajectory - wrench1 = create_wrench_from_msg(data.forces_pt1, data.torques_pt1) + wrench1 = self.create_wrench_from_msg(data.forces_pt1, data.torques_pt1) t1 = seconds_to_duration(traj_duration) traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench1, time_since_reference=t1 diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index daad4404..1c39d528 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -630,7 +630,7 @@ def _id_to_short_code(self, id: str): """Convert a unique id to a 2 letter short code.""" tokens = id.split("-") if len(tokens) > 2: - return "%c%c" % (tokens[0][0], tokens[1][0]) + return f"{tokens[0][0]:c}, {tokens[1][0]:c}" return None def _pretty_print_waypoints( From c473eefef7da5b1c75522a6bf441875918829a9c Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 30 May 2023 16:33:54 +0100 Subject: [PATCH 26/66] Add changes from [SW-62] Elements for publishing the hand camera in spot_ros2 (#7) Co-authored-by: Andrew Messing <129519955+amessing-bdai@users.noreply.github.com> --- spot_wrapper/spot_images.py | 81 +++++++++++++++++++++++++++++++------ 1 file changed, 69 insertions(+), 12 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 9d151f29..30a45165 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -35,6 +35,9 @@ ImageBundle = namedtuple( "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] ) +ImageWithHandBundle = namedtuple( + "ImageBundle", ["frontleft", "frontright", "left", "right", "back", "hand"] +) class SpotImages: @@ -76,6 +79,28 @@ def __init__( ) ) + if self._robot.has_arm(): + self._camera_image_requests.append( + build_image_request( + "hand_color_image", + image_format=image_pb2.Image.FORMAT_JPEG, + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ) + self._depth_image_requests.append( + build_image_request( + "hand_depth", + pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, + ) + ) + self._depth_registered_image_requests.append( + build_image_request( + "hand_depth_in_hand_color_frame", + pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, + ) + ) + def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( @@ -143,27 +168,59 @@ def get_back_rgb_image(self) -> image_pb2.ImageResponse: self._logger.error(e) return None + def get_hand_rgb_image(self): + if not self._robot.has_arm(): + return None + try: + return self._image_client.get_image( + [ + build_image_request( + "hand_color_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + def get_images( self, image_requests: typing.List[image_pb2.ImageRequest] - ) -> typing.Optional[ImageBundle]: + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: try: image_responses = self._image_client.get_image(image_requests) except UnsupportedPixelFormatRequestedError as e: self._logger.error(e) return None - return ImageBundle( - frontleft=image_responses[0], - frontright=image_responses[1], - left=image_responses[2], - right=image_responses[3], - back=image_responses[4], - ) - - def get_camera_images(self) -> ImageBundle: + if self._robot.has_arm(): + return ImageWithHandBundle( + frontleft=image_responses[0], + frontright=image_responses[1], + left=image_responses[2], + right=image_responses[3], + back=image_responses[4], + hand=image_responses[5], + ) + else: + return ImageBundle( + frontleft=image_responses[0], + frontright=image_responses[1], + left=image_responses[2], + right=image_responses[3], + back=image_responses[4], + ) + + def get_camera_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: return self.get_images(self._camera_image_requests) - def get_depth_images(self) -> ImageBundle: + def get_depth_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: return self.get_images(self._depth_image_requests) - def get_depth_registered_images(self) -> ImageBundle: + def get_depth_registered_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: return self.get_images(self._depth_registered_image_requests) From 8963fb7a842205af60babb8bf681ed3d51dfd03b Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 28 Apr 2023 14:15:12 +0100 Subject: [PATCH 27/66] Wrapper for spot cam interaction (#4) * simple led brightness control, only able to set all leds to same value * Add power control, but unclear if it is actually possible to set power for aux and external mic * most basic functional image stream publisher with webrtc * add compositor to handle IR and webrtc stream selection with services Add timestamp for the webrtc images Add compressed version of the webrtc image stream * Add health wrapper, move body of robotToLocalTime out of spot wrapper object robotToLocalTime now takes the timestamp and a robot object, which allows it to be used by the spot cam wrapper as well. * add handler and wrapper for audio commands * update webrtc_client to 3.2 version * add stream quality wrapper and ros handler * initial implementation of ptz wrapper and handler, can list ptzs * ptz handler publishes position and velocity of ptzs, can set position and velocity * add egg info to gitignore --- .gitignore | 1 + spot_wrapper/cam_webrtc_client.py | 137 ++++++ spot_wrapper/cam_wrapper.py | 678 ++++++++++++++++++++++++++++++ spot_wrapper/wrapper.py | 133 ++++-- 4 files changed, 906 insertions(+), 43 deletions(-) create mode 100644 spot_wrapper/cam_webrtc_client.py create mode 100644 spot_wrapper/cam_wrapper.py diff --git a/.gitignore b/.gitignore index c3c89839..27dd47d1 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ *.pyo *.orig build +spot_wrapper.egg-info diff --git a/spot_wrapper/cam_webrtc_client.py b/spot_wrapper/cam_webrtc_client.py new file mode 100644 index 00000000..d79aeb1b --- /dev/null +++ b/spot_wrapper/cam_webrtc_client.py @@ -0,0 +1,137 @@ +# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +import asyncio +import base64 + +import requests +from aiortc import MediaStreamTrack, RTCPeerConnection, RTCSessionDescription +from aiortc.contrib.media import MediaBlackhole + + +class SpotCAMMediaStreamTrack(MediaStreamTrack): + def __init__(self, track, queue): + super().__init__() + self.track = track + self.queue = queue + + async def recv(self): + frame = await self.track.recv() + await self.queue.put(frame) + + return frame + + +class WebRTCClient: + def __init__( + self, + hostname, + sdp_port, + sdp_filename, + cam_ssl_cert, + token, + rtc_config, + media_recorder=None, + recorder_type=None, + ): + self.pc = RTCPeerConnection(configuration=rtc_config) + + self.video_frame_queue = asyncio.Queue() + self.audio_frame_queue = asyncio.Queue() + + self.hostname = hostname + self.token = token + self.sdp_port = sdp_port + self.media_recorder = media_recorder + self.media_black_hole = None + self.recorder_type = recorder_type + self.sdp_filename = sdp_filename + self.cam_ssl_cert = cam_ssl_cert + + def get_bearer_token(self, mock=False): + if mock: + return "token" + return self.token + + def get_sdp_offer_from_spot_cam(self, token): + # then made the sdp request with the token + headers = {"Authorization": f"Bearer {token}"} + server_url = f"https://{self.hostname}:{self.sdp_port}/{self.sdp_filename}" + response = requests.get(server_url, verify=self.cam_ssl_cert, headers=headers) + result = response.json() + return result["id"], base64.b64decode(result["sdp"]).decode() + + def send_sdp_answer_to_spot_cam(self, token, offer_id, sdp_answer): + headers = {"Authorization": f"Bearer {token}"} + server_url = f"https://{self.hostname}:{self.sdp_port}/{self.sdp_filename}" + + payload = {"id": offer_id, "sdp": base64.b64encode(sdp_answer).decode("utf8")} + r = requests.post( + server_url, verify=self.cam_ssl_cert, json=payload, headers=headers + ) + if r.status_code != 200: + raise ValueError(r) + + async def start(self): + # first get a token + try: + token = self.get_bearer_token() + except: + token = self.get_bearer_token(mock=True) + + offer_id, sdp_offer = self.get_sdp_offer_from_spot_cam(token) + + @self.pc.on("icegatheringstatechange") + def _on_ice_gathering_state_change(): + print(f"ICE gathering state changed to {self.pc.iceGatheringState}") + + @self.pc.on("signalingstatechange") + def _on_signaling_state_change(): + print(f"Signaling state changed to: {self.pc.signalingState}") + + @self.pc.on("icecandidate") + def _on_ice_candidate(event): + print(f"Received candidate: {event.candidate}") + + @self.pc.on("iceconnectionstatechange") + async def _on_ice_connection_state_change(): + print(f"ICE connection state changed to: {self.pc.iceConnectionState}") + + if self.pc.iceConnectionState == "checking": + self.send_sdp_answer_to_spot_cam( + token, offer_id, self.pc.localDescription.sdp.encode() + ) + + @self.pc.on("track") + def _on_track(track): + print(f"Received track: {track.kind}") + + if self.media_recorder: + if track.kind == self.recorder_type: + self.media_recorder.addTrack(track) + else: + # We only care about the track we are recording. + self.media_black_hole = MediaBlackhole() + self.media_black_hole.addTrack(track) + loop = asyncio.get_event_loop() + loop.create_task(self.media_black_hole.start()) + else: + if track.kind == "video": + video_track = SpotCAMMediaStreamTrack(track, self.video_frame_queue) + video_track.kind = "video" + self.pc.addTrack(video_track) + + if track.kind == "audio": + self.media_recorder = MediaBlackhole() + self.media_recorder.addTrack(track) + loop = asyncio.get_event_loop() + loop.create_task(self.media_recorder.start()) + + desc = RTCSessionDescription(sdp_offer, "offer") + await self.pc.setRemoteDescription(desc) + + sdp_answer = await self.pc.createAnswer() + await self.pc.setLocalDescription(sdp_answer) diff --git a/spot_wrapper/cam_wrapper.py b/spot_wrapper/cam_wrapper.py new file mode 100644 index 00000000..00460422 --- /dev/null +++ b/spot_wrapper/cam_wrapper.py @@ -0,0 +1,678 @@ +import asyncio +import datetime +import enum +import os.path +import threading +import typing +import wave + +import bosdyn.client +import cv2 +import numpy as np +from aiortc import RTCConfiguration +from bosdyn.client import Robot +from bosdyn.client import spot_cam +from bosdyn.client.spot_cam.compositor import CompositorClient +from bosdyn.client.spot_cam.lighting import LightingClient +from bosdyn.client.spot_cam.power import PowerClient +from bosdyn.client.spot_cam.health import HealthClient +from bosdyn.client.spot_cam.audio import AudioClient +from bosdyn.client.spot_cam.streamquality import StreamQualityClient +from bosdyn.client.spot_cam.ptz import PtzClient +from bosdyn.client.spot_cam.media_log import MediaLogClient +from bosdyn.client.payload import PayloadClient +from bosdyn.api.spot_cam.ptz_pb2 import PtzDescription, PtzVelocity, PtzPosition +from bosdyn.api.spot_cam import audio_pb2 + +from spot_wrapper.cam_webrtc_client import WebRTCClient +from spot_wrapper.wrapper import SpotWrapper + + +class LightingWrapper: + """ + Wrapper for LED brightness interaction + """ + + class LEDPosition(enum.Enum): + """ + Values indicate the position of the specified LED in the brightness list + """ + + REAR_LEFT = 0 + FRONT_LEFT = 1 + FRONT_RIGHT = 2 + REAR_RIGHT = 3 + + def __init__(self, robot: Robot, logger): + self.logger = logger + self.client: LightingClient = robot.ensure_client( + LightingClient.default_service_name + ) + + def set_led_brightness(self, brightness): + """ + Set the brightness of the LEDs to the specified brightness + + Args: + brightness: LEDs will all be set to this brightness, which should be in the range [0, 1]. The value will + be clipped if outside this range. + """ + # Clamp brightness to [0,1] range + brightness = min(max(brightness, 0), 1) + self.client.set_led_brightness([brightness] * 4) + + def get_led_brightness(self) -> typing.List[float]: + """ + Get the brightness of the LEDS + + Returns: + List of floats indicating current brightness of each LED, in the order they are specified in the + LEDPosition enum + """ + return self.client.get_led_brightness() + + +class PowerWrapper: + """ + Wrapper for power interaction + """ + + def __init__(self, robot: Robot, logger): + self.logger = logger + self.client: PowerClient = robot.ensure_client(PowerClient.default_service_name) + + def get_power_status(self): + """ + Get power status for the devices + """ + return self.client.get_power_status() + + def set_power_status( + self, + ptz: typing.Optional[bool] = None, + aux1: typing.Optional[bool] = None, + aux2: typing.Optional[bool] = None, + external_mic: typing.Optional[bool] = None, + ): + """ + Set power status for each of the devices + + Args: + ptz: + aux1: ?? + aux2: ?? + external_mic: + """ + self.client.set_power_status(ptz, aux1, aux2, external_mic) + + def cycle_power( + self, + ptz: typing.Optional[bool] = None, + aux1: typing.Optional[bool] = None, + aux2: typing.Optional[bool] = None, + external_mic: typing.Optional[bool] = None, + ): + """ + Cycle power of the specified devices + + Args: + ptz: + aux1: + aux2: + external_mic: + """ + self.client.cycle_power(ptz, aux1, aux2, external_mic) + + +class CompositorWrapper: + """ + Wrapper for compositor interaction + """ + + def __init__(self, robot: Robot, logger): + self.logger = logger + self.client: CompositorClient = robot.ensure_client( + CompositorClient.default_service_name + ) + + def list_screens(self) -> typing.List[str]: + """ + List the available screens - this includes individual cameras and also panoramic and other stitched images + provided by the camera + + Returns: + List of strings indicating available screens + """ + return [screen.name for screen in self.client.list_screens()] + + def get_visible_cameras(self): + """ + Get the camera data for the camera currently visible on the stream + + Returns: + List of visible camera streams + """ + return self.client.get_visible_cameras() + + def set_screen(self, screen): + """ + Set the screen to be streamed over the network + + Args: + screen: Screen to show + """ + self.client.set_screen(screen) + + def set_ir_colormap(self, colormap, min_temp, max_temp, auto_scale=True): + """ + Set the colormap used for the IR camera + + Args: + colormap: Colormap to use, options are https://dev.bostondynamics.com/protos/bosdyn/api/proto_reference#ircolormap-colormap + min_temp: Minimum temperature on the scale + max_temp: Maximum temperature on the scale + auto_scale: Auto-scales the colormap according to the image. If this is set min_temp and max_temp are + ignored + """ + self.client.set_ir_colormap(colormap, min_temp, max_temp, auto_scale) + + def set_ir_meter_overlay(self, x, y, enable=True): + """ + Set the reticle position on the Spot CAM IR. + + Args: + x: Horizontal coordinate between 0 and 1 + y: Vertical coordinate between 0 and 1 + enable: If true, enable the reticle on the display + """ + self.client.set_ir_meter_overlay(x, y, enable) + + +class HealthWrapper: + """ + Wrapper for health details + """ + + def __init__(self, robot, logger): + self.client: HealthClient = robot.ensure_client( + HealthClient.default_service_name + ) + self.logger = logger + + def get_bit_status( + self, + ) -> typing.Tuple[typing.List[str], typing.List[typing.Tuple[int, str]]]: + """ + Get fault events and degradations + + Returns: + Dictionary + + """ + bit_status = self.client.get_bit_status() + events = [] + for event in bit_status[0]: + events.append(event) + + degradations = [] + for degradation in bit_status[1]: + degradations.append((degradation.type, degradation.description)) + return events, degradations + + def get_temperature(self) -> typing.Tuple[str, float]: + """ + Get temperatures of various components of the camera + + Returns: + Tuple of string and float indicating the component and its temperature in celsius + """ + return [ + (composite.channel_name, composite.temperature / 1000.0) + for composite in self.client.get_temperature() + ] + + # def get_system_log(self): + # """ + # This seems to always time out + # """ + # return self.client.get_system_log() + + +class AudioWrapper: + """ + Wrapper for audio commands on the camera + """ + + def __init__(self, robot, logger): + self.client: AudioClient = robot.ensure_client(AudioClient.default_service_name) + self.logger = logger + + def list_sounds(self) -> typing.List[str]: + """ + List sounds available on the device + + Returns: + List of names of available sounds + """ + return self.client.list_sounds() + + def set_volume(self, percentage): + """ + Set the volume at which sounds should be played + + Args: + percentage: How loud sounds should be from 0 to 100% + """ + self.client.set_volume(percentage) + + def get_volume(self): + """ + Get the current volume at which sounds are played + + Returns: + Current volume as a percentage + """ + return self.client.get_volume() + + def play_sound(self, sound_name, gain=1.0): + """ + Play a sound which is on the device + + Args: + sound_name: Name of the sound to play + gain: Volume gain multiplier + """ + sound = audio_pb2.Sound(name=sound_name) + self.client.play_sound(sound, gain) + + def load_sound(self, sound_file, name): + """ + Load a sound from a wav file and save it with the given name onto the device + Args: + sound_file: Wav file to read from + name: Name to assign to the sound + + Raises: + IOError: If the given file is not a file + wave.Error: If the given file is not a wav file + """ + full_path = os.path.abspath(os.path.expanduser(sound_file)) + print(full_path) + if not os.path.isfile(full_path): + raise IOError(f"Tried to load sound from {full_path} but it is not a file.") + + sound = audio_pb2.Sound(name=name) + + with wave.open(full_path, "rb") as fh: + # Use this to make sure that the file is actually a wav file + pass + + with open(full_path, "rb") as fh: + data = fh.read() + + self.client.load_sound(sound, data) + + def delete_sound(self, name): + """ + Delete a sound from the device + + Args: + name: Name of the sound to delete + """ + self.client.delete_sound(audio_pb2.Sound(name=name)) + + +class StreamQualityWrapper: + """ + Wrapper for stream quality commands + """ + + def __init__(self, robot, logger): + self.client: StreamQualityClient = robot.ensure_client( + StreamQualityClient.default_service_name + ) + self.logger = logger + + def set_stream_params(self, target_bitrate, refresh_interval, idr_interval, awb): + """ + Set image compression and postprocessing parameters + + Note: It is currently not possible to turn off the auto white balance. You will get a crash + + Args: + target_bitrate: Compression level target in bits per second + refresh_interval: How often the whole feed should be refreshed (in frames) + idr_interval: How often an IDR message should be sent (in frames) + awb: Mode for automatic white balance + """ + self.client.set_stream_params(target_bitrate, refresh_interval, idr_interval, 0) + + def get_stream_params(self) -> typing.Dict[str, int]: + """ + Get the current stream quality parameters + + Returns: + Dictionary containing the parameters + """ + params = self.client.get_stream_params() + param_dict = { + "target_bitrate": params.targetbitrate.value, + "refresh_interval": params.refreshinterval.value, + "idr_interval": params.idrinterval.value, + "awb": params.awb.awb, + } + + return param_dict + + def enable_congestion_control(self, enable): + """ + Enable congestion control on the receiver... not sure what this does + + Args: + enable: If true, enable congestion control + """ + self.client.enable_congestion_control(enable) + + +class MediaLogWrapper: + """ + Wrapper for interacting with the media log. And importantly, information about the cameras themselves + """ + + def __init__(self, robot, logger): + self.client: MediaLogClient = robot.ensure_client( + MediaLogClient.default_service_name + ) + self.logger = logger + + def list_cameras(self) -> typing.List: + """ + List the cameras on the spot cam + """ + return self.client.list_cameras() + + +class PTZWrapper: + """ + Wrapper for controlling the PTZ unit + """ + + def __init__(self, robot, logger): + self.client: PtzClient = robot.ensure_client(PtzClient.default_service_name) + self.logger = logger + self.ptzs = {} + descriptions = self.client.list_ptz() + for description in descriptions: + self.ptzs[description.name] = description + + def list_ptz(self) -> typing.Dict[str, typing.Dict]: + """ + List the available ptz units on the device + + Returns: + Dict of descriptions of ptz units + """ + ptzs = [] + + descriptions = self.client.list_ptz() + for ptz_desc in descriptions: + ptzs.append(ptz_desc) + # Also update the internal list of raw ptz definitions + self.ptzs[ptz_desc.name] = ptz_desc + + return ptzs + + def _get_ptz_description(self, name): + """ + Get the bosdyn version of the ptz description + + Args: + name: Get description for this ptz + + Returns: + PtzDescription + """ + if name not in self.ptzs: + self.logger.warn( + f"Tried to retrieve description for ptz {name} but it does not exist." + ) + return None + + return self.ptzs[name] + + def _clamp_value_to_limits(self, value, limits: PtzDescription.Limits): + """ + Clamp the given value to the specified limits. If the limits are unspecified (i.e. both 0), the value is not + clamped + + Args: + value: Value to clamp + limits: PTZ description limit proto + + Returns: + Value clamped to limits + """ + if limits.max.value == 0 and limits.min.value == 0: + # If both max and min are zero, this means the limit is unset. The documentation states that if a limit + # is unset, then all positions are valid. + # https://dev.bostondynamics.com/protos/bosdyn/api/proto_reference#ptzdescription + return value + + return max(min(value, limits.max.value), limits.min.value) + + def _clamp_request_to_limits( + self, ptz_name, pan, tilt, zoom + ) -> typing.Tuple[float, float, float]: + """ + + Args: + ptz_name: Name of the ptz for which the pan, tilt, and zoom should be clamped + + Returns: + Tuple of pan, tilt, zoom, clamped to the limits of the requested ptz + """ + ptz_desc = self._get_ptz_description(ptz_name) + + return ( + self._clamp_value_to_limits(pan, ptz_desc.pan_limit), + self._clamp_value_to_limits(tilt, ptz_desc.tilt_limit), + self._clamp_value_to_limits(zoom, ptz_desc.zoom_limit), + ) + + def get_ptz_position(self, ptz_name) -> PtzPosition: + """ + Get the position of the ptz with the given name + + Args: + ptz_name: Name of the ptz + + Returns: + ptz position proto + """ + return self.client.get_ptz_position(PtzDescription(name=ptz_name)) + + def set_ptz_position(self, ptz_name, pan, tilt, zoom): + """ + Set the position of the specified ptz + + Args: + ptz_name: Name of the ptz + pan: Set the pan to this value in degrees + tilt: Set the tilt to this value in degrees + zoom: Set the zoom to this zoom level + """ + pan, tilt, zoom = self._clamp_request_to_limits(ptz_name, pan, tilt, zoom) + self.client.set_ptz_position( + self._get_ptz_description(ptz_name), pan, tilt, zoom + ) + + def get_ptz_velocity(self, ptz_name) -> PtzVelocity: + """ + Get the velocity of the ptz with the given name + + Args: + ptz_name: Name of the ptz + + Returns: + ptz velocity proto + """ + return self.client.get_ptz_velocity(PtzDescription(name=ptz_name)) + + def set_ptz_velocity(self, ptz_name, pan, tilt, zoom): + """ + Set the velocity of the various axes of the specified ptz + + Args: + ptz_name: Name of the ptz + pan: Set the pan to this value in degrees per second + tilt: Set the tilt to this value in degrees per second + zoom: Set the zoom to this value in zoom level per second + """ + # We do not clamp the velocity to the limits, as it is a rate + self.client.set_ptz_velocity( + self._get_ptz_description(ptz_name), pan, tilt, zoom + ) + + def initialise_lens(self): + """ + Initialises or resets ptz autofocus + """ + self.client.initialize_lens() + + +class ImageStreamWrapper: + """ + A wrapper for the image stream from WebRTC + + Can view the same stream at https://192.168.50.3:31102/h264.sdp.html (depending on the IP of the robot) + + Contains functions adapted from + https://github.com/boston-dynamics/spot-sdk/blob/master/python/examples/spot_cam/webrtc.py + """ + + def __init__( + self, + hostname: str, + robot, + logger, + sdp_port=31102, + sdp_filename="h264.sdp", + cam_ssl_cert_path=None, + ): + """ + Initialise the wrapper + + Args: + hostname: Hostname/IP of the robot + robot: Handle for the robot the camera is on + logger: Logger to use + sdp_port: SDP port of Spot's WebRTC server + sdp_filename: File being streamed from the WebRTC server + cam_ssl_cert_path: Path to the Spot CAM's client cert to check with Spot CAM server + """ + self.shutdown_flag = threading.Event() + self.logger = logger + self.last_image_time = None + self.image_lock = threading.Lock() + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + + config = RTCConfiguration(iceServers=[]) + self.client = WebRTCClient( + hostname, + sdp_port, + sdp_filename, + cam_ssl_cert_path if cam_ssl_cert_path else False, + robot.user_token, + config, + ) + + asyncio.gather( + self.client.start(), + self._process_func(), + self._monitor_shutdown(), + ) + # Put the async loop into a separate thread so we can continue initialisation + self.async_thread = threading.Thread(target=loop.run_forever) + self.async_thread.start() + + async def _monitor_shutdown(self): + while not self.shutdown_flag.is_set(): + await asyncio.sleep(1.0) + + self.logger.info("Image stream wrapper received shutdown flag") + await self.client.pc.close() + asyncio.get_event_loop().stop() + + async def _process_func(self): + while asyncio.get_event_loop().is_running(): + try: + frame = await self.client.video_frame_queue.get() + + pil_image = frame.to_image() + cv_image = np.array(pil_image) + # OpenCV needs BGR + cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) + with self.image_lock: + self.last_image_time = datetime.datetime.now() + self.last_image = cv_image + except Exception as e: + self.logger.error(f"Image stream wrapper exception {e}") + try: + # discard audio frames + while not self.client.audio_frame_queue.empty(): + await self.client.audio_frame_queue.get() + except Exception as e: + self.logger.error( + f"Image stream wrapper exception while discarding audio frames {e}" + ) + + self.shutdown_flag.set() + + +class SpotCamWrapper: + def __init__(self, hostname, username, password, logger): + self._hostname = hostname + self._username = username + self._password = password + self._logger = logger + + # Create robot object and authenticate. + self.sdk = bosdyn.client.create_standard_sdk("Spot CAM Client") + spot_cam.register_all_service_clients(self.sdk) + + self.robot = self.sdk.create_robot(self._hostname) + SpotWrapper.authenticate( + self.robot, self._username, self._password, self._logger + ) + + self.payload_client: PayloadClient = self.robot.ensure_client( + PayloadClient.default_service_name + ) + self.payload_details = None + for payload in self.payload_client.list_payloads(): + if payload.is_enabled and "Spot CAM" in payload.name: + self.payload_details = payload + + if not self.payload_details: + raise SystemError( + "Expected an enabled payload with Spot CAM in the name. This does not appear to exist. " + "Please verify that the spot cam is correctly configured in the payload list on the " + "admin interface" + ) + + self.lighting = LightingWrapper(self.robot, self._logger) + self.power = PowerWrapper(self.robot, self._logger) + self.compositor = CompositorWrapper(self.robot, self._logger) + self.image = ImageStreamWrapper(self._hostname, self.robot, self._logger) + self.health = HealthWrapper(self.robot, self._logger) + self.audio = AudioWrapper(self.robot, self._logger) + self.stream_quality = StreamQualityWrapper(self.robot, self._logger) + self.media_log = MediaLogWrapper(self.robot, self._logger) + self.ptz = PTZWrapper(self.robot, self._logger) + + self._logger.info("Finished setting up spot cam wrapper components") + + def shutdown(self): + self._logger.info("Shutting down Spot CAM wrapper") + self.image.shutdown_flag.set() diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 10d20eec..9851db4b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -55,6 +55,32 @@ VELODYNE_SERVICE_NAME = "velodyne-point-cloud" +def robotToLocalTime(timestamp, robot): + """Takes a timestamp and an estimated skew and return seconds and nano seconds in local time + + Args: + timestamp: google.protobuf.Timestamp + robot: Robot handle to use to get the time skew + Returns: + google.protobuf.Timestamp + """ + + rtime = Timestamp() + + rtime.seconds = timestamp.seconds - robot.time_sync.endpoint.clock_skew.seconds + rtime.nanos = timestamp.nanos - robot.time_sync.endpoint.clock_skew.nanos + if rtime.nanos < 0: + rtime.nanos = rtime.nanos + 1000000000 + rtime.seconds = rtime.seconds - 1 + + # Workaround for timestamps being incomplete + if rtime.seconds < 0: + rtime.seconds = 0 + rtime.nanos = 0 + + return rtime + + class AsyncRobotState(AsyncPeriodicQuery): """Class to get robot state at regular intervals. get_robot_state_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -415,34 +441,12 @@ def __init__( self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) - authenticated = False - while not authenticated: - try: - self._logger.info("Trying to authenticate with robot...") - self._robot.authenticate(self._username, self._password) - self._robot.time_sync.wait_for_sync(10) - self._logger.info("Successfully authenticated.") - authenticated = True - except RpcError as err: - sleep_secs = 15 - self._logger.warning( - "Failed to communicate with robot: {}\nEnsure the robot is powered on and you can " - "ping {}. Robot may still be booting. Will retry in {} seconds".format( - err, self._hostname, sleep_secs - ) - ) - time.sleep(sleep_secs) - except bosdyn.client.auth.InvalidLoginError as err: - self._logger.error("Failed to log in to robot: {}".format(err)) - self._valid = False - return - try: - self._point_cloud_client = self._robot.ensure_client( - VELODYNE_SERVICE_NAME - ) - except Exception as e: - self._point_cloud_client = None - self._logger.warning("No point cloud services are available.") + authenticated = self.authenticate( + self._robot, self._username, self._password, self._logger + ) + if not authenticated: + self._valid = False + return if not self._robot: self._logger.error("Failed to create robot object") @@ -488,6 +492,15 @@ def __init__( self._spot_check_client = self._robot.ensure_client( SpotCheckClient.default_service_name ) + try: + self._point_cloud_client = self._robot.ensure_client( + VELODYNE_SERVICE_NAME + ) + except Exception as e: + self._point_cloud_client = None + self._logger.info("No point cloud services are available.") + + if self._robot.has_arm(): self._manipulation_client = self._robot.ensure_client( ManipulationApiClient.default_service_name @@ -617,6 +630,54 @@ def __init__( self._async_tasks = AsyncTasks(robot_tasks) + self.camera_task_name_to_task_mapping = { + "hand_image": self._hand_image_task, + "side_image": self._side_image_task, + "rear_image": self._rear_image_task, + "front_image": self._front_image_task, + } + + self._robot_id = None + self._lease = None + + @staticmethod + def authenticate(robot, username, password, logger): + """ + Authenticate with a robot through the bosdyn API. A blocking function which will wait until authenticated (if + the robot is still booting) or login fails + + Args: + robot: Robot object which we are authenticating with + username: Username to authenticate with + password: Password for the given username + logger: Logger with which to print messages + + Returns: + + """ + authenticated = False + while not authenticated: + try: + logger.info("Trying to authenticate with robot...") + robot.authenticate(username, password) + robot.time_sync.wait_for_sync(10) + logger.info("Successfully authenticated.") + authenticated = True + except RpcError as err: + sleep_secs = 15 + logger.warn( + "Failed to communicate with robot: {}\nEnsure the robot is powered on and you can " + "ping {}. Robot may still be booting. Will retry in {} seconds".format( + err, robot.address, sleep_secs + ) + ) + time.sleep(sleep_secs) + except bosdyn.client.auth.InvalidLoginError as err: + logger.error("Failed to log in to robot: {}".format(err)) + raise err + + return authenticated + @property def robot_name(self) -> str: return self._robot_name @@ -754,21 +815,7 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: Returns: google.protobuf.Timestamp """ - - rtime = Timestamp() - - rtime.seconds = timestamp.seconds - self.time_skew.seconds - rtime.nanos = timestamp.nanos - self.time_skew.nanos - if rtime.nanos < 0: - rtime.nanos = rtime.nanos + 1000000000 - rtime.seconds = rtime.seconds - 1 - - # Workaround for timestamps being incomplete - if rtime.seconds < 0: - rtime.seconds = 0 - rtime.nanos = 0 - - return rtime + return robotToLocalTime(timestamp, self._robot) def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" From 9b20fe1e997fb2fa4d3036abfb77f13250149602 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 11:31:52 +0100 Subject: [PATCH 28/66] fix bad indent after cherry-pick --- spot_wrapper/wrapper.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9851db4b..b7a7a066 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -630,15 +630,15 @@ def __init__( self._async_tasks = AsyncTasks(robot_tasks) - self.camera_task_name_to_task_mapping = { - "hand_image": self._hand_image_task, - "side_image": self._side_image_task, - "rear_image": self._rear_image_task, - "front_image": self._front_image_task, - } - - self._robot_id = None - self._lease = None + self.camera_task_name_to_task_mapping = { + "hand_image": self._hand_image_task, + "side_image": self._side_image_task, + "rear_image": self._rear_image_task, + "front_image": self._front_image_task, + } + + self._robot_id = None + self._lease = None @staticmethod def authenticate(robot, username, password, logger): From 2ea754a245bf09990663353c01ba7495681ebada Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 11:32:58 +0100 Subject: [PATCH 29/66] fix formatting --- spot_wrapper/spot_arm.py | 2 +- spot_wrapper/wrapper.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 2a687833..eb61736e 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -38,7 +38,7 @@ class AsyncImageService(AsyncPeriodicQuery): callback: Callback function to call when the results of the query are available """ - def __init__(self, client, logger , rate, callback, image_requests): + def __init__(self, client, logger, rate, callback, image_requests): super(AsyncImageService, self).__init__( "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) ) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index b7a7a066..e63b6e13 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -500,7 +500,6 @@ def __init__( self._point_cloud_client = None self._logger.info("No point cloud services are available.") - if self._robot.has_arm(): self._manipulation_client = self._robot.ensure_client( ManipulationApiClient.default_service_name From e913cba3d47062e355deb8e44b65a90c7bf6e7fc Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 11:42:09 +0100 Subject: [PATCH 30/66] fix short code conversion --- spot_wrapper/spot_graph_nav.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 1c39d528..2317ad57 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -630,7 +630,7 @@ def _id_to_short_code(self, id: str): """Convert a unique id to a 2 letter short code.""" tokens = id.split("-") if len(tokens) > 2: - return f"{tokens[0][0]:c}, {tokens[1][0]:c}" + return f"{tokens[0][0]}{tokens[1][0]}" return None def _pretty_print_waypoints( From b5aa38265aa41deb047cdf11f92019c3e4f91755 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 28 Apr 2023 15:00:07 +0100 Subject: [PATCH 31/66] Always include exception message in response strings (#8) --- spot_wrapper/spot_arm.py | 22 +++++++++++----------- spot_wrapper/spot_docking.py | 4 ++-- spot_wrapper/wrapper.py | 18 ++++++++---------- 3 files changed, 21 insertions(+), 23 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index eb61736e..5e50d6a6 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -127,7 +127,7 @@ def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: except Exception as e: return ( False, - "Exception occured while Spot or its arm were trying to power on", + f"Exception occured while Spot or its arm were trying to power on: {e}", ) if not self._robot_params["is_standing"]: @@ -156,7 +156,7 @@ def arm_stow(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while trying to stow" + return False, f"Exception occured while trying to stow: {e}" return True, "Stow arm success" @@ -176,7 +176,7 @@ def arm_unstow(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while trying to unstow" + return False, f"Exception occured while trying to unstow: {e}" return True, "Unstow arm success" @@ -196,7 +196,7 @@ def arm_carry(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while carry mode was issued" + return False, f"Exception occured while carry mode was issued: {e}" return True, "Carry mode success" @@ -298,7 +298,7 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: return True, "Spot Arm moved successfully" except Exception as e: - return False, "Exception occured during arm movement: " + str(e) + return False, f"Exception occured during arm movement: {e}" def create_wrench_from_msg(self, forces, torques): force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) @@ -364,7 +364,7 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: time.sleep(float(traj_duration) + 1.0) except Exception as e: - return False, "Exception occured during arm movement" + return False, f"Exception occured during arm movement: {e}" return True, "Moved arm successfully" @@ -384,7 +384,7 @@ def gripper_open(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while gripper was moving" + return False, f"Exception occured while gripper was moving: {e}" return True, "Open gripper success" @@ -404,7 +404,7 @@ def gripper_close(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while gripper was moving" + return False, f"Exception occured while gripper was moving: {e}" return True, "Closed gripper successfully" @@ -432,7 +432,7 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while gripper was moving" + return False, f"Exception occured while gripper was moving: {e}" return True, "Opened gripper successfully" @@ -501,7 +501,7 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: except Exception as e: return ( False, - "An error occured while trying to move arm \n Exception:" + str(e), + f"An error occured while trying to move arm \n Exception: {e}" ) return True, "Moved arm successfully" @@ -555,6 +555,6 @@ def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): self._robot.logger.info("Finished grasp.") except Exception as e: - return False, "An error occured while trying to grasp from pose" + return False, f"An error occured while trying to grasp from pose {e}" return True, "Grasped successfully" diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py index d1cd14d3..eb6eec7e 100644 --- a/spot_wrapper/spot_docking.py +++ b/spot_wrapper/spot_docking.py @@ -41,7 +41,7 @@ def dock(self, dock_id: int) -> typing.Tuple[bool, str]: self.last_docking_command = None return True, "Success" except Exception as e: - return False, str(e) + return False, f"Exception while trying to dock: {e}" def undock(self, timeout: int = 20) -> typing.Tuple[bool, str]: """Power motors on and undock the robot from the station.""" @@ -52,7 +52,7 @@ def undock(self, timeout: int = 20) -> typing.Tuple[bool, str]: blocking_undock(self._robot, timeout) return True, "Success" except Exception as e: - return False, str(e) + return False, f"Exception while trying to undock: {e}" def get_docking_state(self, **kwargs) -> docking_pb2.DockState: """Get docking state of robot.""" diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index e63b6e13..434140e4 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -870,16 +870,16 @@ def assertEStop(self, severe=True): self._estop_keepalive.settle_then_cut() return True, "Success" - except: - return False, "Error" + except Exception as e: + return False, f"Exception while attempting to estop: {e}" def disengageEStop(self): """Disengages the E-Stop""" try: self._estop_keepalive.allow() return True, "Success" - except: - return False, "Error" + except Exception as e: + return False, f"Exception while attempting to disengage estop {e}" def releaseEStop(self): """Stop eStop keepalive""" @@ -910,7 +910,7 @@ def release(self): self.releaseEStop() return True, "Success" except Exception as e: - return False, str(e) + return False, f"Exception while attempting to release the lease: {e}" def disconnect(self): """Release control of robot as gracefully as posssible.""" @@ -941,9 +941,7 @@ def _robot_command( ) return True, "Success", command_id except Exception as e: - self._logger.error( - "Unable to execute robot command, exception was" + str(e) - ) + self._logger.error(f"Unable to execute robot command: {e}") return False, str(e), None @try_claim @@ -1040,7 +1038,7 @@ def clear_behavior_fault(self, id): ) return True, "Success", rid except Exception as e: - return False, str(e), None + return False, f"Exception while clearing behavior fault: {e}", None @try_claim def power_on(self): @@ -1054,7 +1052,7 @@ def power_on(self): self._logger.info("Powering on") self._robot.power_on() except Exception as e: - return False, str(e) + return False, f"Exception while powering on: {e}" return True, "Success" From 1da2f3a60956e816ab7aa38e7cb90f074f2efb19 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 5 May 2023 13:59:43 +0100 Subject: [PATCH 32/66] fix trajectory status unknown not being reset in trajectory command and async idle (#10) --- spot_wrapper/wrapper.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 434140e4..55098e79 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -243,6 +243,7 @@ def _start_query(self): self._spot_wrapper._robot_params["at_goal"] = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None + self._spot_wrapper._trajectory_status_unknown = False elif ( status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL @@ -1124,6 +1125,7 @@ def trajectory_cmd( """ if mobility_params is None: mobility_params = self._mobility_params + self._trajectory_status_unknown = False self._robot_params["at_goal"] = False self._robot_params["near_goal"] = False self._last_trajectory_command_precise = precise_position From f481dfe2bd6167130bc11d74d5adff54c7e43597 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:12:16 +0100 Subject: [PATCH 33/66] Add changes from [SW-127] Add function to get images by cameras (#11) Co-authored-by: Kaiyu Zheng <125413689+kaiyu-zheng@users.noreply.github.com> --- spot_wrapper/spot_images.py | 143 ++++++++++++++++++++++++++++++++++++ 1 file changed, 143 insertions(+) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 30a45165..c8a7ddca 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,6 +1,8 @@ import typing import logging +from enum import enum from collections import namedtuple +from dataclasses import dataclass, field from bosdyn.client.robot import Robot from bosdyn.client.image import ( @@ -39,6 +41,54 @@ "ImageBundle", ["frontleft", "frontright", "left", "right", "back", "hand"] ) +IMAGE_SOURCES_BY_CAMERA = { + "frontleft": { + "visual": "frontleft_fisheye_image", + "depth": "frontleft_depth", + "depth_registered": "frontleft_depth_in_visual_frame", + }, + "frontright": { + "visual": "frontright_fisheye_image", + "depth": "frontright_depth", + "depth_registered": "frontright_depth_in_visual_frame", + }, + "left": { + "visual": "left_fisheye_image", + "depth": "left_depth", + "depth_registered": "left_depth_in_visual_frame", + }, + "right": { + "visual": "right_fisheye_image", + "depth": "right_depth", + "depth_registered": "right_depth_in_visual_frame", + }, + "back": { + "visual": "back_fisheye_image", + "depth": "back_depth", + "depth_registered": "back_depth_in_visual_frame", + }, + "hand": { + "visual": "hand_color_image", + "depth": "hand_depth", + "depth_registered": "hand_depth_in_color_frame", + }, +} + +IMAGE_TYPES = {"visual", "depth", "depth_registered"} + + +@dataclass(frozen=True, eq=True) +class CameraSource: + camera_name: str + image_types: list[str] + + +@dataclass(frozen=True) +class ImageEntry: + camera_name: str + image_type: str + image_response: image_pb2.ImageResponse + class SpotImages: def __init__( @@ -101,6 +151,31 @@ def __init__( ) ) + # Build image requests by camera + self._image_requests_by_camera = {} + for camera in IMAGE_SOURCES_BY_CAMERA: + if camera == "hand" and not self._robot.has_arm(): + continue + self._image_requests_by_camera[camera] = {} + image_types = IMAGE_SOURCES_BY_CAMERA[camera] + for image_type in image_types: + if image_type.startswith("depth"): + image_format = image_pb2.Image.FORMAT_RAW + pixel_format = image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + else: + image_format = image_pb2.Image.FORMAT_JPEG + pixel_format = image_pb2.Image.PIXEL_FORMAT_RGB_U8 + + source = IMAGE_SOURCES_BY_CAMERA[camera][image_type] + self._image_requests_by_camera[camera][ + image_type + ] = build_image_request( + source, + image_format=image_format, + pixel_format=pixel_format, + quality_percent=75, + ) + def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( @@ -224,3 +299,71 @@ def get_depth_registered_images( self, ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: return self.get_images(self._depth_registered_image_requests) + + def get_images_by_cameras( + self, camera_sources: typing.List[CameraSource] + ) -> typing.Optional[typing.List[ImageEntry]]: + """Calls the GetImage RPC using the image client with requests + corresponding to the given cameras. + Args: + camera_sources: a list of CameraSource objects. There are two + possibilities for each item in this list. Either it is + CameraSource(camera='front') or + CameraSource(camera='front', image_types=['visual', 'depth_registered') + - If the former is provided, the image requests will include all + image types for each specified camera. + - If the latter is provided, the image requests will be + limited to the specified image types for each corresponding + camera. + Note that duplicates of camera names are not allowed. + Returns: + a list, where each entry is (camera_name, image_type, image_response) + e.g. ('frontleft', 'visual', image_response), or none if there was an error + """ + # Build image requests + image_requests = [] + source_types = [] + cameras_specified = set() + for item in camera_sources: + if item.camera_name in cameras_specified: + self._logger.error( + f"Duplicated camera source for camera {item.camera_name}" + ) + return None + image_types = item.image_types + if image_types is None: + image_types = IMAGE_TYPES + for image_type in image_types: + try: + image_requests.append( + self._image_requests_by_camera[item.camera_name][image_type] + ) + except KeyError: + self._logger.error( + f"Unexpected camera name '{item.camera_name}' or image type '{image_type}'" + ) + return None + source_types.append((item.camera_name, image_type)) + cameras_specified.add(item.camera_name) + + # Send image requests + try: + image_responses = self._image_client.get_image(image_requests) + except UnsupportedPixelFormatRequestedError: + self._logger.error( + "UnsupportedPixelFormatRequestedError. " + "Likely pixel_format is set wrong for some image request" + ) + return None + + # Return + result = [] + for i, (camera_name, image_type) in enumerate(source_types): + result.append( + ImageEntry( + camera_name=camera_name, + image_type=image_type, + image_response=image_responses[i], + ) + ) + return result From 5d46cda31cf8e69274f7fcefd70b7ea58a4afe80 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 19 May 2023 13:03:31 +0100 Subject: [PATCH 34/66] fix dataclass typing issue for older python versions (20.04), check lease object is initialised in claim function (#14) --- spot_wrapper/spot_images.py | 2 +- spot_wrapper/wrapper.py | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index c8a7ddca..6891cdcc 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -80,7 +80,7 @@ @dataclass(frozen=True, eq=True) class CameraSource: camera_name: str - image_types: list[str] + image_types: typing.List[str] @dataclass(frozen=True) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 55098e79..a9329a60 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -819,12 +819,13 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" - for resource in self.lease: - if ( - resource.resource == "all-leases" - and SPOT_CLIENT_NAME in resource.lease_owner.client_name - ): - return True, "We already claimed the lease" + if self.lease is not None: + for resource in self.lease: + if ( + resource.resource == "all-leases" + and SPOT_CLIENT_NAME in resource.lease_owner.client_name + ): + return True, "We already claimed the lease" try: self._robot_params["robot_id"] = self._robot.get_id() From b4010061f463a190a877690f6a38d3d610e81554 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:33:53 +0100 Subject: [PATCH 35/66] remove old camera task mapping introduced in merge --- spot_wrapper/wrapper.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index a9329a60..9c4f5c2b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -630,16 +630,6 @@ def __init__( self._async_tasks = AsyncTasks(robot_tasks) - self.camera_task_name_to_task_mapping = { - "hand_image": self._hand_image_task, - "side_image": self._side_image_task, - "rear_image": self._rear_image_task, - "front_image": self._front_image_task, - } - - self._robot_id = None - self._lease = None - @staticmethod def authenticate(robot, username, password, logger): """ From c9eb3515d6a97cfa75b3e975386fa1617a71edb4 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:34:19 +0100 Subject: [PATCH 36/66] formatting --- spot_wrapper/spot_arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 5e50d6a6..aeac4d51 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -501,7 +501,7 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: except Exception as e: return ( False, - f"An error occured while trying to move arm \n Exception: {e}" + f"An error occured while trying to move arm \n Exception: {e}", ) return True, "Moved arm successfully" From 056fc25103b26c62594edbac883a250660ef3b40 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:42:37 +0100 Subject: [PATCH 37/66] Add changes from [WUD-126] Add manipulation client (#13) Co-authored-by: myeatman-bdai <129521731+myeatman-bdai@users.noreply.github.com> --- spot_wrapper/spot_arm.py | 52 ++++++++++++++++++++++++++++++++++++---- spot_wrapper/wrapper.py | 10 +++++--- 2 files changed, 54 insertions(+), 8 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index aeac4d51..d3019cbd 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -61,6 +61,7 @@ def __init__( logger: logging.Logger, robot_params: typing.Dict[str, typing.Any], robot_clients: typing.Dict[str, typing.Any], + max_command_duration: float ): """ Constructor for SpotArm class. @@ -72,15 +73,17 @@ def __init__( robot_clients: Dictionary of robot clients - robot_clients['robot_command_client']: RobotCommandClient object - robot_clients['robot_command_method']: RobotCommand method + max_command_duration: Maximum duration for commands when using the manipulation command method """ self._robot = robot self._logger = logger self._robot_params = robot_params + self._max_command_duration = max_command_duration self._robot_command_client: RobotCommandClient = robot_clients[ "robot_command_client" ] - self._manipulation_client: ManipulationApiClient = robot_clients[ - "manipulation_client" + self._manipulation_api_client: ManipulationApiClient = robot_clients[ + "manipulation_api_client" ] self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] self._image_client: ImageClient = robot_clients["image_client"] @@ -115,6 +118,43 @@ def initialize_hand_image_service(self): def hand_image_task(self): return self._hand_image_task + def _manipulation_request( + self, + request_proto: manipulation_api_pb2, + end_time_secs=None, + timesync_endpoint=None, + ): + """Generic function for sending requests to the manipulation api of a robot. + Args: + request_proto: manipulation_api_pb2 object to send to the robot. + """ + try: + command_id = self._manipulation_api_client.manipulation_api_command( + manipulation_api_request=request_proto + ).manipulation_cmd_id + + return True, "Success", command_id + except Exception as e: + self._logger.error(f"Unable to execute manipulation command: {e}") + return False, str(e), None + + def manipulation_command(self, request: manipulation_api_pb2): + end_time = time.time() + self._max_command_duration + return self._manipulation_request( + request, + end_time_secs=end_time, + timesync_endpoint=self._robot.time_sync.endpoint, + ) + + def get_manipulation_command_feedback(self, cmd_id): + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_id + ) + + return self._manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) + def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: if not self._robot.has_arm(): return False, "Spot with an arm is required for this service" @@ -520,7 +560,7 @@ def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): pick_object=grasp ) # Send the request - cmd_response = self._manipulation_client.manipulation_api_command( + cmd_response = self._manipulation_api_client.manipulation_api_command( manipulation_api_request=grasp_request ) @@ -531,8 +571,10 @@ def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): ) # Send the request - response = self._manipulation_client.manipulation_api_feedback_command( - manipulation_api_feedback_request=feedback_request + response = ( + self._manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) ) print( diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9c4f5c2b..cdacf1eb 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -502,7 +502,7 @@ def __init__( self._logger.info("No point cloud services are available.") if self._robot.has_arm(): - self._manipulation_client = self._robot.ensure_client( + self._manipulation_api_client = self._robot.ensure_client( ManipulationApiClient.default_service_name ) @@ -519,7 +519,7 @@ def __init__( "spot_check_client": self._spot_check_client, "robot_command_method": self._robot_command, "world_objects_client": self._world_objects_client, - "manipulation_client": self._manipulation_client, + "manipulation_api_client": self._manipulation_api_client, } if self._point_cloud_client: self._robot_clients["point_cloud_client"] = self._point_cloud_client @@ -593,7 +593,11 @@ def __init__( if self._robot.has_arm(): self._spot_arm = SpotArm( - self._robot, self._logger, self._robot_params, self._robot_clients + self._robot, + self._logger, + self._robot_params, + self._robot_clients, + MAX_COMMAND_DURATION, ) self._hand_image_task = self._spot_arm.hand_image_task robot_tasks.append(self._hand_image_task) From 65347c46fa2a7f65fb42333198365466204d5aa2 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:53:01 +0100 Subject: [PATCH 38/66] Add changes for added support for the rgb_cameras parameter in spot_ros2 (#15) Co-authored-by: Philip Keller Co-authored-by: Shubham <52372631+skpawar1305@users.noreply.github.com> --- spot_wrapper/spot_images.py | 10 +++++++++- spot_wrapper/wrapper.py | 5 ++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 6891cdcc..536eb7ad 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -97,9 +97,11 @@ def __init__( logger: logging.Logger, robot_params: typing.Dict[str, typing.Any], robot_clients: typing.Dict[str, typing.Any], + rgb_cameras: bool = True, ): self._robot = robot self._logger = logger + self._rgb_cameras = rgb_cameras self._robot_params = robot_params self._image_client: ImageClient = robot_clients["image_client"] @@ -164,7 +166,13 @@ def __init__( pixel_format = image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 else: image_format = image_pb2.Image.FORMAT_JPEG - pixel_format = image_pb2.Image.PIXEL_FORMAT_RGB_U8 + if camera == "hand" or self._rgb_cameras: + pixel_format = image_pb2.Image.PIXEL_FORMAT_RGB_U8 + elif camera != "hand": + self._logger.info( + f"Switching {camera}:{image_type} to greyscale image format." + ) + pixel_format = image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8 source = IMAGE_SOURCES_BY_CAMERA[camera][image_type] self._image_requests_by_camera[camera][ diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index cdacf1eb..4c2aa47b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -378,6 +378,7 @@ def __init__( use_take_lease: bool = False, get_lease_on_action: bool = False, continually_try_stand: bool = True, + rgb_cameras: bool = True, ): """ Args: @@ -397,6 +398,7 @@ def __init__( power on the robot for commands which require it - stand, rollover, self-right. continually_try_stand: If the robot expects to be standing and is not, command a stand. This can result in strange behavior if you use the wrapper and tablet together. + rgb_cameras: If the robot has only body-cameras with greyscale images, this must be set to false. """ self._username = username self._password = password @@ -407,6 +409,7 @@ def __init__( self._use_take_lease = use_take_lease self._get_lease_on_action = get_lease_on_action self._continually_try_stand = continually_try_stand + self._rgb_cameras = rgb_cameras self._frame_prefix = "" if robot_name is not None: self._frame_prefix = robot_name + "/" @@ -614,7 +617,7 @@ def __init__( self._robot, self._logger, self._robot_params, self._robot_clients ) self._spot_images = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients + self._robot, self._logger, self._robot_params, self._robot_clients, self._rgb_cameras ) if self._point_cloud_client: From 3e855a47c1a5a99a24b8ead43650784d4871d991 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 13:07:38 +0100 Subject: [PATCH 39/66] Add changes from [SW-141] Checking edge cases in upload_graph (#12) Co-authored-by: kzheng <125413689+kaiyu-zheng@users.noreply.github.com> --- spot_wrapper/spot_graph_nav.py | 89 +++++++++++++++++++++++++--------- 1 file changed, 67 insertions(+), 22 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 2317ad57..045a5690 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -1,20 +1,23 @@ -import typing import logging import math -import time import os +import time +import typing -from bosdyn.client.robot import Robot -from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.lease import LeaseClient, LeaseWallet, LeaseKeepAlive -from bosdyn.client.graph_nav import GraphNavClient -from bosdyn.client.map_processing import MapProcessingServiceClient -from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.api.graph_nav import graph_nav_pb2 from bosdyn.api.graph_nav import map_pb2 -from bosdyn.api.graph_nav import nav_pb2 from bosdyn.api.graph_nav import map_processing_pb2 - +from bosdyn.api.graph_nav import nav_pb2 +from bosdyn.client.frame_helpers import get_odom_tform_body +from bosdyn.client.graph_nav import GraphNavClient +from bosdyn.client.lease import ( + LeaseClient, + LeaseWallet, + LeaseKeepAlive, +) +from bosdyn.client.map_processing import MapProcessingServiceClient +from bosdyn.client.robot import Robot +from bosdyn.client.robot_state import RobotStateClient from google.protobuf import wrappers_pb2 @@ -37,12 +40,19 @@ def __init__( self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet self._robot_params = robot_params + self._init_current_graph_nav_state() + + def _init_current_graph_nav_state(self): # Store the most recent knowledge of the state of the robot based on rpc calls. self._current_graph = None self._current_edges = dict() # maps to_waypoint to list(from_waypoint) self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() + self._current_anchored_world_objects = ( + dict() + ) # maps object id to a (wo, waypoint, fiducial) + self._current_anchors = dict() # maps anchor id to anchor def list_graph(self) -> typing.List[str]: """List waypoint ids of graph_nav @@ -318,7 +328,7 @@ def _list_graph_waypoint_and_edge_ids(self, *args): def _upload_graph_and_snapshots(self, upload_filepath: str): """Upload the graph and snapshots to the robot.""" self._logger.info("Loading the graph from disk into local storage...") - with open(upload_filepath + "/graph", "rb") as graph_file: + with open(os.path.join(upload_filepath, "graph"), "rb") as graph_file: # Load the graph from disk. data = graph_file.read() self._current_graph = map_pb2.Graph() @@ -330,28 +340,60 @@ def _upload_graph_and_snapshots(self, upload_filepath: str): ) for waypoint in self._current_graph.waypoints: # Load the waypoint snapshots from disk. - with open( - upload_filepath + "/waypoint_snapshots/{}".format(waypoint.snapshot_id), - "rb", - ) as snapshot_file: + if len(waypoint.snapshot_id) == 0: + continue + waypoint_filepath = os.path.join( + upload_filepath, "waypoint_snapshots", waypoint.snapshot_id + ) + if not os.path.exists(waypoint_filepath): + continue + with open(waypoint_filepath, "rb") as snapshot_file: waypoint_snapshot = map_pb2.WaypointSnapshot() waypoint_snapshot.ParseFromString(snapshot_file.read()) self._current_waypoint_snapshots[ waypoint_snapshot.id ] = waypoint_snapshot + + for fiducial in waypoint_snapshot.objects: + if not fiducial.HasField("apriltag_properties"): + continue + + str_id = str(fiducial.apriltag_properties.tag_id) + if ( + str_id in self._current_anchored_world_objects + and len(self._current_anchored_world_objects[str_id]) == 1 + ): + # Replace the placeholder tuple with a tuple of (wo, waypoint, fiducial). + anchored_wo = self._current_anchored_world_objects[str_id][0] + self._current_anchored_world_objects[str_id] = ( + anchored_wo, + waypoint, + fiducial, + ) for edge in self._current_graph.edges: # Load the edge snapshots from disk. - self._logger.info(f"Trying edge: {edge.snapshot_id}") - if not edge.snapshot_id: + if len(edge.snapshot_id) == 0: + continue + edge_filepath = os.path.join( + upload_filepath, "edge_snapshots", edge.snapshot_id + ) + if not os.path.exists(edge_filepath): continue - with open( - upload_filepath + "/edge_snapshots/{}".format(edge.snapshot_id), "rb" - ) as snapshot_file: + with open(edge_filepath, "rb") as snapshot_file: edge_snapshot = map_pb2.EdgeSnapshot() edge_snapshot.ParseFromString(snapshot_file.read()) self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot + for anchor in self._current_graph.anchoring.anchors: + self._current_anchors[anchor.id] = anchor # Upload the graph to the robot. self._logger.info("Uploading the graph and snapshots to the robot...") + if self._lease is None: + self._logger.error( + "Graph nav module did not have a lease to the robot. Claim it before attempting to upload the graph " + "and snapshots." + ) + return + self._graph_nav_client.upload_graph( lease=self._lease.lease_proto, graph=self._current_graph ) @@ -370,7 +412,8 @@ def _upload_graph_and_snapshots(self, upload_filepath: str): if not localization_state.localization.waypoint_id: # The robot is not localized to the newly uploaded graph. self._logger.info( - "Upload complete! The robot is currently not localized to the map; please localize the robot using a fiducial before attempting a navigation command." + "Upload complete! The robot is currently not localized to the map; please localize the robot using a " + "fiducial before attempting a navigation command." ) def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: @@ -498,7 +541,9 @@ def _navigate_route( def _clear_graph(self, *args) -> bool: """Clear the state of the map on the robot, removing all waypoints and edges.""" - return self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) + result = self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) + self._init_current_graph_nav_state() + return result def _check_success(self, command_id: int = -1) -> bool: """Use a navigation command id to get feedback from the robot and sit when command succeeds.""" From 03c987578f99edb59f09a024acb1662e69ce954a Mon Sep 17 00:00:00 2001 From: David Watkins <129521611+davidwatkins-bdai@users.noreply.github.com> Date: Thu, 1 Jun 2023 13:54:27 -0400 Subject: [PATCH 40/66] Updated bosdyn to 3.2.3 (#16) --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 76c87a8b..2c4b4fde 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,4 +1,4 @@ -bosdyn_core==3.2.2.post1 +bosdyn_core==3.2.3 protobuf==4.22.1 setuptools==45.2.0 pytest==7.3.1 \ No newline at end of file From 3ed105d72b729cf250d86a01bbed7922fdcce1c0 Mon Sep 17 00:00:00 2001 From: Vedant Gupta <134331854+Guptabot@users.noreply.github.com> Date: Fri, 2 Jun 2023 19:01:23 -0400 Subject: [PATCH 41/66] [OC-4] Build a Spot Dance Interface (#17) * [OC-5] add method to upload hardcoded file * [OC-4] Code cleanup for PR * [OC-4] Create separate SpotDacing class * [OC-4] remove old execute_dance code * [OC-4] Fix authentication method for compatibility with WUD test * [OC-5] format files with black * [OC-4] Modify code based on PR comments * delete execute_dance from wrapper.py * [OC-4] Get lease from decorator * [OC-4] Remove print statements * [OC-4] Remove extra imports * [OC-4] Remove extra argument --- spot_wrapper/spot_dance.py | 79 ++++++++++++++++++++++++++++++++++++++ spot_wrapper/wrapper.py | 26 ++++++++++++- 2 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 spot_wrapper/spot_dance.py diff --git a/spot_wrapper/spot_dance.py b/spot_wrapper/spot_dance.py new file mode 100644 index 00000000..e4882de3 --- /dev/null +++ b/spot_wrapper/spot_dance.py @@ -0,0 +1,79 @@ +import time + +from bosdyn.choreography.client.choreography import ( + load_choreography_sequence_from_txt_file, +) +from bosdyn.client import ResponseError +from bosdyn.client.exceptions import UnauthenticatedError +from bosdyn.client.robot import Robot +from bosdyn.choreography.client.choreography import ChoreographyClient + + +class SpotDance: + def __init__( + self, + robot: Robot, + choreography_client: ChoreographyClient, + is_licensed_for_choreography: bool, + ): + self._robot = robot + self._choreography_client = choreography_client + self._is_licensed_for_choreography = is_licensed_for_choreography + + def execute_dance(self, filepath: str) -> tuple[bool, str]: + """uploads and executes the dance at filepath to Spot""" + + if not self._is_licensed_for_choreography: + return False, "Robot is not licensed for choreography." + if self._robot.is_estopped(): + error_msg = "Robot is estopped. Please use an external E-Stop client" + "such as the estop SDK example, to configure E-Stop." + return False, error_msg + try: + choreography = load_choreography_sequence_from_txt_file(filepath) + except Exception as execp: + error_msg = "Failed to load choreography. Raised exception: " + str(execp) + return False, error_msg + try: + upload_response = self._choreography_client.upload_choreography( + choreography, non_strict_parsing=True + ) + except UnauthenticatedError as err: + error_msg = "The robot license must contain 'choreography' permissions to upload and execute dances." + "Please contact Boston Dynamics Support to get the appropriate license file. " + return False, error_msg + except ResponseError as err: + error_msg = "Choreography sequence upload failed. The following warnings were produced: " + for warn in err.response.warnings: + error_msg += warn + return False, error_msg + + # Routine is valid! Power on robot and execute routine. + try: + self._robot.power_on() + routine_name = choreography.name + client_start_time = time.time() + 5.0 + start_slice = 0 # start the choreography at the beginning + + # Issue the command to the robot's choreography service. + + self._choreography_client.execute_choreography( + choreography_name=routine_name, + client_start_time=client_start_time, + choreography_starting_slice=start_slice, + ) + + # Estimate how long the choreographed sequence will take, and sleep that long. + + total_choreography_slices = 0 + for move in choreography.moves: + total_choreography_slices += move.requested_slices + estimated_time_seconds = ( + total_choreography_slices / choreography.slices_per_minute * 60.0 + ) + time.sleep(estimated_time_seconds) + + self._robot.power_off() + return True, "sucess" + except Exception as e: + return False, f"Error executing dance: {e}" diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 4c2aa47b..de146782 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -35,6 +35,11 @@ from bosdyn.client.robot_state import RobotStateClient from bosdyn.client.world_object import WorldObjectClient from bosdyn.client.spot_check import SpotCheckClient +from bosdyn.client.license import LicenseClient +from bosdyn.choreography.client.choreography import ( + ChoreographyClient, + load_choreography_sequence_from_txt_file, +) from bosdyn.geometry import EulerZXY from bosdyn.api import basic_command_pb2 @@ -47,6 +52,7 @@ from .spot_graph_nav import SpotGraphNav from .spot_check import SpotCheck from .spot_images import SpotImages +from .spot_dance import SpotDance SPOT_CLIENT_NAME = "ros_spot" MAX_COMMAND_DURATION = 1e5 @@ -428,6 +434,7 @@ def __init__( self._near_goal = False self._trajectory_status_unknown = False self._last_robot_command_feedback = False + self._is_licensed_for_choreography = True self._last_stand_command = None self._last_sit_command = None self._last_trajectory_command = None @@ -441,6 +448,7 @@ def __init__( self._logger.error("Error creating SDK object: %s", e) self._valid = False return + self._sdk.register_service_client(ChoreographyClient) self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) @@ -503,7 +511,17 @@ def __init__( except Exception as e: self._point_cloud_client = None self._logger.info("No point cloud services are available.") - + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) + if not self._license_client.get_feature_enabled( + [ChoreographyClient.license_name] + )[ChoreographyClient.license_name]: + self._logger.error(f"Robot is not licensed for choreography: {e}") + self._is_licensed_for_choreography = False if self._robot.has_arm(): self._manipulation_api_client = self._robot.ensure_client( ManipulationApiClient.default_service_name @@ -635,6 +653,12 @@ def __init__( self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) + self._spot_dance = SpotDance( + self._robot, + self._choreography_client, + self._is_licensed_for_choreography, + ) + self._async_tasks = AsyncTasks(robot_tasks) @staticmethod From 439965af6f8e9e8e9d36414dc7301e107329c99d Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 14:23:36 +0100 Subject: [PATCH 42/66] formatting --- spot_wrapper/spot_arm.py | 2 +- spot_wrapper/wrapper.py | 14 +++++++++----- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index d3019cbd..bfb34095 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -61,7 +61,7 @@ def __init__( logger: logging.Logger, robot_params: typing.Dict[str, typing.Any], robot_clients: typing.Dict[str, typing.Any], - max_command_duration: float + max_command_duration: float, ): """ Constructor for SpotArm class. diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 96045438..524e5123 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -635,7 +635,11 @@ def __init__( self._robot, self._logger, self._robot_params, self._robot_clients ) self._spot_images = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients, self._rgb_cameras + self._robot, + self._logger, + self._robot_params, + self._robot_clients, + self._rgb_cameras, ) if self._point_cloud_client: @@ -654,10 +658,10 @@ def __init__( robot_tasks.append(self._world_objects_task) self._spot_dance = SpotDance( - self._robot, - self._choreography_client, - self._is_licensed_for_choreography, - ) + self._robot, + self._choreography_client, + self._is_licensed_for_choreography, + ) self._async_tasks = AsyncTasks(robot_tasks) From 90d60ac1ec9fe21d592553102bded72bbe085241 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 15:19:36 +0100 Subject: [PATCH 43/66] fix startup issues when choreography or arm is not present --- spot_wrapper/spot_images.py | 3 +- spot_wrapper/wrapper.py | 69 +++++++++++++++++++++++-------------- 2 files changed, 44 insertions(+), 28 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 536eb7ad..563a5291 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,8 +1,7 @@ import typing import logging -from enum import enum from collections import namedtuple -from dataclasses import dataclass, field +from dataclasses import dataclass from bosdyn.client.robot import Robot from bosdyn.client.image import ( diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 524e5123..c2cba15b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -16,6 +16,7 @@ from bosdyn.client import frame_helpers from bosdyn.client import math_helpers from bosdyn.client import robot_command +from bosdyn.client.robot import UnregisteredServiceError from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks from bosdyn.client.docking import DockingClient @@ -36,10 +37,17 @@ from bosdyn.client.world_object import WorldObjectClient from bosdyn.client.spot_check import SpotCheckClient from bosdyn.client.license import LicenseClient -from bosdyn.choreography.client.choreography import ( - ChoreographyClient, - load_choreography_sequence_from_txt_file, -) + +HAVE_CHOREOGRAPHY_MODULE = True +try: + from bosdyn.choreography.client.choreography import ( + ChoreographyClient, + load_choreography_sequence_from_txt_file, + ) + from .spot_dance import SpotDance +except ModuleNotFoundError: + HAVE_CHOREOGRAPHY_MODULE = False + from bosdyn.geometry import EulerZXY from bosdyn.api import basic_command_pb2 @@ -52,7 +60,6 @@ from .spot_graph_nav import SpotGraphNav from .spot_check import SpotCheck from .spot_images import SpotImages -from .spot_dance import SpotDance SPOT_CLIENT_NAME = "ros_spot" MAX_COMMAND_DURATION = 1e5 @@ -445,10 +452,10 @@ def __init__( try: self._sdk = create_standard_sdk(SPOT_CLIENT_NAME) except Exception as e: - self._logger.error("Error creating SDK object: %s", e) + self._logger.error(f"Error creating SDK object: {e}") self._valid = False return - self._sdk.register_service_client(ChoreographyClient) + self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) @@ -508,24 +515,33 @@ def __init__( self._point_cloud_client = self._robot.ensure_client( VELODYNE_SERVICE_NAME ) - except Exception as e: + except UnregisteredServiceError as e: self._point_cloud_client = None self._logger.info("No point cloud services are available.") - self._choreography_client = self._robot.ensure_client( - ChoreographyClient.default_service_name - ) - self._license_client = self._robot.ensure_client( - LicenseClient.default_service_name - ) - if not self._license_client.get_feature_enabled( - [ChoreographyClient.license_name] - )[ChoreographyClient.license_name]: - self._logger.error(f"Robot is not licensed for choreography: {e}") - self._is_licensed_for_choreography = False + + if HAVE_CHOREOGRAPHY_MODULE: + self._sdk.register_service_client(ChoreographyClient) + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) + if not self._license_client.get_feature_enabled( + [ChoreographyClient.license_name] + )[ChoreographyClient.license_name]: + self._logger.error(f"Robot is not licensed for choreography: {e}") + self._is_licensed_for_choreography = False + else: + self._choreography_client = None + if self._robot.has_arm(): self._manipulation_api_client = self._robot.ensure_client( ManipulationApiClient.default_service_name ) + else: + self._manipulation_api_client = None + self._robot_clients = { "robot_state_client": self._robot_state_client, @@ -541,9 +557,9 @@ def __init__( "robot_command_method": self._robot_command, "world_objects_client": self._world_objects_client, "manipulation_api_client": self._manipulation_api_client, + "choreography_client": self._choreography_client, + "point_cloud_client": self._point_cloud_client, } - if self._point_cloud_client: - self._robot_clients["point_cloud_client"] = self._point_cloud_client initialised = True except Exception as e: @@ -657,11 +673,12 @@ def __init__( self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) - self._spot_dance = SpotDance( - self._robot, - self._choreography_client, - self._is_licensed_for_choreography, - ) + if HAVE_CHOREOGRAPHY_MODULE: + self._spot_dance = SpotDance( + self._robot, + self._choreography_client, + self._is_licensed_for_choreography, + ) self._async_tasks = AsyncTasks(robot_tasks) From 7201b3a69f21cafb56b78940258928680ec2a4e4 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 15:37:32 +0100 Subject: [PATCH 44/66] small changes to choreo check and output when services are not available --- spot_wrapper/wrapper.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index c2cba15b..908a9936 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -38,13 +38,14 @@ from bosdyn.client.spot_check import SpotCheckClient from bosdyn.client.license import LicenseClient -HAVE_CHOREOGRAPHY_MODULE = True try: from bosdyn.choreography.client.choreography import ( ChoreographyClient, load_choreography_sequence_from_txt_file, ) from .spot_dance import SpotDance + + HAVE_CHOREOGRAPHY_MODULE = True except ModuleNotFoundError: HAVE_CHOREOGRAPHY_MODULE = False @@ -441,7 +442,6 @@ def __init__( self._near_goal = False self._trajectory_status_unknown = False self._last_robot_command_feedback = False - self._is_licensed_for_choreography = True self._last_stand_command = None self._last_sit_command = None self._last_trajectory_command = None @@ -456,7 +456,6 @@ def __init__( self._valid = False return - self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) @@ -517,7 +516,7 @@ def __init__( ) except UnregisteredServiceError as e: self._point_cloud_client = None - self._logger.info("No point cloud services are available.") + self._logger.info("Velodyne point cloud service is not available.") if HAVE_CHOREOGRAPHY_MODULE: self._sdk.register_service_client(ChoreographyClient) @@ -530,8 +529,12 @@ def __init__( if not self._license_client.get_feature_enabled( [ChoreographyClient.license_name] )[ChoreographyClient.license_name]: - self._logger.error(f"Robot is not licensed for choreography: {e}") + self._logger.info( + f"Robot is not licensed for choreography: {e}" + ) self._is_licensed_for_choreography = False + else: + self._is_licensed_for_choreography = True else: self._choreography_client = None @@ -541,7 +544,7 @@ def __init__( ) else: self._manipulation_api_client = None - + self._logger.info("Manipulation API is not available.") self._robot_clients = { "robot_state_client": self._robot_state_client, @@ -673,7 +676,7 @@ def __init__( self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) - if HAVE_CHOREOGRAPHY_MODULE: + if self._is_licensed_for_choreography: self._spot_dance = SpotDance( self._robot, self._choreography_client, From 9b7ecf55f4ed8ea69677972a110cf073a49f72de Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 15:46:15 +0100 Subject: [PATCH 45/66] better choreo ordering --- spot_wrapper/wrapper.py | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 908a9936..4bd2b45f 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -518,24 +518,27 @@ def __init__( self._point_cloud_client = None self._logger.info("Velodyne point cloud service is not available.") + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) + if HAVE_CHOREOGRAPHY_MODULE: - self._sdk.register_service_client(ChoreographyClient) - self._choreography_client = self._robot.ensure_client( - ChoreographyClient.default_service_name - ) - self._license_client = self._robot.ensure_client( - LicenseClient.default_service_name - ) - if not self._license_client.get_feature_enabled( + if self._license_client.get_feature_enabled( [ChoreographyClient.license_name] )[ChoreographyClient.license_name]: + self._sdk.register_service_client(ChoreographyClient) + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) + self._is_licensed_for_choreography = True + else: self._logger.info( f"Robot is not licensed for choreography: {e}" ) self._is_licensed_for_choreography = False - else: - self._is_licensed_for_choreography = True + self._choreography_client = None else: + self._is_licensed_for_choreography = False self._choreography_client = None if self._robot.has_arm(): From 0a67be94c18c2ea263736951fc5a0400edccb625 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 15:48:22 +0100 Subject: [PATCH 46/66] message when choreo module is missing --- spot_wrapper/wrapper.py | 1 + 1 file changed, 1 insertion(+) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 4bd2b45f..b6fda72b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -538,6 +538,7 @@ def __init__( self._is_licensed_for_choreography = False self._choreography_client = None else: + self._logger.info(f"Choreography is not available.") self._is_licensed_for_choreography = False self._choreography_client = None From 5d53c58ea5b7a190dbcd71a63b46c2548f283897 Mon Sep 17 00:00:00 2001 From: Ming Jie See Date: Fri, 9 Jun 2023 14:46:05 +0100 Subject: [PATCH 47/66] cleanup unused spot_image and renamed graphnav private methods --- spot_wrapper/spot_graph_nav.py | 8 ++++---- spot_wrapper/wrapper.py | 3 --- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 045a5690..f3d917a9 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -97,8 +97,8 @@ def navigate_initial_localization( self._lease = self._lease_wallet.get_lease() self._lease_keepalive = LeaseKeepAlive(self._lease_client) if upload_filepath: - self._clear_graph() - self._upload_graph_and_snapshots(upload_filepath) + self.clear_graph() + self.upload_graph_and_snapshots(upload_filepath) else: self._download_current_graph() self._logger.info( @@ -325,7 +325,7 @@ def _list_graph_waypoint_and_edge_ids(self, *args): ) = self._update_waypoints_and_edges(graph, localization_id, self._logger) return self._current_annotation_name_to_wp_id, self._current_edges - def _upload_graph_and_snapshots(self, upload_filepath: str): + def upload_graph_and_snapshots(self, upload_filepath: str): """Upload the graph and snapshots to the robot.""" self._logger.info("Loading the graph from disk into local storage...") with open(os.path.join(upload_filepath, "graph"), "rb") as graph_file: @@ -539,7 +539,7 @@ def _navigate_route( return True, "Finished navigating route!" - def _clear_graph(self, *args) -> bool: + def clear_graph(self, *args) -> bool: """Clear the state of the map on the robot, removing all waypoints and edges.""" result = self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) self._init_current_graph_nav_state() diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index b6fda72b..f4b4733e 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -631,9 +631,6 @@ def __init__( "rates": self._rates, "callbacks": self._callbacks, } - self.spot_image = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients - ) if self._robot.has_arm(): self._spot_arm = SpotArm( From 17dd90c7ba8c5f2c9850161a64ba21984e839bf6 Mon Sep 17 00:00:00 2001 From: Ming Jie See Date: Fri, 9 Jun 2023 15:06:57 +0100 Subject: [PATCH 48/66] update graph_nav private methods --- spot_wrapper/spot_graph_nav.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index f3d917a9..fac56cfc 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -105,9 +105,9 @@ def navigate_initial_localization( "Re-using existing graph on robot. Check that the correct graph is loaded!" ) if initial_localization_fiducial: - self._set_initial_localization_fiducial() + self.set_initial_localization_fiducial() if initial_localization_waypoint: - self._set_initial_localization_waypoint([initial_localization_waypoint]) + self.set_initial_localization_waypoint([initial_localization_waypoint]) self._list_graph_waypoint_and_edge_ids() self._get_localization_state() @@ -166,7 +166,7 @@ def _get_localization_state(self, *args): f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}" ) - def _set_initial_localization_fiducial(self, *args): + def set_initial_localization_fiducial(self, *args): """Trigger localization when near a fiducial.""" robot_state = self._robot_state_client.get_robot_state() current_odom_tform_body = get_odom_tform_body( @@ -180,7 +180,7 @@ def _set_initial_localization_fiducial(self, *args): ko_tform_body=current_odom_tform_body, ) - def _set_initial_localization_waypoint(self, *args): + def set_initial_localization_waypoint(self, *args): """Trigger localization to a waypoint.""" # Take the first argument as the localization waypoint. if len(args) < 1: From 0ec85565f653680fdcdf1dcfd65dd93803faedda Mon Sep 17 00:00:00 2001 From: Ming Jie See Date: Fri, 9 Jun 2023 23:57:17 +0100 Subject: [PATCH 49/66] custom arm not found Exception --- spot_wrapper/wrapper.py | 245 +++++++++++----------------------------- 1 file changed, 68 insertions(+), 177 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 1fb5c6b4..5c03d507 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -96,6 +96,14 @@ def robotToLocalTime(timestamp, robot): return rtime +class MissingSpotArm(Exception): + """Raised when the arm is not available on the robot""" + + def __init__(self, message="Spot arm not available"): + # Call the base class constructor with the parameters it needs + super().__init__(message) + + class AsyncRobotState(AsyncPeriodicQuery): """Class to get robot state at regular intervals. get_robot_state_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -107,9 +115,7 @@ class AsyncRobotState(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncRobotState, self).__init__( - "robot-state", client, logger, period_sec=1.0 / max(rate, 1.0) - ) + super(AsyncRobotState, self).__init__("robot-state", client, logger, period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -132,9 +138,7 @@ class AsyncMetrics(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncMetrics, self).__init__( - "robot-metrics", client, logger, period_sec=1.0 / max(rate, 1.0) - ) + super(AsyncMetrics, self).__init__("robot-metrics", client, logger, period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -157,9 +161,7 @@ class AsyncLease(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncLease, self).__init__( - "lease", client, logger, period_sec=1.0 / max(rate, 1.0) - ) + super(AsyncLease, self).__init__("lease", client, logger, period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -189,19 +191,13 @@ def __init__(self, client, logger, rate, spot_wrapper): def _start_query(self): if self._spot_wrapper._last_stand_command != None: try: - response = self._client.robot_command_feedback( - self._spot_wrapper._last_stand_command - ) - status = ( - response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status - ) + response = self._client.robot_command_feedback(self._spot_wrapper._last_stand_command) + status = response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status self._spot_wrapper._robot_params["is_sitting"] = False if status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING: self._spot_wrapper._robot_params["is_standing"] = True self._spot_wrapper._last_stand_command = None - elif ( - status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS - ): + elif status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS: self._spot_wrapper._robot_params["is_standing"] = False else: self._logger.warning("Stand command in unknown state") @@ -213,9 +209,7 @@ def _start_query(self): if self._spot_wrapper._last_sit_command != None: try: self._spot_wrapper._robot_params["is_standing"] = False - response = self._client.robot_command_feedback( - self._spot_wrapper._last_sit_command - ) + response = self._client.robot_command_feedback(self._spot_wrapper._last_sit_command) if ( response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status == basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING @@ -238,49 +232,31 @@ def _start_query(self): if self._spot_wrapper._last_trajectory_command != None: try: - response = self._client.robot_command_feedback( - self._spot_wrapper._last_trajectory_command - ) + response = self._client.robot_command_feedback(self._spot_wrapper._last_trajectory_command) status = ( response.feedback.synchronized_feedback.mobility_command_feedback.se2_trajectory_feedback.status ) # STATUS_AT_GOAL always means that the robot reached the goal. If the trajectory command did not # request precise positioning, then STATUS_NEAR_GOAL also counts as reaching the goal - if ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL - or ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL - and not self._spot_wrapper._last_trajectory_command_precise - ) + if status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL or ( + status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL + and not self._spot_wrapper._last_trajectory_command_precise ): self._spot_wrapper._robot_params["at_goal"] = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None self._spot_wrapper._trajectory_status_unknown = False - elif ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL - ): + elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL: is_moving = True - elif ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL - ): + elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL: is_moving = True self._spot_wrapper._robot_params["near_goal"] = True - elif ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN - ): + elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN: self._spot_wrapper._trajectory_status_unknown = True self._spot_wrapper._last_trajectory_command = None else: self._logger.error( - "Received trajectory command status outside of expected range, value is {}".format( - status - ) + "Received trajectory command status outside of expected range, value is {}".format(status) ) self._spot_wrapper._last_trajectory_command = None except (ResponseError, RpcError) as e: @@ -314,9 +290,7 @@ class AsyncEStopMonitor(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, spot_wrapper): - super(AsyncEStopMonitor, self).__init__( - "estop_alive", client, logger, period_sec=1.0 / rate - ) + super(AsyncEStopMonitor, self).__init__("estop_alive", client, logger, period_sec=1.0 / rate) self._spot_wrapper = spot_wrapper def _start_query(self): @@ -325,20 +299,10 @@ def _start_query(self): return last_estop_status = self._spot_wrapper._estop_keepalive.status_queue.queue[-1] - if ( - last_estop_status[0] - == self._spot_wrapper._estop_keepalive.KeepAliveStatus.ERROR - ): - self._logger.error( - "Estop keepalive has an error: {}".format(last_estop_status[1]) - ) - elif ( - last_estop_status - == self._spot_wrapper._estop_keepalive.KeepAliveStatus.DISABLED - ): - self._logger.error( - "Estop keepalive is disabled: {}".format(last_estop_status[1]) - ) + if last_estop_status[0] == self._spot_wrapper._estop_keepalive.KeepAliveStatus.ERROR: + self._logger.error("Estop keepalive has an error: {}".format(last_estop_status[1])) + elif last_estop_status == self._spot_wrapper._estop_keepalive.KeepAliveStatus.DISABLED: + self._logger.error("Estop keepalive is disabled: {}".format(last_estop_status[1])) else: # estop keepalive is ok pass @@ -462,9 +426,7 @@ def __init__( self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) - authenticated = self.authenticate( - self._robot, self._username, self._password, self._logger - ) + authenticated = self.authenticate(self._robot, self._username, self._password, self._logger) if not authenticated: self._valid = False return @@ -479,65 +441,35 @@ def __init__( initialised = False while not initialised: try: - self._robot_state_client = self._robot.ensure_client( - RobotStateClient.default_service_name - ) - self._world_objects_client = self._robot.ensure_client( - WorldObjectClient.default_service_name - ) - self._robot_command_client = self._robot.ensure_client( - RobotCommandClient.default_service_name - ) - self._graph_nav_client = self._robot.ensure_client( - GraphNavClient.default_service_name - ) - self._map_processing_client = self._robot.ensure_client( - MapProcessingServiceClient.default_service_name - ) - self._power_client = self._robot.ensure_client( - PowerClient.default_service_name - ) - self._lease_client = self._robot.ensure_client( - LeaseClient.default_service_name - ) + self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name) + self._world_objects_client = self._robot.ensure_client(WorldObjectClient.default_service_name) + self._robot_command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) + self._graph_nav_client = self._robot.ensure_client(GraphNavClient.default_service_name) + self._map_processing_client = self._robot.ensure_client(MapProcessingServiceClient.default_service_name) + self._power_client = self._robot.ensure_client(PowerClient.default_service_name) + self._lease_client = self._robot.ensure_client(LeaseClient.default_service_name) self._lease_wallet = self._lease_client.lease_wallet - self._image_client = self._robot.ensure_client( - ImageClient.default_service_name - ) - self._estop_client = self._robot.ensure_client( - EstopClient.default_service_name - ) - self._docking_client = self._robot.ensure_client( - DockingClient.default_service_name - ) - self._spot_check_client = self._robot.ensure_client( - SpotCheckClient.default_service_name - ) + self._image_client = self._robot.ensure_client(ImageClient.default_service_name) + self._estop_client = self._robot.ensure_client(EstopClient.default_service_name) + self._docking_client = self._robot.ensure_client(DockingClient.default_service_name) + self._spot_check_client = self._robot.ensure_client(SpotCheckClient.default_service_name) try: - self._point_cloud_client = self._robot.ensure_client( - VELODYNE_SERVICE_NAME - ) + self._point_cloud_client = self._robot.ensure_client(VELODYNE_SERVICE_NAME) except UnregisteredServiceError as e: self._point_cloud_client = None self._logger.info("Velodyne point cloud service is not available.") - self._license_client = self._robot.ensure_client( - LicenseClient.default_service_name - ) + self._license_client = self._robot.ensure_client(LicenseClient.default_service_name) if HAVE_CHOREOGRAPHY_MODULE: - if self._license_client.get_feature_enabled( - [ChoreographyClient.license_name] - )[ChoreographyClient.license_name]: + if self._license_client.get_feature_enabled([ChoreographyClient.license_name])[ + ChoreographyClient.license_name + ]: self._sdk.register_service_client(ChoreographyClient) - self._choreography_client = self._robot.ensure_client( - ChoreographyClient.default_service_name - ) + self._choreography_client = self._robot.ensure_client(ChoreographyClient.default_service_name) self._is_licensed_for_choreography = True else: - self._logger.info( - f"Robot is not licensed for choreography: {e}" - ) + self._logger.info(f"Robot is not licensed for choreography: {e}") self._is_licensed_for_choreography = False self._choreography_client = None else: @@ -576,9 +508,7 @@ def __init__( sleep_secs = 15 self._logger.warning( "Unable to create client service: {}. This usually means the robot hasn't " - "finished booting yet. Will wait {} seconds and try again.".format( - e, sleep_secs - ) + "finished booting yet. Will wait {} seconds and try again.".format(e, sleep_secs) ) time.sleep(sleep_secs) @@ -602,12 +532,8 @@ def __init__( max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", None), ) - self._idle_task = AsyncIdle( - self._robot_command_client, self._logger, 10.0, self - ) - self._estop_monitor = AsyncEStopMonitor( - self._estop_client, self._logger, 20.0, self - ) + self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) + self._estop_monitor = AsyncEStopMonitor(self._estop_client, self._logger, 20.0, self) self._estop_endpoint = None self._estop_keepalive = None @@ -634,9 +560,7 @@ def __init__( "rates": self._rates, "callbacks": self._callbacks, } - self.spot_image = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + self.spot_image = SpotImages(self._robot, self._logger, self._robot_params, self._robot_clients) if self._robot.has_arm(): self._spot_arm = SpotArm( @@ -651,15 +575,9 @@ def __init__( else: self._spot_arm = None - self._spot_docking = SpotDocking( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._spot_graph_nav = SpotGraphNav( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._spot_check = SpotCheck( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + self._spot_docking = SpotDocking(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_graph_nav = SpotGraphNav(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_check = SpotCheck(self._robot, self._logger, self._robot_params, self._robot_clients) self._spot_images = SpotImages( self._robot, self._logger, @@ -669,17 +587,13 @@ def __init__( ) if self._point_cloud_client: - self._spot_eap = SpotEAP( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + self._spot_eap = SpotEAP(self._robot, self._logger, self._robot_params, self._robot_clients) self._point_cloud_task = self._spot_eap.async_task robot_tasks.append(self._point_cloud_task) else: self._spot_eap = None - self._spot_world_objects = SpotWorldObjects( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + self._spot_world_objects = SpotWorldObjects(self._robot, self._logger, self._robot_params, self._robot_clients) self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) @@ -747,7 +661,7 @@ def spot_images(self) -> SpotImages: def spot_arm(self) -> SpotArm: """Return SpotArm instance""" if not self._robot.has_arm(): - raise Exception("SpotArm is not available on this robot") + raise MissingSpotArm() else: return self._spot_arm @@ -876,10 +790,7 @@ def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" if self.lease is not None: for resource in self.lease: - if ( - resource.resource == "all-leases" - and SPOT_CLIENT_NAME in resource.lease_owner.client_name - ): + if resource.resource == "all-leases" and SPOT_CLIENT_NAME in resource.lease_owner.client_name: return True, "We already claimed the lease" try: @@ -908,9 +819,7 @@ def updateTasks(self): def resetEStop(self): """Get keepalive for eStop""" - self._estop_endpoint = EstopEndpoint( - self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout - ) + self._estop_endpoint = EstopEndpoint(self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout) self._estop_endpoint.force_simple_setup() # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) @@ -1023,17 +932,13 @@ def sit(self): @try_claim(power_on=True) def simple_stand(self, monitor_command=True): """If the e-stop is enabled, and the motor power is enabled, stand the robot up.""" - response = self._robot_command( - RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) - ) + response = self._robot_command(RobotCommandBuilder.synchro_stand_command(params=self._mobility_params)) if monitor_command: self._last_stand_command = response[2] return response[0], response[1] @try_claim(power_on=True) - def stand( - self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, body_roll=0 - ): + def stand(self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, body_roll=0): """ If the e-stop is enabled, and the motor power is enabled, stand the robot up. Executes a stand command, but one where the robot will assume the pose specified by the given parameters. @@ -1052,15 +957,11 @@ def stand( # If any of the orientation parameters are nonzero use them to pose the body body_orientation = EulerZXY(yaw=body_yaw, pitch=body_pitch, roll=body_roll) response = self._robot_command( - RobotCommandBuilder.synchro_stand_command( - body_height=body_height, footprint_R_body=body_orientation - ) + RobotCommandBuilder.synchro_stand_command(body_height=body_height, footprint_R_body=body_orientation) ) else: # Otherwise just use the mobility params - response = self._robot_command( - RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) - ) + response = self._robot_command(RobotCommandBuilder.synchro_stand_command(params=self._mobility_params)) if monitor_command: self._last_stand_command = response[2] @@ -1075,9 +976,7 @@ def battery_change_pose(self, dir_hint: int = 1): dir_hint: 1 rolls to the right side of the robot, 2 to the left """ if self._robot_params["is_sitting"]: - response = self._robot_command( - RobotCommandBuilder.battery_change_pose_command(dir_hint) - ) + response = self._robot_command(RobotCommandBuilder.battery_change_pose_command(dir_hint)) return response[0], response[1] return False, "Call sit before trying to roll over" @@ -1090,9 +989,7 @@ def safe_power_off(self): def clear_behavior_fault(self, id): """Clear the behavior fault defined by id.""" try: - rid = self._robot_command_client.clear_behavior_fault( - behavior_fault_id=id, lease=None - ) + rid = self._robot_command_client.clear_behavior_fault(behavior_fault_id=id, lease=None) return True, "Success", rid except Exception as e: return False, f"Exception while clearing behavior fault: {e}", None @@ -1145,9 +1042,7 @@ def velocity_cmd( """ end_time = time.time() + cmd_duration response = self._robot_command( - RobotCommandBuilder.synchro_velocity_command( - v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params - ), + RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), end_time_secs=end_time, timesync_endpoint=self._robot.time_sync.endpoint, ) @@ -1229,9 +1124,7 @@ def trajectory_cmd( self._last_trajectory_command = response[2] return response[0], response[1] - def robot_command( - self, robot_command: robot_command_pb2.RobotCommand - ) -> typing.Tuple[bool, str]: + def robot_command(self, robot_command: robot_command_pb2.RobotCommand) -> typing.Tuple[bool, str]: end_time = time.time() + MAX_COMMAND_DURATION return self._robot_command( robot_command, @@ -1239,9 +1132,7 @@ def robot_command( timesync_endpoint=self._robot.time_sync.endpoint, ) - def get_robot_command_feedback( - self, cmd_id: int - ) -> robot_command_pb2.RobotCommandFeedbackResponse: + def get_robot_command_feedback(self, cmd_id: int) -> robot_command_pb2.RobotCommandFeedbackResponse: return self._robot_command_client.robot_command_feedback(cmd_id) def check_is_powered_on(self): From 651fed617ef016c3a33e007b8097a4e5d07e8232 Mon Sep 17 00:00:00 2001 From: Ming Jie See Date: Sat, 10 Jun 2023 00:03:31 +0100 Subject: [PATCH 50/66] _get_lease private method in graphNav --- spot_wrapper/spot_graph_nav.py | 242 ++++++++++----------------------- 1 file changed, 71 insertions(+), 171 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index fac56cfc..7282025c 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -10,11 +10,7 @@ from bosdyn.api.graph_nav import nav_pb2 from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.client.graph_nav import GraphNavClient -from bosdyn.client.lease import ( - LeaseClient, - LeaseWallet, - LeaseKeepAlive, -) +from bosdyn.client.lease import LeaseClient, LeaseWallet, LeaseKeepAlive, Lease from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.robot import Robot from bosdyn.client.robot_state import RobotStateClient @@ -32,9 +28,7 @@ def __init__( self._robot = robot self._logger = logger self._graph_nav_client: GraphNavClient = robot_clients["graph_nav_client"] - self._map_processing_client: MapProcessingServiceClient = robot_clients[ - "map_processing_client" - ] + self._map_processing_client: MapProcessingServiceClient = robot_clients["map_processing_client"] self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] self._lease_client: LeaseClient = robot_clients["lease_client"] self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet @@ -42,6 +36,10 @@ def __init__( self._init_current_graph_nav_state() + def _get_lease(self) -> Lease: + self._lease = self._lease_wallet.get_lease() + return self._lease + def _init_current_graph_nav_state(self): # Store the most recent knowledge of the state of the robot based on rpc calls. self._current_graph = None @@ -49,9 +47,7 @@ def _init_current_graph_nav_state(self): self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() - self._current_anchored_world_objects = ( - dict() - ) # maps object id to a (wo, waypoint, fiducial) + self._current_anchored_world_objects = dict() # maps object id to a (wo, waypoint, fiducial) self._current_anchors = dict() # maps anchor id to anchor def list_graph(self) -> typing.List[str]: @@ -61,12 +57,7 @@ def list_graph(self) -> typing.List[str]: """ ids, eds = self._list_graph_waypoint_and_edge_ids() - return [ - v - for k, v in sorted( - ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) - ) - ] + return [v for k, v in sorted(ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")))] def navigate_initial_localization( self, @@ -94,16 +85,14 @@ def navigate_initial_localization( self._powered_on = self._started_powered_on # Claim lease, power on robot, start graphnav. - self._lease = self._lease_wallet.get_lease() + self._lease = self._get_lease() self._lease_keepalive = LeaseKeepAlive(self._lease_client) if upload_filepath: self.clear_graph() self.upload_graph_and_snapshots(upload_filepath) else: self._download_current_graph() - self._logger.info( - "Re-using existing graph on robot. Check that the correct graph is loaded!" - ) + self._logger.info("Re-using existing graph on robot. Check that the correct graph is loaded!") if initial_localization_fiducial: self.set_initial_localization_fiducial() if initial_localization_waypoint: @@ -141,9 +130,7 @@ def download_navigation_graph(self, download_path: str) -> typing.List[str]: self._download_full_graph() return self.list_graph() - def navigation_close_loops( - self, close_fiducial_loops: bool, close_odometry_loops: bool - ) -> typing.Tuple[bool, str]: + def navigation_close_loops(self, close_fiducial_loops: bool, close_odometry_loops: bool) -> typing.Tuple[bool, str]: return self._auto_close_loops(close_fiducial_loops, close_odometry_loops) def optmize_anchoring(self) -> typing.Tuple[bool, str]: @@ -159,19 +146,13 @@ def _get_localization_state(self, *args): """Get the current localization and state of the robot.""" state = self._graph_nav_client.get_localization_state() self._logger.info(f"Got localization: \n{str(state.localization)}") - odom_tform_body = get_odom_tform_body( - state.robot_kinematics.transforms_snapshot - ) - self._logger.info( - f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}" - ) + odom_tform_body = get_odom_tform_body(state.robot_kinematics.transforms_snapshot) + self._logger.info(f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}") def set_initial_localization_fiducial(self, *args): """Trigger localization when near a fiducial.""" robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() + current_odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot).to_proto() # Create an empty instance for initial localization since we are asking it to localize # based on the nearest fiducial. localization = nav_pb2.Localization() @@ -198,9 +179,7 @@ def set_initial_localization_waypoint(self, *args): return robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() + current_odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot).to_proto() # Create an initial localization to the specified waypoint as the identity. localization = nav_pb2.Localization() localization.waypoint_id = destination_waypoint @@ -230,9 +209,7 @@ def _download_full_graph(self, *args): return self._write_full_graph(graph) self._logger.info( - "Graph downloaded with {} waypoints and {} edges".format( - len(graph.waypoints), len(graph.edges) - ) + "Graph downloaded with {} waypoints and {} edges".format(len(graph.waypoints), len(graph.edges)) ) # Download the waypoint and edge snapshots. self._download_and_write_waypoint_snapshots(graph.waypoints) @@ -250,14 +227,10 @@ def _download_and_write_waypoint_snapshots(self, waypoints): if len(waypoint.snapshot_id) == 0: continue try: - waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot( - waypoint.snapshot_id - ) + waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot(waypoint.snapshot_id) except Exception: # Failure in downloading waypoint snapshot. Continue to next snapshot. - self._logger.error( - "Failed to download waypoint snapshot: " + waypoint.snapshot_id - ) + self._logger.error("Failed to download waypoint snapshot: " + waypoint.snapshot_id) continue self._write_bytes( self._download_filepath + "/waypoint_snapshots", @@ -280,14 +253,10 @@ def _download_and_write_edge_snapshots(self, edges): continue num_to_download += 1 try: - edge_snapshot = self._graph_nav_client.download_edge_snapshot( - edge.snapshot_id - ) + edge_snapshot = self._graph_nav_client.download_edge_snapshot(edge.snapshot_id) except Exception: # Failure in downloading edge snapshot. Continue to next snapshot. - self._logger.error( - "Failed to download edge snapshot: " + edge.snapshot_id - ) + self._logger.error("Failed to download edge snapshot: " + edge.snapshot_id) continue self._write_bytes( self._download_filepath + "/edge_snapshots", @@ -296,9 +265,7 @@ def _download_and_write_edge_snapshots(self, edges): ) num_edge_snapshots_downloaded += 1 self._logger.info( - "Downloaded {} of the total {} edge snapshots.".format( - num_edge_snapshots_downloaded, num_to_download - ) + "Downloaded {} of the total {} edge snapshots.".format(num_edge_snapshots_downloaded, num_to_download) ) def _write_bytes(self, filepath: str, filename: str, data): @@ -314,9 +281,7 @@ def _list_graph_waypoint_and_edge_ids(self, *args): # Download current graph graph = self._download_current_graph() - localization_id = ( - self._graph_nav_client.get_localization_state().localization.waypoint_id - ) + localization_id = self._graph_nav_client.get_localization_state().localization.waypoint_id # Update and print waypoints and edges ( @@ -342,17 +307,13 @@ def upload_graph_and_snapshots(self, upload_filepath: str): # Load the waypoint snapshots from disk. if len(waypoint.snapshot_id) == 0: continue - waypoint_filepath = os.path.join( - upload_filepath, "waypoint_snapshots", waypoint.snapshot_id - ) + waypoint_filepath = os.path.join(upload_filepath, "waypoint_snapshots", waypoint.snapshot_id) if not os.path.exists(waypoint_filepath): continue with open(waypoint_filepath, "rb") as snapshot_file: waypoint_snapshot = map_pb2.WaypointSnapshot() waypoint_snapshot.ParseFromString(snapshot_file.read()) - self._current_waypoint_snapshots[ - waypoint_snapshot.id - ] = waypoint_snapshot + self._current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot for fiducial in waypoint_snapshot.objects: if not fiducial.HasField("apriltag_properties"): @@ -374,9 +335,7 @@ def upload_graph_and_snapshots(self, upload_filepath: str): # Load the edge snapshots from disk. if len(edge.snapshot_id) == 0: continue - edge_filepath = os.path.join( - upload_filepath, "edge_snapshots", edge.snapshot_id - ) + edge_filepath = os.path.join(upload_filepath, "edge_snapshots", edge.snapshot_id) if not os.path.exists(edge_filepath): continue with open(edge_filepath, "rb") as snapshot_file: @@ -394,9 +353,7 @@ def upload_graph_and_snapshots(self, upload_filepath: str): ) return - self._graph_nav_client.upload_graph( - lease=self._lease.lease_proto, graph=self._current_graph - ) + self._graph_nav_client.upload_graph(lease=self._lease.lease_proto, graph=self._current_graph) # Upload the snapshots to the robot. for waypoint_snapshot in self._current_waypoint_snapshots.values(): self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) @@ -418,7 +375,7 @@ def upload_graph_and_snapshots(self, upload_filepath: str): def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: """Navigate to a specific waypoint.""" - self._lease = self._lease_wallet.get_lease() + self._lease = self._get_lease() destination_waypoint = self._find_unique_waypoint_id( waypoint_id, self._current_graph, @@ -426,9 +383,7 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: self._logger, ) if not destination_waypoint: - self._logger.error( - "Failed to find the appropriate unique waypoint id for the navigation command." - ) + self._logger.error("Failed to find the appropriate unique waypoint id for the navigation command.") return ( False, "Failed to find the appropriate unique waypoint id for the navigation command.", @@ -445,9 +400,7 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: while not is_finished: # Issue the navigation command about twice a second such that it is easy to terminate the # navigation command (with estop or killing the program). - nav_to_cmd_id = self._graph_nav_client.navigate_to( - destination_waypoint, 1.0, leases=[sublease.lease_proto] - ) + nav_to_cmd_id = self._graph_nav_client.navigate_to(destination_waypoint, 1.0, leases=[sublease.lease_proto]) time.sleep(0.5) # Sleep for half a second to allow for command execution. # Poll the robot for feedback to determine if the navigation command is complete. is_finished = self._check_success(nav_to_cmd_id) @@ -456,10 +409,7 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: self._lease_keepalive = LeaseKeepAlive(self._lease_client) status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): + if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: return True, "Successfully completed the navigation commands!" elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: return ( @@ -471,17 +421,12 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: False, "Robot got stuck when navigating the route, the robot will now sit down.", ) - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: return False, "Robot is impaired." else: return False, "Navigation command is not complete yet." - def _navigate_route( - self, waypoint_ids: typing.List[str] - ) -> typing.Tuple[bool, str]: + def _navigate_route(self, waypoint_ids: typing.List[str]) -> typing.Tuple[bool, str]: """Navigate through a specific route of waypoints. Note that each waypoint must have an edge between them, aka be adjacent. """ @@ -493,9 +438,7 @@ def _navigate_route( self._logger, ) if not waypoint_ids[i]: - self._logger.error( - "navigate_route: Failed to find the unique waypoint id." - ) + self._logger.error("navigate_route: Failed to find the unique waypoint id.") return False, "Failed to find the unique waypoint id." edge_ids_list = [] @@ -508,9 +451,7 @@ def _navigate_route( if edge_id is not None: edge_ids_list.append(edge_id) else: - self._logger.error( - f"Failed to find an edge between waypoints: {start_wp} and {end_wp}" - ) + self._logger.error(f"Failed to find an edge between waypoints: {start_wp} and {end_wp}") return ( False, f"Failed to find an edge between waypoints: {start_wp} and {end_wp}", @@ -541,6 +482,7 @@ def _navigate_route( def clear_graph(self, *args) -> bool: """Clear the state of the map on the robot, removing all waypoints and edges.""" + self._lease = self._get_lease() result = self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) self._init_current_graph_nav_state() return result @@ -551,26 +493,16 @@ def _check_success(self, command_id: int = -1) -> bool: # No command, so we have not status to check. return False status = self._graph_nav_client.navigation_feedback(command_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): + if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: # Successfully completed the navigation commands! return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - self._logger.error( - "Robot got lost when navigating the route, the robot will now sit down." - ) + self._logger.error("Robot got lost when navigating the route, the robot will now sit down.") return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - self._logger.error( - "Robot got stuck when navigating the route, the robot will now sit down." - ) + self._logger.error("Robot got stuck when navigating the route, the robot will now sit down.") return True - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: self._logger.error("Robot is impaired.") return True else: @@ -589,74 +521,49 @@ def _match_edge( for edge_from_id in current_edges[edge_to_id]: if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint2, to_waypoint=waypoint1 - ) + return map_pb2.Edge.Id(from_waypoint=waypoint2, to_waypoint=waypoint1) elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint1, to_waypoint=waypoint2 - ) + return map_pb2.Edge.Id(from_waypoint=waypoint1, to_waypoint=waypoint2) return None - def _auto_close_loops( - self, close_fiducial_loops: bool, close_odometry_loops: bool, *args - ): + def _auto_close_loops(self, close_fiducial_loops: bool, close_odometry_loops: bool, *args): """Automatically find and close all loops in the graph.""" - response: map_processing_pb2.ProcessTopologyResponse = ( - self._map_processing_client.process_topology( - params=map_processing_pb2.ProcessTopologyRequest.Params( - do_fiducial_loop_closure=wrappers_pb2.BoolValue( - value=close_fiducial_loops - ), - do_odometry_loop_closure=wrappers_pb2.BoolValue( - value=close_odometry_loops - ), - ), - modify_map_on_server=True, - ) - ) - self._logger.info( - "Created {} new edge(s).".format(len(response.new_subgraph.edges)) + response: map_processing_pb2.ProcessTopologyResponse = self._map_processing_client.process_topology( + params=map_processing_pb2.ProcessTopologyRequest.Params( + do_fiducial_loop_closure=wrappers_pb2.BoolValue(value=close_fiducial_loops), + do_odometry_loop_closure=wrappers_pb2.BoolValue(value=close_odometry_loops), + ), + modify_map_on_server=True, ) + self._logger.info("Created {} new edge(s).".format(len(response.new_subgraph.edges))) if response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_OK: return True, "Successfully closed loops." - elif ( - response.status - == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS - ): + elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: return False, "Missing waypoint snapshots." - elif ( - response.status - == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH - ): + elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH: return False, "Invalid graph." - elif ( - response.status - == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING - ): + elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: return False, "Map modified during processing." else: return False, "Unknown error during map processing." def _optimize_anchoring(self, *args): """Call anchoring optimization on the server, producing a globally optimal reference frame for waypoints to be expressed in.""" - response: map_processing_pb2.ProcessAnchoringResponse = ( - self._map_processing_client.process_anchoring( - params=map_processing_pb2.ProcessAnchoringRequest.Params(), - modify_anchoring_on_server=True, - stream_intermediate_results=False, - ) + response: map_processing_pb2.ProcessAnchoringResponse = self._map_processing_client.process_anchoring( + params=map_processing_pb2.ProcessAnchoringRequest.Params(), + modify_anchoring_on_server=True, + stream_intermediate_results=False, ) if response.status == map_processing_pb2.ProcessAnchoringResponse.STATUS_OK: - self._logger.info( - "Optimized anchoring after {} iteration(s).".format(response.iteration) - ) + self._logger.info("Optimized anchoring after {} iteration(s).".format(response.iteration)) return True, "Successfully optimized anchoring." else: self._logger.error("Error optimizing {}".format(response)) status_mapping = { - map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: "Missing waypoint snapshots.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: ( + "Missing waypoint snapshots." + ), map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAPH: "Invalid graph.", map_processing_pb2.ProcessAnchoringResponse.STATUS_OPTIMIZATION_FAILURE: "Optimization failure.", map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_PARAMS: "Invalid parameters.", @@ -664,8 +571,12 @@ def _optimize_anchoring(self, *args): map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_ITERATIONS: "Max iterations, timeout.", map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_TIME: "Max time reached, timeout.", map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_HINTS: "Invalid hints.", - map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: "Map modified during processing.", - map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT: "Invalid gravity alignment.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: ( + "Map modified during processing." + ), + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT: ( + "Invalid gravity alignment." + ), } if response.status in status_mapping: return False, status_mapping[response.status] @@ -747,17 +658,12 @@ def _update_waypoints_and_edges( # Determine the timestamp that this waypoint was created at. timestamp = -1.0 try: - timestamp = ( - waypoint.annotations.creation_time.seconds - + waypoint.annotations.creation_time.nanos / 1e9 - ) + timestamp = waypoint.annotations.creation_time.seconds + waypoint.annotations.creation_time.nanos / 1e9 except: # Must be operating on an older graph nav map, since the creation_time is not # available within the waypoint annotations message. pass - waypoint_to_timestamp.append( - (waypoint.id, timestamp, waypoint.annotations.name) - ) + waypoint_to_timestamp.append((waypoint.id, timestamp, waypoint.annotations.name)) # Determine how many waypoints have the same short code. short_code = self._id_to_short_code(waypoint.id) @@ -779,17 +685,13 @@ def _update_waypoints_and_edges( # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, # fallback to sorting by annotation name. - waypoint_to_timestamp = sorted( - waypoint_to_timestamp, key=lambda x: (x[1], x[2]) - ) + waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from # when the waypoint was created. logger.info("%d waypoints:" % len(graph.waypoints)) for waypoint in waypoint_to_timestamp: - self._pretty_print_waypoints( - waypoint[0], waypoint[2], short_code_to_count, localization_id, logger - ) + self._pretty_print_waypoints(waypoint[0], waypoint[2], short_code_to_count, localization_id, logger) for edge in graph.edges: if edge.id.to_waypoint in edges: @@ -797,8 +699,6 @@ def _update_waypoints_and_edges( edges[edge.id.to_waypoint].append(edge.id.from_waypoint) else: edges[edge.id.to_waypoint] = [edge.id.from_waypoint] - logger.info( - f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}" - ) + logger.info(f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}") return name_to_id, edges From d64f65faa6e026fd5f7af824573ba40e307bad73 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 10 Jun 2023 17:07:57 +0100 Subject: [PATCH 51/66] fix black --- spot_wrapper/spot_graph_nav.py | 215 ++++++++++++++++++++++-------- spot_wrapper/wrapper.py | 235 ++++++++++++++++++++++++--------- 2 files changed, 337 insertions(+), 113 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 7282025c..ff383900 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -28,7 +28,9 @@ def __init__( self._robot = robot self._logger = logger self._graph_nav_client: GraphNavClient = robot_clients["graph_nav_client"] - self._map_processing_client: MapProcessingServiceClient = robot_clients["map_processing_client"] + self._map_processing_client: MapProcessingServiceClient = robot_clients[ + "map_processing_client" + ] self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] self._lease_client: LeaseClient = robot_clients["lease_client"] self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet @@ -47,7 +49,9 @@ def _init_current_graph_nav_state(self): self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() - self._current_anchored_world_objects = dict() # maps object id to a (wo, waypoint, fiducial) + self._current_anchored_world_objects = ( + dict() + ) # maps object id to a (wo, waypoint, fiducial) self._current_anchors = dict() # maps anchor id to anchor def list_graph(self) -> typing.List[str]: @@ -57,7 +61,12 @@ def list_graph(self) -> typing.List[str]: """ ids, eds = self._list_graph_waypoint_and_edge_ids() - return [v for k, v in sorted(ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")))] + return [ + v + for k, v in sorted( + ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) + ) + ] def navigate_initial_localization( self, @@ -92,7 +101,9 @@ def navigate_initial_localization( self.upload_graph_and_snapshots(upload_filepath) else: self._download_current_graph() - self._logger.info("Re-using existing graph on robot. Check that the correct graph is loaded!") + self._logger.info( + "Re-using existing graph on robot. Check that the correct graph is loaded!" + ) if initial_localization_fiducial: self.set_initial_localization_fiducial() if initial_localization_waypoint: @@ -130,7 +141,9 @@ def download_navigation_graph(self, download_path: str) -> typing.List[str]: self._download_full_graph() return self.list_graph() - def navigation_close_loops(self, close_fiducial_loops: bool, close_odometry_loops: bool) -> typing.Tuple[bool, str]: + def navigation_close_loops( + self, close_fiducial_loops: bool, close_odometry_loops: bool + ) -> typing.Tuple[bool, str]: return self._auto_close_loops(close_fiducial_loops, close_odometry_loops) def optmize_anchoring(self) -> typing.Tuple[bool, str]: @@ -146,13 +159,19 @@ def _get_localization_state(self, *args): """Get the current localization and state of the robot.""" state = self._graph_nav_client.get_localization_state() self._logger.info(f"Got localization: \n{str(state.localization)}") - odom_tform_body = get_odom_tform_body(state.robot_kinematics.transforms_snapshot) - self._logger.info(f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}") + odom_tform_body = get_odom_tform_body( + state.robot_kinematics.transforms_snapshot + ) + self._logger.info( + f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}" + ) def set_initial_localization_fiducial(self, *args): """Trigger localization when near a fiducial.""" robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot).to_proto() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() # Create an empty instance for initial localization since we are asking it to localize # based on the nearest fiducial. localization = nav_pb2.Localization() @@ -179,7 +198,9 @@ def set_initial_localization_waypoint(self, *args): return robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot).to_proto() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() # Create an initial localization to the specified waypoint as the identity. localization = nav_pb2.Localization() localization.waypoint_id = destination_waypoint @@ -209,7 +230,9 @@ def _download_full_graph(self, *args): return self._write_full_graph(graph) self._logger.info( - "Graph downloaded with {} waypoints and {} edges".format(len(graph.waypoints), len(graph.edges)) + "Graph downloaded with {} waypoints and {} edges".format( + len(graph.waypoints), len(graph.edges) + ) ) # Download the waypoint and edge snapshots. self._download_and_write_waypoint_snapshots(graph.waypoints) @@ -227,10 +250,14 @@ def _download_and_write_waypoint_snapshots(self, waypoints): if len(waypoint.snapshot_id) == 0: continue try: - waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot(waypoint.snapshot_id) + waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot( + waypoint.snapshot_id + ) except Exception: # Failure in downloading waypoint snapshot. Continue to next snapshot. - self._logger.error("Failed to download waypoint snapshot: " + waypoint.snapshot_id) + self._logger.error( + "Failed to download waypoint snapshot: " + waypoint.snapshot_id + ) continue self._write_bytes( self._download_filepath + "/waypoint_snapshots", @@ -253,10 +280,14 @@ def _download_and_write_edge_snapshots(self, edges): continue num_to_download += 1 try: - edge_snapshot = self._graph_nav_client.download_edge_snapshot(edge.snapshot_id) + edge_snapshot = self._graph_nav_client.download_edge_snapshot( + edge.snapshot_id + ) except Exception: # Failure in downloading edge snapshot. Continue to next snapshot. - self._logger.error("Failed to download edge snapshot: " + edge.snapshot_id) + self._logger.error( + "Failed to download edge snapshot: " + edge.snapshot_id + ) continue self._write_bytes( self._download_filepath + "/edge_snapshots", @@ -265,7 +296,9 @@ def _download_and_write_edge_snapshots(self, edges): ) num_edge_snapshots_downloaded += 1 self._logger.info( - "Downloaded {} of the total {} edge snapshots.".format(num_edge_snapshots_downloaded, num_to_download) + "Downloaded {} of the total {} edge snapshots.".format( + num_edge_snapshots_downloaded, num_to_download + ) ) def _write_bytes(self, filepath: str, filename: str, data): @@ -281,7 +314,9 @@ def _list_graph_waypoint_and_edge_ids(self, *args): # Download current graph graph = self._download_current_graph() - localization_id = self._graph_nav_client.get_localization_state().localization.waypoint_id + localization_id = ( + self._graph_nav_client.get_localization_state().localization.waypoint_id + ) # Update and print waypoints and edges ( @@ -307,13 +342,17 @@ def upload_graph_and_snapshots(self, upload_filepath: str): # Load the waypoint snapshots from disk. if len(waypoint.snapshot_id) == 0: continue - waypoint_filepath = os.path.join(upload_filepath, "waypoint_snapshots", waypoint.snapshot_id) + waypoint_filepath = os.path.join( + upload_filepath, "waypoint_snapshots", waypoint.snapshot_id + ) if not os.path.exists(waypoint_filepath): continue with open(waypoint_filepath, "rb") as snapshot_file: waypoint_snapshot = map_pb2.WaypointSnapshot() waypoint_snapshot.ParseFromString(snapshot_file.read()) - self._current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot + self._current_waypoint_snapshots[ + waypoint_snapshot.id + ] = waypoint_snapshot for fiducial in waypoint_snapshot.objects: if not fiducial.HasField("apriltag_properties"): @@ -335,7 +374,9 @@ def upload_graph_and_snapshots(self, upload_filepath: str): # Load the edge snapshots from disk. if len(edge.snapshot_id) == 0: continue - edge_filepath = os.path.join(upload_filepath, "edge_snapshots", edge.snapshot_id) + edge_filepath = os.path.join( + upload_filepath, "edge_snapshots", edge.snapshot_id + ) if not os.path.exists(edge_filepath): continue with open(edge_filepath, "rb") as snapshot_file: @@ -353,7 +394,9 @@ def upload_graph_and_snapshots(self, upload_filepath: str): ) return - self._graph_nav_client.upload_graph(lease=self._lease.lease_proto, graph=self._current_graph) + self._graph_nav_client.upload_graph( + lease=self._lease.lease_proto, graph=self._current_graph + ) # Upload the snapshots to the robot. for waypoint_snapshot in self._current_waypoint_snapshots.values(): self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) @@ -383,7 +426,9 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: self._logger, ) if not destination_waypoint: - self._logger.error("Failed to find the appropriate unique waypoint id for the navigation command.") + self._logger.error( + "Failed to find the appropriate unique waypoint id for the navigation command." + ) return ( False, "Failed to find the appropriate unique waypoint id for the navigation command.", @@ -400,7 +445,9 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: while not is_finished: # Issue the navigation command about twice a second such that it is easy to terminate the # navigation command (with estop or killing the program). - nav_to_cmd_id = self._graph_nav_client.navigate_to(destination_waypoint, 1.0, leases=[sublease.lease_proto]) + nav_to_cmd_id = self._graph_nav_client.navigate_to( + destination_waypoint, 1.0, leases=[sublease.lease_proto] + ) time.sleep(0.5) # Sleep for half a second to allow for command execution. # Poll the robot for feedback to determine if the navigation command is complete. is_finished = self._check_success(nav_to_cmd_id) @@ -409,7 +456,10 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: self._lease_keepalive = LeaseKeepAlive(self._lease_client) status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) - if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): return True, "Successfully completed the navigation commands!" elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: return ( @@ -421,12 +471,17 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: False, "Robot got stuck when navigating the route, the robot will now sit down.", ) - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): return False, "Robot is impaired." else: return False, "Navigation command is not complete yet." - def _navigate_route(self, waypoint_ids: typing.List[str]) -> typing.Tuple[bool, str]: + def _navigate_route( + self, waypoint_ids: typing.List[str] + ) -> typing.Tuple[bool, str]: """Navigate through a specific route of waypoints. Note that each waypoint must have an edge between them, aka be adjacent. """ @@ -438,7 +493,9 @@ def _navigate_route(self, waypoint_ids: typing.List[str]) -> typing.Tuple[bool, self._logger, ) if not waypoint_ids[i]: - self._logger.error("navigate_route: Failed to find the unique waypoint id.") + self._logger.error( + "navigate_route: Failed to find the unique waypoint id." + ) return False, "Failed to find the unique waypoint id." edge_ids_list = [] @@ -451,7 +508,9 @@ def _navigate_route(self, waypoint_ids: typing.List[str]) -> typing.Tuple[bool, if edge_id is not None: edge_ids_list.append(edge_id) else: - self._logger.error(f"Failed to find an edge between waypoints: {start_wp} and {end_wp}") + self._logger.error( + f"Failed to find an edge between waypoints: {start_wp} and {end_wp}" + ) return ( False, f"Failed to find an edge between waypoints: {start_wp} and {end_wp}", @@ -493,16 +552,26 @@ def _check_success(self, command_id: int = -1) -> bool: # No command, so we have not status to check. return False status = self._graph_nav_client.navigation_feedback(command_id) - if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): # Successfully completed the navigation commands! return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - self._logger.error("Robot got lost when navigating the route, the robot will now sit down.") + self._logger.error( + "Robot got lost when navigating the route, the robot will now sit down." + ) return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - self._logger.error("Robot got stuck when navigating the route, the robot will now sit down.") + self._logger.error( + "Robot got stuck when navigating the route, the robot will now sit down." + ) return True - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): self._logger.error("Robot is impaired.") return True else: @@ -521,42 +590,69 @@ def _match_edge( for edge_from_id in current_edges[edge_to_id]: if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id(from_waypoint=waypoint2, to_waypoint=waypoint1) + return map_pb2.Edge.Id( + from_waypoint=waypoint2, to_waypoint=waypoint1 + ) elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id(from_waypoint=waypoint1, to_waypoint=waypoint2) + return map_pb2.Edge.Id( + from_waypoint=waypoint1, to_waypoint=waypoint2 + ) return None - def _auto_close_loops(self, close_fiducial_loops: bool, close_odometry_loops: bool, *args): + def _auto_close_loops( + self, close_fiducial_loops: bool, close_odometry_loops: bool, *args + ): """Automatically find and close all loops in the graph.""" - response: map_processing_pb2.ProcessTopologyResponse = self._map_processing_client.process_topology( - params=map_processing_pb2.ProcessTopologyRequest.Params( - do_fiducial_loop_closure=wrappers_pb2.BoolValue(value=close_fiducial_loops), - do_odometry_loop_closure=wrappers_pb2.BoolValue(value=close_odometry_loops), - ), - modify_map_on_server=True, + response: map_processing_pb2.ProcessTopologyResponse = ( + self._map_processing_client.process_topology( + params=map_processing_pb2.ProcessTopologyRequest.Params( + do_fiducial_loop_closure=wrappers_pb2.BoolValue( + value=close_fiducial_loops + ), + do_odometry_loop_closure=wrappers_pb2.BoolValue( + value=close_odometry_loops + ), + ), + modify_map_on_server=True, + ) + ) + self._logger.info( + "Created {} new edge(s).".format(len(response.new_subgraph.edges)) ) - self._logger.info("Created {} new edge(s).".format(len(response.new_subgraph.edges))) if response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_OK: return True, "Successfully closed loops." - elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS + ): return False, "Missing waypoint snapshots." - elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH: + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH + ): return False, "Invalid graph." - elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING + ): return False, "Map modified during processing." else: return False, "Unknown error during map processing." def _optimize_anchoring(self, *args): """Call anchoring optimization on the server, producing a globally optimal reference frame for waypoints to be expressed in.""" - response: map_processing_pb2.ProcessAnchoringResponse = self._map_processing_client.process_anchoring( - params=map_processing_pb2.ProcessAnchoringRequest.Params(), - modify_anchoring_on_server=True, - stream_intermediate_results=False, + response: map_processing_pb2.ProcessAnchoringResponse = ( + self._map_processing_client.process_anchoring( + params=map_processing_pb2.ProcessAnchoringRequest.Params(), + modify_anchoring_on_server=True, + stream_intermediate_results=False, + ) ) if response.status == map_processing_pb2.ProcessAnchoringResponse.STATUS_OK: - self._logger.info("Optimized anchoring after {} iteration(s).".format(response.iteration)) + self._logger.info( + "Optimized anchoring after {} iteration(s).".format(response.iteration) + ) return True, "Successfully optimized anchoring." else: self._logger.error("Error optimizing {}".format(response)) @@ -658,12 +754,17 @@ def _update_waypoints_and_edges( # Determine the timestamp that this waypoint was created at. timestamp = -1.0 try: - timestamp = waypoint.annotations.creation_time.seconds + waypoint.annotations.creation_time.nanos / 1e9 + timestamp = ( + waypoint.annotations.creation_time.seconds + + waypoint.annotations.creation_time.nanos / 1e9 + ) except: # Must be operating on an older graph nav map, since the creation_time is not # available within the waypoint annotations message. pass - waypoint_to_timestamp.append((waypoint.id, timestamp, waypoint.annotations.name)) + waypoint_to_timestamp.append( + (waypoint.id, timestamp, waypoint.annotations.name) + ) # Determine how many waypoints have the same short code. short_code = self._id_to_short_code(waypoint.id) @@ -685,13 +786,17 @@ def _update_waypoints_and_edges( # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, # fallback to sorting by annotation name. - waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) + waypoint_to_timestamp = sorted( + waypoint_to_timestamp, key=lambda x: (x[1], x[2]) + ) # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from # when the waypoint was created. logger.info("%d waypoints:" % len(graph.waypoints)) for waypoint in waypoint_to_timestamp: - self._pretty_print_waypoints(waypoint[0], waypoint[2], short_code_to_count, localization_id, logger) + self._pretty_print_waypoints( + waypoint[0], waypoint[2], short_code_to_count, localization_id, logger + ) for edge in graph.edges: if edge.id.to_waypoint in edges: @@ -699,6 +804,8 @@ def _update_waypoints_and_edges( edges[edge.id.to_waypoint].append(edge.id.from_waypoint) else: edges[edge.id.to_waypoint] = [edge.id.from_waypoint] - logger.info(f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}") + logger.info( + f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}" + ) return name_to_id, edges diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 5c03d507..5952c9aa 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -115,7 +115,9 @@ class AsyncRobotState(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncRobotState, self).__init__("robot-state", client, logger, period_sec=1.0 / max(rate, 1.0)) + super(AsyncRobotState, self).__init__( + "robot-state", client, logger, period_sec=1.0 / max(rate, 1.0) + ) self._callback = None if rate > 0.0: self._callback = callback @@ -138,7 +140,9 @@ class AsyncMetrics(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncMetrics, self).__init__("robot-metrics", client, logger, period_sec=1.0 / max(rate, 1.0)) + super(AsyncMetrics, self).__init__( + "robot-metrics", client, logger, period_sec=1.0 / max(rate, 1.0) + ) self._callback = None if rate > 0.0: self._callback = callback @@ -161,7 +165,9 @@ class AsyncLease(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncLease, self).__init__("lease", client, logger, period_sec=1.0 / max(rate, 1.0)) + super(AsyncLease, self).__init__( + "lease", client, logger, period_sec=1.0 / max(rate, 1.0) + ) self._callback = None if rate > 0.0: self._callback = callback @@ -191,13 +197,19 @@ def __init__(self, client, logger, rate, spot_wrapper): def _start_query(self): if self._spot_wrapper._last_stand_command != None: try: - response = self._client.robot_command_feedback(self._spot_wrapper._last_stand_command) - status = response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status + response = self._client.robot_command_feedback( + self._spot_wrapper._last_stand_command + ) + status = ( + response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status + ) self._spot_wrapper._robot_params["is_sitting"] = False if status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING: self._spot_wrapper._robot_params["is_standing"] = True self._spot_wrapper._last_stand_command = None - elif status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS: + elif ( + status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS + ): self._spot_wrapper._robot_params["is_standing"] = False else: self._logger.warning("Stand command in unknown state") @@ -209,7 +221,9 @@ def _start_query(self): if self._spot_wrapper._last_sit_command != None: try: self._spot_wrapper._robot_params["is_standing"] = False - response = self._client.robot_command_feedback(self._spot_wrapper._last_sit_command) + response = self._client.robot_command_feedback( + self._spot_wrapper._last_sit_command + ) if ( response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status == basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING @@ -232,31 +246,49 @@ def _start_query(self): if self._spot_wrapper._last_trajectory_command != None: try: - response = self._client.robot_command_feedback(self._spot_wrapper._last_trajectory_command) + response = self._client.robot_command_feedback( + self._spot_wrapper._last_trajectory_command + ) status = ( response.feedback.synchronized_feedback.mobility_command_feedback.se2_trajectory_feedback.status ) # STATUS_AT_GOAL always means that the robot reached the goal. If the trajectory command did not # request precise positioning, then STATUS_NEAR_GOAL also counts as reaching the goal - if status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL or ( - status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL - and not self._spot_wrapper._last_trajectory_command_precise + if ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL + or ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL + and not self._spot_wrapper._last_trajectory_command_precise + ) ): self._spot_wrapper._robot_params["at_goal"] = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None self._spot_wrapper._trajectory_status_unknown = False - elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL: + elif ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL + ): is_moving = True - elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL: + elif ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL + ): is_moving = True self._spot_wrapper._robot_params["near_goal"] = True - elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN: + elif ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN + ): self._spot_wrapper._trajectory_status_unknown = True self._spot_wrapper._last_trajectory_command = None else: self._logger.error( - "Received trajectory command status outside of expected range, value is {}".format(status) + "Received trajectory command status outside of expected range, value is {}".format( + status + ) ) self._spot_wrapper._last_trajectory_command = None except (ResponseError, RpcError) as e: @@ -290,7 +322,9 @@ class AsyncEStopMonitor(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, spot_wrapper): - super(AsyncEStopMonitor, self).__init__("estop_alive", client, logger, period_sec=1.0 / rate) + super(AsyncEStopMonitor, self).__init__( + "estop_alive", client, logger, period_sec=1.0 / rate + ) self._spot_wrapper = spot_wrapper def _start_query(self): @@ -299,10 +333,20 @@ def _start_query(self): return last_estop_status = self._spot_wrapper._estop_keepalive.status_queue.queue[-1] - if last_estop_status[0] == self._spot_wrapper._estop_keepalive.KeepAliveStatus.ERROR: - self._logger.error("Estop keepalive has an error: {}".format(last_estop_status[1])) - elif last_estop_status == self._spot_wrapper._estop_keepalive.KeepAliveStatus.DISABLED: - self._logger.error("Estop keepalive is disabled: {}".format(last_estop_status[1])) + if ( + last_estop_status[0] + == self._spot_wrapper._estop_keepalive.KeepAliveStatus.ERROR + ): + self._logger.error( + "Estop keepalive has an error: {}".format(last_estop_status[1]) + ) + elif ( + last_estop_status + == self._spot_wrapper._estop_keepalive.KeepAliveStatus.DISABLED + ): + self._logger.error( + "Estop keepalive is disabled: {}".format(last_estop_status[1]) + ) else: # estop keepalive is ok pass @@ -426,7 +470,9 @@ def __init__( self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) - authenticated = self.authenticate(self._robot, self._username, self._password, self._logger) + authenticated = self.authenticate( + self._robot, self._username, self._password, self._logger + ) if not authenticated: self._valid = False return @@ -441,35 +487,65 @@ def __init__( initialised = False while not initialised: try: - self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name) - self._world_objects_client = self._robot.ensure_client(WorldObjectClient.default_service_name) - self._robot_command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) - self._graph_nav_client = self._robot.ensure_client(GraphNavClient.default_service_name) - self._map_processing_client = self._robot.ensure_client(MapProcessingServiceClient.default_service_name) - self._power_client = self._robot.ensure_client(PowerClient.default_service_name) - self._lease_client = self._robot.ensure_client(LeaseClient.default_service_name) + self._robot_state_client = self._robot.ensure_client( + RobotStateClient.default_service_name + ) + self._world_objects_client = self._robot.ensure_client( + WorldObjectClient.default_service_name + ) + self._robot_command_client = self._robot.ensure_client( + RobotCommandClient.default_service_name + ) + self._graph_nav_client = self._robot.ensure_client( + GraphNavClient.default_service_name + ) + self._map_processing_client = self._robot.ensure_client( + MapProcessingServiceClient.default_service_name + ) + self._power_client = self._robot.ensure_client( + PowerClient.default_service_name + ) + self._lease_client = self._robot.ensure_client( + LeaseClient.default_service_name + ) self._lease_wallet = self._lease_client.lease_wallet - self._image_client = self._robot.ensure_client(ImageClient.default_service_name) - self._estop_client = self._robot.ensure_client(EstopClient.default_service_name) - self._docking_client = self._robot.ensure_client(DockingClient.default_service_name) - self._spot_check_client = self._robot.ensure_client(SpotCheckClient.default_service_name) + self._image_client = self._robot.ensure_client( + ImageClient.default_service_name + ) + self._estop_client = self._robot.ensure_client( + EstopClient.default_service_name + ) + self._docking_client = self._robot.ensure_client( + DockingClient.default_service_name + ) + self._spot_check_client = self._robot.ensure_client( + SpotCheckClient.default_service_name + ) try: - self._point_cloud_client = self._robot.ensure_client(VELODYNE_SERVICE_NAME) + self._point_cloud_client = self._robot.ensure_client( + VELODYNE_SERVICE_NAME + ) except UnregisteredServiceError as e: self._point_cloud_client = None self._logger.info("Velodyne point cloud service is not available.") - self._license_client = self._robot.ensure_client(LicenseClient.default_service_name) + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) if HAVE_CHOREOGRAPHY_MODULE: - if self._license_client.get_feature_enabled([ChoreographyClient.license_name])[ - ChoreographyClient.license_name - ]: + if self._license_client.get_feature_enabled( + [ChoreographyClient.license_name] + )[ChoreographyClient.license_name]: self._sdk.register_service_client(ChoreographyClient) - self._choreography_client = self._robot.ensure_client(ChoreographyClient.default_service_name) + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) self._is_licensed_for_choreography = True else: - self._logger.info(f"Robot is not licensed for choreography: {e}") + self._logger.info( + f"Robot is not licensed for choreography: {e}" + ) self._is_licensed_for_choreography = False self._choreography_client = None else: @@ -508,7 +584,9 @@ def __init__( sleep_secs = 15 self._logger.warning( "Unable to create client service: {}. This usually means the robot hasn't " - "finished booting yet. Will wait {} seconds and try again.".format(e, sleep_secs) + "finished booting yet. Will wait {} seconds and try again.".format( + e, sleep_secs + ) ) time.sleep(sleep_secs) @@ -532,8 +610,12 @@ def __init__( max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", None), ) - self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) - self._estop_monitor = AsyncEStopMonitor(self._estop_client, self._logger, 20.0, self) + self._idle_task = AsyncIdle( + self._robot_command_client, self._logger, 10.0, self + ) + self._estop_monitor = AsyncEStopMonitor( + self._estop_client, self._logger, 20.0, self + ) self._estop_endpoint = None self._estop_keepalive = None @@ -560,7 +642,9 @@ def __init__( "rates": self._rates, "callbacks": self._callbacks, } - self.spot_image = SpotImages(self._robot, self._logger, self._robot_params, self._robot_clients) + self.spot_image = SpotImages( + self._robot, self._logger, self._robot_params, self._robot_clients + ) if self._robot.has_arm(): self._spot_arm = SpotArm( @@ -575,9 +659,15 @@ def __init__( else: self._spot_arm = None - self._spot_docking = SpotDocking(self._robot, self._logger, self._robot_params, self._robot_clients) - self._spot_graph_nav = SpotGraphNav(self._robot, self._logger, self._robot_params, self._robot_clients) - self._spot_check = SpotCheck(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_docking = SpotDocking( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_graph_nav = SpotGraphNav( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_check = SpotCheck( + self._robot, self._logger, self._robot_params, self._robot_clients + ) self._spot_images = SpotImages( self._robot, self._logger, @@ -587,13 +677,17 @@ def __init__( ) if self._point_cloud_client: - self._spot_eap = SpotEAP(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_eap = SpotEAP( + self._robot, self._logger, self._robot_params, self._robot_clients + ) self._point_cloud_task = self._spot_eap.async_task robot_tasks.append(self._point_cloud_task) else: self._spot_eap = None - self._spot_world_objects = SpotWorldObjects(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_world_objects = SpotWorldObjects( + self._robot, self._logger, self._robot_params, self._robot_clients + ) self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) @@ -790,7 +884,10 @@ def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" if self.lease is not None: for resource in self.lease: - if resource.resource == "all-leases" and SPOT_CLIENT_NAME in resource.lease_owner.client_name: + if ( + resource.resource == "all-leases" + and SPOT_CLIENT_NAME in resource.lease_owner.client_name + ): return True, "We already claimed the lease" try: @@ -819,7 +916,9 @@ def updateTasks(self): def resetEStop(self): """Get keepalive for eStop""" - self._estop_endpoint = EstopEndpoint(self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout) + self._estop_endpoint = EstopEndpoint( + self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout + ) self._estop_endpoint.force_simple_setup() # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) @@ -932,13 +1031,17 @@ def sit(self): @try_claim(power_on=True) def simple_stand(self, monitor_command=True): """If the e-stop is enabled, and the motor power is enabled, stand the robot up.""" - response = self._robot_command(RobotCommandBuilder.synchro_stand_command(params=self._mobility_params)) + response = self._robot_command( + RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) + ) if monitor_command: self._last_stand_command = response[2] return response[0], response[1] @try_claim(power_on=True) - def stand(self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, body_roll=0): + def stand( + self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, body_roll=0 + ): """ If the e-stop is enabled, and the motor power is enabled, stand the robot up. Executes a stand command, but one where the robot will assume the pose specified by the given parameters. @@ -957,11 +1060,15 @@ def stand(self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, b # If any of the orientation parameters are nonzero use them to pose the body body_orientation = EulerZXY(yaw=body_yaw, pitch=body_pitch, roll=body_roll) response = self._robot_command( - RobotCommandBuilder.synchro_stand_command(body_height=body_height, footprint_R_body=body_orientation) + RobotCommandBuilder.synchro_stand_command( + body_height=body_height, footprint_R_body=body_orientation + ) ) else: # Otherwise just use the mobility params - response = self._robot_command(RobotCommandBuilder.synchro_stand_command(params=self._mobility_params)) + response = self._robot_command( + RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) + ) if monitor_command: self._last_stand_command = response[2] @@ -976,7 +1083,9 @@ def battery_change_pose(self, dir_hint: int = 1): dir_hint: 1 rolls to the right side of the robot, 2 to the left """ if self._robot_params["is_sitting"]: - response = self._robot_command(RobotCommandBuilder.battery_change_pose_command(dir_hint)) + response = self._robot_command( + RobotCommandBuilder.battery_change_pose_command(dir_hint) + ) return response[0], response[1] return False, "Call sit before trying to roll over" @@ -989,7 +1098,9 @@ def safe_power_off(self): def clear_behavior_fault(self, id): """Clear the behavior fault defined by id.""" try: - rid = self._robot_command_client.clear_behavior_fault(behavior_fault_id=id, lease=None) + rid = self._robot_command_client.clear_behavior_fault( + behavior_fault_id=id, lease=None + ) return True, "Success", rid except Exception as e: return False, f"Exception while clearing behavior fault: {e}", None @@ -1042,7 +1153,9 @@ def velocity_cmd( """ end_time = time.time() + cmd_duration response = self._robot_command( - RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), + RobotCommandBuilder.synchro_velocity_command( + v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params + ), end_time_secs=end_time, timesync_endpoint=self._robot.time_sync.endpoint, ) @@ -1124,7 +1237,9 @@ def trajectory_cmd( self._last_trajectory_command = response[2] return response[0], response[1] - def robot_command(self, robot_command: robot_command_pb2.RobotCommand) -> typing.Tuple[bool, str]: + def robot_command( + self, robot_command: robot_command_pb2.RobotCommand + ) -> typing.Tuple[bool, str]: end_time = time.time() + MAX_COMMAND_DURATION return self._robot_command( robot_command, @@ -1132,7 +1247,9 @@ def robot_command(self, robot_command: robot_command_pb2.RobotCommand) -> typing timesync_endpoint=self._robot.time_sync.endpoint, ) - def get_robot_command_feedback(self, cmd_id: int) -> robot_command_pb2.RobotCommandFeedbackResponse: + def get_robot_command_feedback( + self, cmd_id: int + ) -> robot_command_pb2.RobotCommandFeedbackResponse: return self._robot_command_client.robot_command_feedback(cmd_id) def check_is_powered_on(self): From 7843a486e4c770cc96e6cba7dd587aadb484f5b3 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 10 Jun 2023 17:09:49 +0100 Subject: [PATCH 52/66] import ordering and removal of unused imports --- spot_wrapper/cam_wrapper.py | 14 +++++++------- spot_wrapper/spot_arm.py | 26 +++++++++++++------------- spot_wrapper/spot_check.py | 8 ++++---- spot_wrapper/spot_dance.py | 2 +- spot_wrapper/spot_docking.py | 6 +++--- spot_wrapper/spot_eap.py | 3 +-- spot_wrapper/spot_images.py | 6 +++--- spot_wrapper/spot_world_objects.py | 2 +- spot_wrapper/wrapper.py | 15 ++++++--------- 9 files changed, 39 insertions(+), 43 deletions(-) diff --git a/spot_wrapper/cam_wrapper.py b/spot_wrapper/cam_wrapper.py index 00460422..ea348f44 100644 --- a/spot_wrapper/cam_wrapper.py +++ b/spot_wrapper/cam_wrapper.py @@ -10,19 +10,19 @@ import cv2 import numpy as np from aiortc import RTCConfiguration +from bosdyn.api.spot_cam import audio_pb2 +from bosdyn.api.spot_cam.ptz_pb2 import PtzDescription, PtzVelocity, PtzPosition from bosdyn.client import Robot from bosdyn.client import spot_cam +from bosdyn.client.payload import PayloadClient +from bosdyn.client.spot_cam.audio import AudioClient from bosdyn.client.spot_cam.compositor import CompositorClient +from bosdyn.client.spot_cam.health import HealthClient from bosdyn.client.spot_cam.lighting import LightingClient +from bosdyn.client.spot_cam.media_log import MediaLogClient from bosdyn.client.spot_cam.power import PowerClient -from bosdyn.client.spot_cam.health import HealthClient -from bosdyn.client.spot_cam.audio import AudioClient -from bosdyn.client.spot_cam.streamquality import StreamQualityClient from bosdyn.client.spot_cam.ptz import PtzClient -from bosdyn.client.spot_cam.media_log import MediaLogClient -from bosdyn.client.payload import PayloadClient -from bosdyn.api.spot_cam.ptz_pb2 import PtzDescription, PtzVelocity, PtzPosition -from bosdyn.api.spot_cam import audio_pb2 +from bosdyn.client.spot_cam.streamquality import StreamQualityClient from spot_wrapper.cam_webrtc_client import WebRTCClient from spot_wrapper.wrapper import SpotWrapper diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index bfb34095..9b143356 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -1,22 +1,22 @@ +import logging import time import typing -import logging -from bosdyn.util import seconds_to_duration -from bosdyn.client.async_tasks import AsyncPeriodicQuery -from bosdyn.client import robot_command -from bosdyn.client.robot import Robot -from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient -from bosdyn.client.image import ImageClient, build_image_request -from bosdyn.client.manipulation_api_client import ManipulationApiClient -from bosdyn.api import robot_command_pb2 from bosdyn.api import arm_command_pb2 -from bosdyn.api import synchronized_command_pb2 from bosdyn.api import geometry_pb2 -from bosdyn.api import trajectory_pb2 -from bosdyn.api import manipulation_api_pb2 from bosdyn.api import image_pb2 +from bosdyn.api import manipulation_api_pb2 +from bosdyn.api import robot_command_pb2 +from bosdyn.api import synchronized_command_pb2 +from bosdyn.api import trajectory_pb2 +from bosdyn.client import robot_command +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.image import ImageClient, build_image_request +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.robot import Robot +from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.util import seconds_to_duration from google.protobuf.duration_pb2 import Duration """List of hand image sources for asynchronous periodic query""" diff --git a/spot_wrapper/spot_check.py b/spot_wrapper/spot_check.py index ddeab131..0a5eec1e 100644 --- a/spot_wrapper/spot_check.py +++ b/spot_wrapper/spot_check.py @@ -1,14 +1,14 @@ -import typing import logging import time +import typing -from bosdyn.client.robot import Robot +from bosdyn.api import header_pb2 from bosdyn.client import robot_command +from bosdyn.client.lease import LeaseClient, LeaseWallet, Lease +from bosdyn.client.robot import Robot from bosdyn.client.spot_check import SpotCheckClient, run_spot_check from bosdyn.client.spot_check import spot_check_pb2 -from bosdyn.api import header_pb2 from google.protobuf.timestamp_pb2 import Timestamp -from bosdyn.client.lease import LeaseClient, LeaseWallet, Lease class SpotCheck: diff --git a/spot_wrapper/spot_dance.py b/spot_wrapper/spot_dance.py index e4882de3..485e5859 100644 --- a/spot_wrapper/spot_dance.py +++ b/spot_wrapper/spot_dance.py @@ -1,12 +1,12 @@ import time +from bosdyn.choreography.client.choreography import ChoreographyClient from bosdyn.choreography.client.choreography import ( load_choreography_sequence_from_txt_file, ) from bosdyn.client import ResponseError from bosdyn.client.exceptions import UnauthenticatedError from bosdyn.client.robot import Robot -from bosdyn.choreography.client.choreography import ChoreographyClient class SpotDance: diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py index eb6eec7e..1eaf91f7 100644 --- a/spot_wrapper/spot_docking.py +++ b/spot_wrapper/spot_docking.py @@ -1,10 +1,10 @@ -import typing import logging +import typing -from bosdyn.client.robot import Robot +from bosdyn.api.docking import docking_pb2 from bosdyn.client import robot_command from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock -from bosdyn.api.docking import docking_pb2 +from bosdyn.client.robot import Robot class SpotDocking: diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py index 05557655..548d6ff1 100644 --- a/spot_wrapper/spot_eap.py +++ b/spot_wrapper/spot_eap.py @@ -1,10 +1,9 @@ -import typing import logging +import typing from bosdyn.client.async_tasks import AsyncPeriodicQuery from bosdyn.client.point_cloud import PointCloudClient, build_pc_request from bosdyn.client.robot import Robot -from bosdyn.client import robot_command """List of point cloud sources""" point_cloud_sources = ["velodyne-point-cloud"] diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 563a5291..5e3c6476 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,15 +1,15 @@ -import typing import logging +import typing from collections import namedtuple from dataclasses import dataclass -from bosdyn.client.robot import Robot +from bosdyn.api import image_pb2 from bosdyn.client.image import ( ImageClient, build_image_request, UnsupportedPixelFormatRequestedError, ) -from bosdyn.api import image_pb2 +from bosdyn.client.robot import Robot """List of body image sources for periodic query""" CAMERA_IMAGE_SOURCES = [ diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py index da7061a3..ae228729 100644 --- a/spot_wrapper/spot_world_objects.py +++ b/spot_wrapper/spot_world_objects.py @@ -1,5 +1,5 @@ -import typing import logging +import typing from bosdyn.client.async_tasks import AsyncPeriodicQuery from bosdyn.client.robot import Robot diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 5952c9aa..70002f57 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -6,38 +6,35 @@ import bosdyn.client.auth from bosdyn.api import image_pb2 +from bosdyn.api import lease_pb2 +from bosdyn.api import point_cloud_pb2 from bosdyn.api import robot_command_pb2 from bosdyn.api import robot_state_pb2 from bosdyn.api import world_object_pb2 -from bosdyn.api import lease_pb2 -from bosdyn.api import point_cloud_pb2 from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 from bosdyn.client import create_standard_sdk, ResponseError, RpcError from bosdyn.client import frame_helpers from bosdyn.client import math_helpers -from bosdyn.client import robot_command -from bosdyn.client.robot import UnregisteredServiceError -from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks from bosdyn.client.docking import DockingClient from bosdyn.client.estop import ( EstopClient, EstopEndpoint, EstopKeepAlive, - MotorsOnError, ) from bosdyn.client.graph_nav import GraphNavClient -from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.image import ImageClient from bosdyn.client.lease import LeaseClient, LeaseKeepAlive +from bosdyn.client.license import LicenseClient from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.power import PowerClient from bosdyn.client.robot import UnregisteredServiceError from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.world_object import WorldObjectClient from bosdyn.client.spot_check import SpotCheckClient -from bosdyn.client.license import LicenseClient +from bosdyn.client.time_sync import TimeSyncEndpoint +from bosdyn.client.world_object import WorldObjectClient try: from bosdyn.choreography.client.choreography import ( From e6df4814f459baebc3d7453bac98b61164f49949 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 10 Jun 2023 17:20:18 +0100 Subject: [PATCH 53/66] wait for arm commands to complete rather than sleeping, using block_until_arm_arrives SDK function --- spot_wrapper/spot_arm.py | 68 +++++++++++++++++++++------------------- 1 file changed, 35 insertions(+), 33 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 9b143356..62a61123 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -14,7 +14,11 @@ from bosdyn.client.image import ImageClient, build_image_request from bosdyn.client.manipulation_api_client import ManipulationApiClient from bosdyn.client.robot import Robot -from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.robot_command import ( + RobotCommandBuilder, + RobotCommandClient, + block_until_arm_arrives, +) from bosdyn.client.robot_state import RobotStateClient from bosdyn.util import seconds_to_duration from google.protobuf.duration_pb2 import Duration @@ -180,6 +184,19 @@ def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: return True, "Spot has an arm, is powered on, and standing" + def wait_for_arm_command_to_complete(self, cmd_id, timeout_sec=None): + """ + Wrapper around the SDK function for convenience + + Args: + cmd_id: ID of the command that we are waiting on + timeout_sec: After this time, timeout regardless of what the robot state is + + """ + block_until_arm_arrives( + self._robot_command_client, cmd_id=cmd_id, timeout_sec=timeout_sec + ) + def arm_stow(self) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() @@ -191,9 +208,9 @@ def arm_stow(self) -> typing.Tuple[bool, str]: stow = RobotCommandBuilder.arm_stow_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(stow) + cmd_id = self._robot_command_client.robot_command(stow) self._logger.info("Command stow issued") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while trying to stow: {e}" @@ -211,9 +228,9 @@ def arm_unstow(self) -> typing.Tuple[bool, str]: unstow = RobotCommandBuilder.arm_ready_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(unstow) + cmd_id = self._robot_command_client.robot_command(unstow) self._logger.info("Command unstow issued") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while trying to unstow: {e}" @@ -231,9 +248,9 @@ def arm_carry(self) -> typing.Tuple[bool, str]: carry = RobotCommandBuilder.arm_carry_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(carry) + cmd_id = self._robot_command_client.robot_command(carry) self._logger.info("Command carry issued") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while carry mode was issued: {e}" @@ -322,19 +339,7 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: # Send the request cmd_id = self._robot_command_client.robot_command(arm_command) - - # Query for feedback to determine how long it will take - feedback_resp = self._robot_command_client.robot_command_feedback( - cmd_id - ) - joint_move_feedback = ( - feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback - ) - time_to_goal: Duration = joint_move_feedback.time_to_goal - time_to_goal_in_seconds: float = time_to_goal.seconds + ( - float(time_to_goal.nanos) / float(10**9) - ) - time.sleep(time_to_goal_in_seconds) + self.wait_for_arm_command_to_complete(cmd_id) return True, "Spot Arm moved successfully" except Exception as e: @@ -398,11 +403,9 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: ) # Send the request - self._robot_command_client.robot_command(robot_command) + cmd_id = self._robot_command_client.robot_command(robot_command) self._logger.info("Force trajectory command sent") - - time.sleep(float(traj_duration) + 1.0) - + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured during arm movement: {e}" @@ -419,9 +422,9 @@ def gripper_open(self) -> typing.Tuple[bool, str]: command = RobotCommandBuilder.claw_gripper_open_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) + cmd_id = self._robot_command_client.robot_command(command) self._logger.info("Command gripper open sent") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while gripper was moving: {e}" @@ -439,9 +442,9 @@ def gripper_close(self) -> typing.Tuple[bool, str]: command = RobotCommandBuilder.claw_gripper_close_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) + cmd_id = self._robot_command_client.robot_command(command) self._logger.info("Command gripper close sent") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while gripper was moving: {e}" @@ -467,9 +470,9 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: command = RobotCommandBuilder.claw_gripper_open_angle_command(angle) # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) + cmd_id = self._robot_command_client.robot_command(command) self._logger.info("Command gripper open angle sent") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while gripper was moving: {e}" @@ -533,10 +536,9 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: command = RobotCommandBuilder.build_synchro_command(robot_command) # Send the request - self._robot_command_client.robot_command(robot_command) + cmd_id = self._robot_command_client.robot_command(robot_command) self._logger.info("Moving arm to position.") - - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return ( From ea44635319e216081fd0fbc71e48c7ff7dfe9b05 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 18 Jun 2023 13:30:55 +0100 Subject: [PATCH 54/66] no longer use convenience dict to access robot params in wrapper class --- spot_wrapper/wrapper.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 70002f57..9ed103ec 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -794,7 +794,7 @@ def is_valid(self) -> bool: @property def id(self) -> str: """Return robot's ID""" - return self._robot_params["robot_id"] + return self._robot_id @property def robot_state(self) -> robot_state_pb2.RobotState: @@ -829,25 +829,25 @@ def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: @property def is_standing(self) -> bool: """Return boolean of standing state""" - return self._robot_params["is_standing"] + return self._is_standing @property def is_sitting(self) -> bool: """Return boolean of standing state""" - return self._robot_params["is_sitting"] + return self._is_sitting @property def is_moving(self) -> bool: """Return boolean of walking state""" - return self._robot_params["is_moving"] + return self._is_moving @property def near_goal(self) -> bool: - return self._robot_params["near_goal"] + return self._near_goal @property def at_goal(self) -> bool: - return self._robot_params["at_goal"] + return self._at_goal def is_estopped(self, timeout=None) -> bool: return self._robot.is_estopped(timeout=timeout) @@ -888,7 +888,7 @@ def claim(self) -> typing.Tuple[bool, str]: return True, "We already claimed the lease" try: - self._robot_params["robot_id"] = self._robot.get_id() + self._robot_id = self._robot.get_id() self.getLease() if self._start_estop and not self.check_is_powered_on(): # If we are requested to start the estop, and the robot is not already powered on, then we reset the From 7e9515c79cc2e21e714f41910c7f5f2c47e951e3 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 18 Jun 2023 13:32:37 +0100 Subject: [PATCH 55/66] missed some robot params usages in wrapper --- spot_wrapper/wrapper.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9ed103ec..5ac4a195 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1079,7 +1079,7 @@ def battery_change_pose(self, dir_hint: int = 1): Args: dir_hint: 1 rolls to the right side of the robot, 2 to the left """ - if self._robot_params["is_sitting"]: + if self._is_sitting: response = self._robot_command( RobotCommandBuilder.battery_change_pose_command(dir_hint) ) @@ -1187,8 +1187,8 @@ def trajectory_cmd( if mobility_params is None: mobility_params = self._mobility_params self._trajectory_status_unknown = False - self._robot_params["at_goal"] = False - self._robot_params["near_goal"] = False + self._at_goal = False + self._near_goal = False self._last_trajectory_command_precise = precise_position self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration From 776e84009a3fefd0949397d57316cbb0b45e1399 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 18 Jun 2023 14:31:23 +0100 Subject: [PATCH 56/66] add block_until_manipulation_completes method --- spot_wrapper/spot_arm.py | 101 ++++++++++++++++++++++++++------------- 1 file changed, 67 insertions(+), 34 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 62a61123..ebb21ca7 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -548,7 +548,62 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: return True, "Moved arm successfully" - def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): + @staticmethod + def block_until_manipulation_completes( + manipulation_client: ManipulationApiClient, + cmd_id: int, + timeout_sec: float = None, + ): + """ + Helper that blocks until the arm achieves a finishing state for the specific manipulation command. + + Args: + manipulation_client: manipulation client, used to request feedback + cmd_id: command ID returned by the robot when the arm movement command was sent. + timeout_sec: optional number of seconds after which we'll return no matter what + the robot's state is. + + Returns: + True if successfully completed the grasp, False if the grasp failed + """ + if timeout_sec is not None: + start_time = time.time() + end_time = start_time + timeout_sec + now = time.time() + + while timeout_sec is None or now < end_time: + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_id + ) + + # Send the request + response = manipulation_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) + manipulation_state = response.current_state + # TODO: Incorporate more of the feedback states if needed for different grasp commands. + if manipulation_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED: + return True + elif manipulation_state == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED: + return False + + time.sleep(0.1) + now = time.time() + return False + + def grasp_3d( + self, frame: str, object_rt_frame: typing.List[float] + ) -> typing.Tuple[bool, str]: + """ + Attempt to grasp an object + + Args: + frame: Frame in the which the object_rt_frame vector is given + object_rt_frame: xyz position of the object in the given frame + + Returns: + Bool indicating success, and a message with information. + """ try: frm = str(frame) pos = geometry_pb2.Vec3( @@ -566,39 +621,17 @@ def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): manipulation_api_request=grasp_request ) - # Get feedback from the robot - while True: - feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( - manipulation_cmd_id=cmd_response.manipulation_cmd_id - ) - - # Send the request - response = ( - self._manipulation_api_client.manipulation_api_feedback_command( - manipulation_api_feedback_request=feedback_request - ) - ) - - print( - "Current state: ", - manipulation_api_pb2.ManipulationFeedbackState.Name( - response.current_state - ), - ) - - if ( - response.current_state - == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED - or response.current_state - == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED - ): - break - - time.sleep(0.25) - - self._robot.logger.info("Finished grasp.") + success = self.block_until_manipulation_completes( + self._manipulation_api_client, cmd_response.cmd_id + ) + if success: + msg = "Grasped successfully" + self._logger.info(msg) + return True, msg + else: + msg = "Grasp failed." + self._logger.info(msg) + return False, msg except Exception as e: return False, f"An error occured while trying to grasp from pose {e}" - - return True, "Grasped successfully" From ef2174d8246c80e7dc6dad12aa8615d71da83b8f Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 18 Jun 2023 15:02:56 +0100 Subject: [PATCH 57/66] improve arm module comments, manipulation request actually makes use of the timesync endpoint and end time --- spot_wrapper/spot_arm.py | 88 +++++++++++++++++++++++++++++++--------- 1 file changed, 69 insertions(+), 19 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index ebb21ca7..4a599021 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -20,8 +20,8 @@ block_until_arm_arrives, ) from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.util import seconds_to_duration -from google.protobuf.duration_pb2 import Duration """List of hand image sources for asynchronous periodic query""" HAND_IMAGE_SOURCES = [ @@ -95,12 +95,14 @@ def __init__( self._rates: typing.Dict[str, float] = robot_params["rates"] self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + self._hand_image_requests = [] self._hand_image_task: AsyncImageService = None self.initialize_hand_image_service() def initialize_hand_image_service(self): - self._hand_image_requests = [] - + """ + Initialises the hand image task for retrieving the hand image + """ for source in HAND_IMAGE_SOURCES: self._hand_image_requests.append( build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) @@ -114,19 +116,21 @@ def initialize_hand_image_service(self): self._hand_image_requests, ) - self.camera_task_name_to_task_mapping: typing.Dict[str, AsyncImageService] = { - "hand_image": self._hand_image_task, - } - @property - def hand_image_task(self): + def hand_image_task(self) -> AsyncImageService: + """ + Get the hand image task + + Returns: + Async image service for getting the hand image + """ return self._hand_image_task def _manipulation_request( self, request_proto: manipulation_api_pb2, - end_time_secs=None, - timesync_endpoint=None, + end_time_secs: typing.Optional[float] = None, + timesync_endpoint: typing.Optional[TimeSyncEndpoint] = None, ): """Generic function for sending requests to the manipulation api of a robot. Args: @@ -134,7 +138,9 @@ def _manipulation_request( """ try: command_id = self._manipulation_api_client.manipulation_api_command( - manipulation_api_request=request_proto + manipulation_api_request=request_proto, + end_time_secs=end_time_secs, + timesync_endpoint=timesync_endpoint, ).manipulation_cmd_id return True, "Success", command_id @@ -186,7 +192,7 @@ def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: def wait_for_arm_command_to_complete(self, cmd_id, timeout_sec=None): """ - Wrapper around the SDK function for convenience + Wait until a command issued to the arm complets. Wrapper around the SDK function for convenience Args: cmd_id: ID of the command that we are waiting on @@ -198,6 +204,12 @@ def wait_for_arm_command_to_complete(self, cmd_id, timeout_sec=None): ) def arm_stow(self) -> typing.Tuple[bool, str]: + """ + Moves the arm to the stowed position + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -218,6 +230,12 @@ def arm_stow(self) -> typing.Tuple[bool, str]: return True, "Stow arm success" def arm_unstow(self) -> typing.Tuple[bool, str]: + """ + Unstows the arm + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -345,7 +363,7 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: except Exception as e: return False, f"Exception occured during arm movement: {e}" - def create_wrench_from_msg(self, forces, torques): + def create_wrench_from_forces_and_torques(self, forces, torques): force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) return geometry_pb2.Wrench(force=force, torque=torque) @@ -361,14 +379,18 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: traj_duration = data.duration # first point on trajectory - wrench0 = self.create_wrench_from_msg(data.forces_pt0, data.torques_pt0) + wrench0 = self.create_wrench_from_forces_and_torques( + data.forces_pt0, data.torques_pt0 + ) t0 = seconds_to_duration(0) traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench0, time_since_reference=t0 ) # Second point on the trajectory - wrench1 = self.create_wrench_from_msg(data.forces_pt1, data.torques_pt1) + wrench1 = self.create_wrench_from_forces_and_torques( + data.forces_pt1, data.torques_pt1 + ) t1 = seconds_to_duration(traj_duration) traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench1, time_since_reference=t1 @@ -412,6 +434,12 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: return True, "Moved arm successfully" def gripper_open(self) -> typing.Tuple[bool, str]: + """ + Fully opens the gripper + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -432,6 +460,12 @@ def gripper_open(self) -> typing.Tuple[bool, str]: return True, "Open gripper success" def gripper_close(self) -> typing.Tuple[bool, str]: + """ + Closes the gripper + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -452,8 +486,15 @@ def gripper_close(self) -> typing.Tuple[bool, str]: return True, "Closed gripper successfully" def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: - # takes an angle between 0 (closed) and 90 (fully opened) and opens the - # gripper at this angle + """ + Takes an angle between 0 (closed) and 90 (fully opened) and opens the gripper at this angle + + Args: + gripper_ang: Angle to which the gripper should be opened + + Returns: + Boolean success, string message + """ if gripper_ang > 90 or gripper_ang < 0: return False, "Gripper angle must be between 0 and 90" try: @@ -480,6 +521,15 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: return True, "Opened gripper successfully" def hand_pose(self, data) -> typing.Tuple[bool, str]: + """ + Set the pose of the hand + + Args: + data: + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -516,7 +566,7 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: ) arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( - root_frame_name=data.frame, + root_frame_name=frame, pose_trajectory_in_task=hand_trajectory, force_remain_near_current_joint_configuration=True, ) @@ -553,7 +603,7 @@ def block_until_manipulation_completes( manipulation_client: ManipulationApiClient, cmd_id: int, timeout_sec: float = None, - ): + ) -> bool: """ Helper that blocks until the arm achieves a finishing state for the specific manipulation command. From 8b1c477f1897a60b75d3d875d28bea7c84b3ed23 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 27 Jun 2023 19:20:13 +0100 Subject: [PATCH 58/66] restore wrapper to main branch state --- spot_wrapper/wrapper.py | 2202 ++++++++++++++++++++++++++++++++------- 1 file changed, 1838 insertions(+), 364 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index e4668932..9d6a34c7 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1,40 +1,56 @@ import functools import logging +import math +import os import time import traceback import typing +from enum import Enum +from collections import namedtuple +from dataclasses import dataclass, field import bosdyn.client.auth +from bosdyn.api import arm_command_pb2 +from bosdyn.api import geometry_pb2 from bosdyn.api import image_pb2 -from bosdyn.api import lease_pb2 -from bosdyn.api import point_cloud_pb2 +from bosdyn.api import manipulation_api_pb2 from bosdyn.api import robot_command_pb2 from bosdyn.api import robot_state_pb2 -from bosdyn.api import world_object_pb2 -from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 +from bosdyn.api import synchronized_command_pb2 +from bosdyn.api import trajectory_pb2 +from bosdyn.api.graph_nav import graph_nav_pb2 +from bosdyn.api.graph_nav import map_pb2 +from bosdyn.api.graph_nav import nav_pb2 from bosdyn.client import create_standard_sdk, ResponseError, RpcError from bosdyn.client import frame_helpers from bosdyn.client import math_helpers +from bosdyn.client import robot_command from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks -from bosdyn.client.docking import DockingClient +from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock from bosdyn.client.estop import ( EstopClient, EstopEndpoint, EstopKeepAlive, + MotorsOnError, ) +from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.client.graph_nav import GraphNavClient -from bosdyn.client.image import ImageClient +from bosdyn.client.image import ( + ImageClient, + build_image_request, + UnsupportedPixelFormatRequestedError, +) from bosdyn.client.lease import LeaseClient, LeaseKeepAlive -from bosdyn.client.license import LicenseClient from bosdyn.client.manipulation_api_client import ManipulationApiClient -from bosdyn.client.map_processing import MapProcessingServiceClient -from bosdyn.client.power import PowerClient +from bosdyn.client.point_cloud import build_pc_request +from bosdyn.client.power import safe_power_off, PowerClient, power_on from bosdyn.client.robot import UnregisteredServiceError from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.spot_check import SpotCheckClient -from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.world_object import WorldObjectClient +from bosdyn.client.exceptions import UnauthenticatedError +from bosdyn.client.license import LicenseClient +from bosdyn.client import ResponseError, RpcError, create_standard_sdk try: from bosdyn.choreography.client.choreography import ( @@ -42,28 +58,130 @@ ) from .spot_dance import SpotDance - HAVE_CHOREOGRAPHY_MODULE = True + HAVE_CHOREOGRAPHY = True except ModuleNotFoundError: - HAVE_CHOREOGRAPHY_MODULE = False + HAVE_CHOREOGRAPHY = False from bosdyn.geometry import EulerZXY +from bosdyn.util import seconds_to_duration +from google.protobuf.duration_pb2 import Duration -from bosdyn.api import basic_command_pb2 -from google.protobuf.timestamp_pb2 import Timestamp +MAX_COMMAND_DURATION = 1e5 -from .spot_arm import SpotArm -from .spot_world_objects import SpotWorldObjects -from .spot_eap import SpotEAP -from .spot_docking import SpotDocking -from .spot_graph_nav import SpotGraphNav -from .spot_check import SpotCheck -from .spot_images import SpotImages +### Release +from . import graph_nav_util -SPOT_CLIENT_NAME = "ros_spot" -MAX_COMMAND_DURATION = 1e5 +### Debug +# import graph_nav_util -"""Service name for getting pointcloud of VLP16 connected to Spot Core""" +from bosdyn.api import basic_command_pb2 +from google.protobuf.timestamp_pb2 import Timestamp + +front_image_sources = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "frontleft_depth", + "frontright_depth", +] +"""List of image sources for front image periodic query""" +side_image_sources = [ + "left_fisheye_image", + "right_fisheye_image", + "left_depth", + "right_depth", +] +"""List of image sources for side image periodic query""" +rear_image_sources = ["back_fisheye_image", "back_depth"] +"""List of image sources for rear image periodic query""" VELODYNE_SERVICE_NAME = "velodyne-point-cloud" +"""Service name for getting pointcloud of VLP16 connected to Spot Core""" +point_cloud_sources = ["velodyne-point-cloud"] +"""List of point cloud sources""" +hand_image_sources = [ + "hand_image", + "hand_depth", + "hand_color_image", + "hand_depth_in_hand_color_frame", +] +"""List of image sources for hand image periodic query""" + + +# TODO: Missing Hand images +CAMERA_IMAGE_SOURCES = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "left_fisheye_image", + "right_fisheye_image", + "back_fisheye_image", +] +DEPTH_IMAGE_SOURCES = [ + "frontleft_depth", + "frontright_depth", + "left_depth", + "right_depth", + "back_depth", +] +DEPTH_REGISTERED_IMAGE_SOURCES = [ + "frontleft_depth_in_visual_frame", + "frontright_depth_in_visual_frame", + "right_depth_in_visual_frame", + "left_depth_in_visual_frame", + "back_depth_in_visual_frame", +] +ImageBundle = namedtuple( + "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] +) +ImageWithHandBundle = namedtuple( + "ImageBundle", ["frontleft", "frontright", "left", "right", "back", "hand"] +) + +IMAGE_SOURCES_BY_CAMERA = { + "frontleft": { + "visual": "frontleft_fisheye_image", + "depth": "frontleft_depth", + "depth_registered": "frontleft_depth_in_visual_frame", + }, + "frontright": { + "visual": "frontright_fisheye_image", + "depth": "frontright_depth", + "depth_registered": "frontright_depth_in_visual_frame", + }, + "left": { + "visual": "left_fisheye_image", + "depth": "left_depth", + "depth_registered": "left_depth_in_visual_frame", + }, + "right": { + "visual": "right_fisheye_image", + "depth": "right_depth", + "depth_registered": "right_depth_in_visual_frame", + }, + "back": { + "visual": "back_fisheye_image", + "depth": "back_depth", + "depth_registered": "back_depth_in_visual_frame", + }, + "hand": { + "visual": "hand_color_image", + "depth": "hand_depth", + "depth_registered": "hand_depth_in_hand_color_frame", + }, +} + +IMAGE_TYPES = {"visual", "depth", "depth_registered"} + + +@dataclass(frozen=True, eq=True) +class CameraSource: + camera_name: str + image_types: typing.List[str] + + +@dataclass(frozen=True) +class ImageEntry: + camera_name: str + image_type: str + image_response: image_pb2.ImageResponse def robotToLocalTime(timestamp, robot): @@ -92,14 +210,6 @@ def robotToLocalTime(timestamp, robot): return rtime -class MissingSpotArm(Exception): - """Raised when the arm is not available on the robot""" - - def __init__(self, message="Spot arm not available"): - # Call the base class constructor with the parameters it needs - super().__init__(message) - - class AsyncRobotState(AsyncPeriodicQuery): """Class to get robot state at regular intervals. get_robot_state_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -175,6 +285,62 @@ def _start_query(self): return callback_future +class AsyncImageService(AsyncPeriodicQuery): + """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback, image_requests): + super(AsyncImageService, self).__init__( + "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + self._image_requests = image_requests + + def _start_query(self): + if self._callback: + callback_future = self._client.get_image_async(self._image_requests) + callback_future.add_done_callback(self._callback) + return callback_future + + +class AsyncPointCloudService(AsyncPeriodicQuery): + """ + Class to get point cloud at regular intervals. get_point_cloud_from_sources_async query sent to the robot at + every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback, point_cloud_requests): + super(AsyncPointCloudService, self).__init__( + "robot_point_cloud_service", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + self._point_cloud_requests = point_cloud_requests + + def _start_query(self): + if self._callback and self._point_cloud_requests: + callback_future = self._client.get_point_cloud_async( + self._point_cloud_requests + ) + callback_future.add_done_callback(self._callback) + return callback_future + + class AsyncIdle(AsyncPeriodicQuery): """Class to check if the robot is moving, and if not, command a stand with the set mobility parameters @@ -199,24 +365,24 @@ def _start_query(self): status = ( response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status ) - self._spot_wrapper._robot_params["is_sitting"] = False + self._spot_wrapper._is_sitting = False if status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING: - self._spot_wrapper._robot_params["is_standing"] = True + self._spot_wrapper._is_standing = True self._spot_wrapper._last_stand_command = None elif ( status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS ): - self._spot_wrapper._robot_params["is_standing"] = False + self._spot_wrapper._is_standing = False else: self._logger.warning("Stand command in unknown state") - self._spot_wrapper._robot_params["is_standing"] = False + self._spot_wrapper._is_standing = False except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_stand_command = None if self._spot_wrapper._last_sit_command != None: try: - self._spot_wrapper._robot_params["is_standing"] = False + self._spot_wrapper._is_standing = False response = self._client.robot_command_feedback( self._spot_wrapper._last_sit_command ) @@ -224,10 +390,10 @@ def _start_query(self): response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status == basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING ): - self._spot_wrapper._robot_params["is_sitting"] = True + self._spot_wrapper._is_sitting = True self._spot_wrapper._last_sit_command = None else: - self._spot_wrapper._robot_params["is_sitting"] = False + self._spot_wrapper._is_sitting = False except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_sit_command = None @@ -259,7 +425,7 @@ def _start_query(self): and not self._spot_wrapper._last_trajectory_command_precise ) ): - self._spot_wrapper._robot_params["at_goal"] = True + self._spot_wrapper._at_goal = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None self._spot_wrapper._trajectory_status_unknown = False @@ -273,7 +439,7 @@ def _start_query(self): == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL ): is_moving = True - self._spot_wrapper._robot_params["near_goal"] = True + self._spot_wrapper._near_goal = True elif ( status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN @@ -291,14 +457,14 @@ def _start_query(self): self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_trajectory_command = None - self._spot_wrapper._robot_params["is_moving"] = is_moving + self._spot_wrapper._is_moving = is_moving # We must check if any command currently has a non-None value for its id. If we don't do this, this stand # command can cause other commands to be interrupted before they get to start if ( - self._spot_wrapper._robot_params["is_standing"] + self._spot_wrapper.is_standing and self._spot_wrapper._continually_try_stand - and not self._spot_wrapper._robot_params["is_moving"] + and not self._spot_wrapper.is_moving and self._spot_wrapper._last_trajectory_command is not None and self._spot_wrapper._last_stand_command is not None and self._spot_wrapper._last_velocity_command_time is not None @@ -348,6 +514,31 @@ def _start_query(self): pass +class AsyncWorldObjects(AsyncPeriodicQuery): + """Class to get world objects. list_world_objects_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback): + super(AsyncWorldObjects, self).__init__( + "world-objects", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + + def _start_query(self): + if self._callback: + callback_future = self._client.list_world_objects_async() + callback_future.add_done_callback(self._callback) + return callback_future + + def try_claim(func=None, *, power_on=False): """ Decorator which tries to acquire the lease before executing the wrapped function @@ -383,6 +574,8 @@ def wrapper_try_claim(self, *args, **kwargs): class SpotWrapper: """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically""" + SPOT_CLIENT_NAME = "ros_spot" + def __init__( self, username: str, @@ -392,8 +585,8 @@ def __init__( logger: logging.Logger, start_estop: bool = True, estop_timeout: float = 9.0, - rates: typing.Optional[typing.Dict[str, float]] = None, - callbacks: typing.Optional[typing.Dict[str, typing.Callable]] = None, + rates: typing.Optional[typing.Dict] = None, + callbacks: typing.Optional[typing.Dict] = None, use_take_lease: bool = False, get_lease_on_action: bool = False, continually_try_stand: bool = True, @@ -423,8 +616,6 @@ def __init__( self._password = password self._hostname = hostname self._robot_name = robot_name - self._rates = rates or {} - self._callbacks = callbacks or {} self._use_take_lease = use_take_lease self._get_lease_on_action = get_lease_on_action self._continually_try_stand = continually_try_stand @@ -433,6 +624,14 @@ def __init__( if robot_name is not None: self._frame_prefix = robot_name + "/" self._logger = logger + if rates is None: + self._rates = {} + else: + self._rates = rates + if callbacks is None: + self._callbacks = {} + else: + self._callbacks = callbacks self._estop_timeout = estop_timeout self._start_estop = start_estop self._keep_alive = True @@ -454,15 +653,71 @@ def __init__( self._last_velocity_command_time = None self._last_docking_command = None + self._front_image_requests = [] + for source in front_image_sources: + self._front_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._side_image_requests = [] + for source in side_image_sources: + self._side_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._rear_image_requests = [] + for source in rear_image_sources: + self._rear_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._point_cloud_requests = [] + for source in point_cloud_sources: + self._point_cloud_requests.append(build_pc_request(source)) + + self._hand_image_requests = [] + for source in hand_image_sources: + self._hand_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._camera_image_requests = [] + for camera_source in CAMERA_IMAGE_SOURCES: + self._camera_image_requests.append( + build_image_request( + camera_source, + image_format=image_pb2.Image.FORMAT_JPEG, + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8 + if self._rgb_cameras + else image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8, + quality_percent=50, + ) + ) + + self._depth_image_requests = [] + for camera_source in DEPTH_IMAGE_SOURCES: + self._depth_image_requests.append( + build_image_request( + camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + ) + ) + + self._depth_registered_image_requests = [] + for camera_source in DEPTH_REGISTERED_IMAGE_SOURCES: + self._depth_registered_image_requests.append( + build_image_request( + camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + ) + ) + try: - self._sdk = create_standard_sdk(SPOT_CLIENT_NAME) + self._sdk = create_standard_sdk(self.SPOT_CLIENT_NAME) except Exception as e: - self._logger.error(f"Error creating SDK object: {e}") + self._logger.error("Error creating SDK object: %s", e) self._valid = False return - if HAVE_CHOREOGRAPHY_MODULE: + if HAVE_CHOREOGRAPHY: self._sdk.register_service_client(ChoreographyClient) - self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) @@ -473,226 +728,250 @@ def __init__( self._valid = False return - if not self._robot: - self._logger.error("Failed to create robot object") - self._valid = False - return - - # Clients - self._logger.info("Creating clients...") - initialised = False - while not initialised: - try: - self._robot_state_client = self._robot.ensure_client( - RobotStateClient.default_service_name - ) - self._world_objects_client = self._robot.ensure_client( - WorldObjectClient.default_service_name - ) - self._robot_command_client = self._robot.ensure_client( - RobotCommandClient.default_service_name - ) - self._graph_nav_client = self._robot.ensure_client( - GraphNavClient.default_service_name - ) - self._map_processing_client = self._robot.ensure_client( - MapProcessingServiceClient.default_service_name - ) - self._power_client = self._robot.ensure_client( - PowerClient.default_service_name - ) - self._lease_client = self._robot.ensure_client( - LeaseClient.default_service_name - ) - self._lease_wallet = self._lease_client.lease_wallet - self._image_client = self._robot.ensure_client( - ImageClient.default_service_name - ) - self._estop_client = self._robot.ensure_client( - EstopClient.default_service_name - ) - self._docking_client = self._robot.ensure_client( - DockingClient.default_service_name - ) - self._spot_check_client = self._robot.ensure_client( - SpotCheckClient.default_service_name - ) + if self._robot: + # Clients + self._logger.info("Creating clients...") + initialised = False + while not initialised: try: - self._point_cloud_client = self._robot.ensure_client( - VELODYNE_SERVICE_NAME + self._robot_state_client = self._robot.ensure_client( + RobotStateClient.default_service_name + ) + self._world_objects_client = self._robot.ensure_client( + WorldObjectClient.default_service_name + ) + self._robot_command_client = self._robot.ensure_client( + RobotCommandClient.default_service_name + ) + self._graph_nav_client = self._robot.ensure_client( + GraphNavClient.default_service_name + ) + self._power_client = self._robot.ensure_client( + PowerClient.default_service_name + ) + self._lease_client = self._robot.ensure_client( + LeaseClient.default_service_name + ) + self._lease_wallet = self._lease_client.lease_wallet + self._image_client = self._robot.ensure_client( + ImageClient.default_service_name + ) + self._estop_client = self._robot.ensure_client( + EstopClient.default_service_name + ) + self._docking_client = self._robot.ensure_client( + DockingClient.default_service_name + ) + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name ) - except UnregisteredServiceError as e: - self._point_cloud_client = None - self._logger.info("Velodyne point cloud service is not available.") - self._license_client = self._robot.ensure_client( - LicenseClient.default_service_name - ) + if HAVE_CHOREOGRAPHY: + if self._license_client.get_feature_enabled( + [ChoreographyClient.license_name] + )[ChoreographyClient.license_name]: + self._is_licensed_for_choreography = True + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) + else: + self._logger.info( + f"Robot is not licensed for choreography: {e}" + ) + self._is_licensed_for_choreography = False + self._choreography_client = None + else: + self._logger.info(f"Choreography is not available.") + self._choreography_client = None + self._is_licensed_for_choreography = False - if HAVE_CHOREOGRAPHY_MODULE: - if self._license_client.get_feature_enabled( - [ChoreographyClient.license_name] - )[ChoreographyClient.license_name]: - self._sdk.register_service_client(ChoreographyClient) - self._choreography_client = self._robot.ensure_client( - ChoreographyClient.default_service_name + try: + self._point_cloud_client = self._robot.ensure_client( + VELODYNE_SERVICE_NAME ) - self._is_licensed_for_choreography = True - else: + except UnregisteredServiceError: + self._point_cloud_client = None self._logger.info( - f"Robot is not licensed for choreography: {e}" + "No velodyne point cloud service is available." ) - self._is_licensed_for_choreography = False - self._choreography_client = None - else: - self._logger.info(f"Choreography is not available.") - self._is_licensed_for_choreography = False - self._choreography_client = None - if self._robot.has_arm(): - self._manipulation_api_client = self._robot.ensure_client( - ManipulationApiClient.default_service_name + if self._robot.has_arm(): + self._manipulation_api_client = self._robot.ensure_client( + ManipulationApiClient.default_service_name + ) + else: + self._manipulation_api_client = None + self._logger.info("Manipulation API is not available.") + + initialised = True + except Exception as e: + sleep_secs = 15 + self._logger.warning( + "Unable to create client service: {}. This usually means the robot hasn't " + "finished booting yet. Will wait {} seconds and try again.".format( + e, sleep_secs + ) ) - else: - self._manipulation_api_client = None - self._logger.info("Manipulation API is not available.") - - self._robot_clients = { - "robot_state_client": self._robot_state_client, - "robot_command_client": self._robot_command_client, - "graph_nav_client": self._graph_nav_client, - "map_processing_client": self._map_processing_client, - "power_client": self._power_client, - "lease_client": self._lease_client, - "image_client": self._image_client, - "estop_client": self._estop_client, - "docking_client": self._docking_client, - "spot_check_client": self._spot_check_client, - "robot_command_method": self._robot_command, - "world_objects_client": self._world_objects_client, - "manipulation_api_client": self._manipulation_api_client, - "choreography_client": self._choreography_client, - "point_cloud_client": self._point_cloud_client, - } - - initialised = True - except Exception as e: - sleep_secs = 15 - self._logger.warning( - "Unable to create client service: {}. This usually means the robot hasn't " - "finished booting yet. Will wait {} seconds and try again.".format( - e, sleep_secs + time.sleep(sleep_secs) + + # Add hand camera requests + if self._robot.has_arm(): + self._camera_image_requests.append( + build_image_request( + "hand_color_image", + image_format=image_pb2.Image.FORMAT_JPEG, + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ) + self._depth_image_requests.append( + build_image_request( + "hand_depth", + pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, + ) + ) + self._depth_registered_image_requests.append( + build_image_request( + "hand_depth_in_hand_color_frame", + pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, ) ) - time.sleep(sleep_secs) - # Core Async Tasks - self._async_task_list = [] - self._robot_state_task = AsyncRobotState( - self._robot_state_client, - self._logger, - max(0.0, self._rates.get("robot_state", 0.0)), - self._callbacks.get("robot_state", None), - ) - self._robot_metrics_task = AsyncMetrics( - self._robot_state_client, - self._logger, - max(0.0, self._rates.get("metrics", 0.0)), - self._callbacks.get("metrics", None), - ) - self._lease_task = AsyncLease( - self._lease_client, - self._logger, - max(0.0, self._rates.get("lease", 0.0)), - self._callbacks.get("lease", None), - ) - self._idle_task = AsyncIdle( - self._robot_command_client, self._logger, 10.0, self - ) - self._estop_monitor = AsyncEStopMonitor( - self._estop_client, self._logger, 20.0, self - ) + # Build image requests by camera + self._image_requests_by_camera = {} + for camera in IMAGE_SOURCES_BY_CAMERA: + if camera == "hand" and not self._robot.has_arm(): + continue + self._image_requests_by_camera[camera] = {} + image_types = IMAGE_SOURCES_BY_CAMERA[camera] + for image_type in image_types: + if image_type.startswith("depth"): + image_format = image_pb2.Image.FORMAT_RAW + pixel_format = image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + else: + image_format = image_pb2.Image.FORMAT_JPEG + if camera == "hand" or self._rgb_cameras: + pixel_format = image_pb2.Image.PIXEL_FORMAT_RGB_U8 + elif camera != "hand": + self._logger.info( + f"Switching {camera}:{image_type} to greyscale image format." + ) + pixel_format = image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8 + + source = IMAGE_SOURCES_BY_CAMERA[camera][image_type] + self._image_requests_by_camera[camera][ + image_type + ] = build_image_request( + source, + image_format=image_format, + pixel_format=pixel_format, + quality_percent=75, + ) - self._estop_endpoint = None - self._estop_keepalive = None - self._robot_id = None - self._lease = None - - robot_tasks = [ - self._robot_state_task, - self._robot_metrics_task, - self._lease_task, - self._idle_task, - self._estop_monitor, - ] + # Store the most recent knowledge of the state of the robot based on rpc calls. + self._init_current_graph_nav_state() - # Create component objects for different functionality - self._robot_params = { - "is_standing": self._is_standing, - "is_sitting": self._is_sitting, - "is_moving": self._is_moving, - "at_goal": self._at_goal, - "near_goal": self._near_goal, - "robot_id": self._robot_id, - "estop_timeout": self._estop_timeout, - "rates": self._rates, - "callbacks": self._callbacks, - } - self.spot_image = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + # Async Tasks + self._async_task_list = [] + self._robot_state_task = AsyncRobotState( + self._robot_state_client, + self._logger, + max(0.0, self._rates.get("robot_state", 0.0)), + self._callbacks.get("robot_state", None), + ) + self._robot_metrics_task = AsyncMetrics( + self._robot_state_client, + self._logger, + max(0.0, self._rates.get("metrics", 0.0)), + self._callbacks.get("metrics", None), + ) + self._lease_task = AsyncLease( + self._lease_client, + self._logger, + max(0.0, self._rates.get("lease", 0.0)), + self._callbacks.get("lease", None), + ) + self._front_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("front_image", 0.0)), + self._callbacks.get("front_image", None), + self._front_image_requests, + ) + self._side_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("side_image", 0.0)), + self._callbacks.get("side_image", None), + self._side_image_requests, + ) + self._rear_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("rear_image", 0.0)), + self._callbacks.get("rear_image", None), + self._rear_image_requests, + ) + self._hand_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("hand_image", 0.0)), + self._callbacks.get("hand_image", None), + self._hand_image_requests, + ) - if self._robot.has_arm(): - self._spot_arm = SpotArm( - self._robot, + self._idle_task = AsyncIdle( + self._robot_command_client, self._logger, 10.0, self + ) + self._estop_monitor = AsyncEStopMonitor( + self._estop_client, self._logger, 20.0, self + ) + self._world_objects_task = AsyncWorldObjects( + self._world_objects_client, self._logger, - self._robot_params, - self._robot_clients, - MAX_COMMAND_DURATION, + 10.0, + self._callbacks.get("world_objects", None), ) - self._hand_image_task = self._spot_arm.hand_image_task - robot_tasks.append(self._hand_image_task) - else: - self._spot_arm = None - self._spot_docking = SpotDocking( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._spot_graph_nav = SpotGraphNav( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._spot_check = SpotCheck( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._spot_images = SpotImages( - self._robot, - self._logger, - self._robot_params, - self._robot_clients, - self._rgb_cameras, - ) + self._estop_endpoint = None + self._estop_keepalive = None - if self._point_cloud_client: - self._spot_eap = SpotEAP( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._point_cloud_task = self._spot_eap.async_task - robot_tasks.append(self._point_cloud_task) - else: - self._spot_eap = None + robot_tasks = [ + self._robot_state_task, + self._robot_metrics_task, + self._lease_task, + self._front_image_task, + self._idle_task, + self._estop_monitor, + self._world_objects_task, + ] + + if self._point_cloud_client: + self._point_cloud_task = AsyncPointCloudService( + self._point_cloud_client, + self._logger, + max(0.0, self._rates.get("point_cloud", 0.0)), + self._callbacks.get("lidar_points", None), + self._point_cloud_requests, + ) + robot_tasks.append(self._point_cloud_task) - self._spot_world_objects = SpotWorldObjects( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._world_objects_task = self._spot_world_objects.async_task - robot_tasks.append(self._world_objects_task) + self._async_tasks = AsyncTasks(robot_tasks) - if self._is_licensed_for_choreography: - self._spot_dance = SpotDance( - self._robot, self._choreography_client, self._logger - ) + self.camera_task_name_to_task_mapping = { + "hand_image": self._hand_image_task, + "side_image": self._side_image_task, + "rear_image": self._rear_image_task, + "front_image": self._front_image_task, + } - self._async_tasks = AsyncTasks(robot_tasks) + if self._is_licensed_for_choreography: + self._spot_dance = SpotDance( + self._robot, self._choreography_client, self._logger + ) + + self._robot_id = None + self._lease = None @staticmethod def authenticate(robot, username, password, logger): @@ -733,127 +1012,104 @@ def authenticate(robot, username, password, logger): return authenticated @property - def robot_name(self) -> str: + def robot_name(self): return self._robot_name @property - def frame_prefix(self) -> str: + def frame_prefix(self): return self._frame_prefix @property - def spot_images(self) -> SpotImages: - """Return SpotImages instance""" - return self._spot_images - - @property - def spot_arm(self) -> SpotArm: - """Return SpotArm instance""" - if not self._robot.has_arm(): - raise MissingSpotArm() - else: - return self._spot_arm - - @property - def spot_eap_lidar(self) -> SpotEAP: - """Return SpotEAP instance""" - return self._spot_eap - - @property - def spot_world_objects(self) -> SpotWorldObjects: - """Return SpotWorldObjects instance""" - return self._spot_world_objects - - @property - def spot_docking(self) -> SpotDocking: - """Return SpotDocking instance""" - return self._spot_docking - - @property - def spot_graph_nav(self) -> SpotGraphNav: - """Return SpotGraphNav instance""" - return self._spot_graph_nav - - @property - def spot_check(self) -> SpotCheck: - """Return SpotCheck instance""" - return self._spot_check - - @property - def logger(self) -> logging.Logger: + def logger(self): """Return logger instance of the SpotWrapper""" return self._logger @property - def is_valid(self) -> bool: + def is_valid(self): """Return boolean indicating if the wrapper initialized successfully""" return self._valid @property - def id(self) -> str: + def id(self): """Return robot's ID""" return self._robot_id @property - def robot_state(self) -> robot_state_pb2.RobotState: + def robot_state(self): """Return latest proto from the _robot_state_task""" return self._robot_state_task.proto @property - def metrics(self) -> robot_state_pb2.RobotMetrics: + def metrics(self): """Return latest proto from the _robot_metrics_task""" return self._robot_metrics_task.proto @property - def lease(self) -> typing.List[lease_pb2.LeaseResource]: + def lease(self): """Return latest proto from the _lease_task""" return self._lease_task.proto @property - def world_objects(self) -> world_object_pb2.ListWorldObjectResponse: + def world_objects(self): """Return most recent proto from _world_objects_task""" - return self.spot_world_objects.async_task.proto + return self._world_objects_task.proto + + @property + def front_images(self): + """Return latest proto from the _front_image_task""" + return self._front_image_task.proto + + @property + def side_images(self): + """Return latest proto from the _side_image_task""" + return self._side_image_task.proto + + @property + def rear_images(self): + """Return latest proto from the _rear_image_task""" + return self._rear_image_task.proto @property - def hand_images(self) -> typing.List[image_pb2.ImageResponse]: + def hand_images(self): """Return latest proto from the _hand_image_task""" - return self.spot_arm.hand_image_task.proto + return self._hand_image_task.proto @property - def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: + def point_clouds(self): """Return latest proto from the _point_cloud_task""" - return self.spot_eap_lidar.async_task.proto + return self._point_cloud_task.proto @property - def is_standing(self) -> bool: + def is_standing(self): """Return boolean of standing state""" return self._is_standing @property - def is_sitting(self) -> bool: + def is_sitting(self): """Return boolean of standing state""" return self._is_sitting @property - def is_moving(self) -> bool: + def is_moving(self): """Return boolean of walking state""" return self._is_moving @property - def near_goal(self) -> bool: + def near_goal(self): return self._near_goal @property - def at_goal(self) -> bool: + def at_goal(self): return self._at_goal - def is_estopped(self, timeout=None) -> bool: + def is_estopped(self, timeout=None): return self._robot.is_estopped(timeout=timeout) def has_arm(self, timeout=None): return self._robot.has_arm(timeout=timeout) @property - def time_skew(self) -> Timestamp: + def time_skew(self): """Return the time skew between local and spot time""" return self._robot.time_sync.endpoint.clock_skew @@ -864,7 +1120,7 @@ def resetMobilityParams(self): """ self._mobility_params = RobotCommandBuilder.mobility_params() - def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: + def robotToLocalTime(self, timestamp): """Takes a timestamp and an estimated skew and return seconds and nano seconds in local time Args: @@ -874,13 +1130,13 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: """ return robotToLocalTime(timestamp, self._robot) - def claim(self) -> typing.Tuple[bool, str]: + def claim(self): """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" if self.lease is not None: for resource in self.lease: if ( resource.resource == "all-leases" - and SPOT_CLIENT_NAME in resource.lease_owner.client_name + and self.SPOT_CLIENT_NAME in resource.lease_owner.client_name ): return True, "We already claimed the lease" @@ -898,7 +1154,7 @@ def claim(self) -> typing.Tuple[bool, str]: self._logger.error("Failed to initialize robot communication: %s", err) return False, str(err) except Exception as err: - self._logger.error(traceback.format_exc(), flush=True) + print(traceback.format_exc(), flush=True) return False, str(err) def updateTasks(self): @@ -906,12 +1162,12 @@ def updateTasks(self): try: self._async_tasks.update() except Exception as e: - self._logger.error(f"Update tasks failed with error: {str(e)}") + print(f"Update tasks failed with error: {str(e)}") def resetEStop(self): """Get keepalive for eStop""" self._estop_endpoint = EstopEndpoint( - self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout + self._estop_client, self.SPOT_CLIENT_NAME, self._estop_timeout ) self._estop_endpoint.force_simple_setup() # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) @@ -978,12 +1234,7 @@ def disconnect(self): self.releaseLease() self.releaseEStop() - def _robot_command( - self, - command_proto: robot_command_pb2.RobotCommand, - end_time_secs: typing.Optional[float] = None, - timesync_endpoint: typing.Optional[TimeSyncEndpoint] = None, - ) -> typing.Tuple[bool, str, typing.Optional[str]]: + def _robot_command(self, command_proto, end_time_secs=None, timesync_endpoint=None): """Generic blocking function for sending commands to robots. Args: @@ -1003,6 +1254,24 @@ def _robot_command( self._logger.error(f"Unable to execute robot command: {e}") return False, str(e), None + def _manipulation_request( + self, request_proto, end_time_secs=None, timesync_endpoint=None + ): + """Generic function for sending requests to the manipulation api of a robot. + + Args: + request_proto: manipulation_api_pb2 object to send to the robot. + """ + try: + command_id = self._manipulation_api_client.manipulation_api_command( + manipulation_api_request=request_proto + ).manipulation_cmd_id + + return True, "Success", command_id + except Exception as e: + self._logger.error(f"Unable to execute manipulation command: {e}") + return False, str(e), None + @try_claim def stop(self): """Stop the robot's motion.""" @@ -1117,7 +1386,7 @@ def power_on(self): return True, "Was already powered on" - def set_mobility_params(self, mobility_params: spot_command_pb2.MobilityParams): + def set_mobility_params(self, mobility_params): """Set Params for mobility and movement Args: @@ -1125,18 +1394,17 @@ def set_mobility_params(self, mobility_params: spot_command_pb2.MobilityParams): """ self._mobility_params = mobility_params - def get_mobility_params(self) -> spot_command_pb2.MobilityParams: + def get_mobility_params(self): """Get mobility params""" return self._mobility_params + def list_world_objects(self, object_types, time_start_point): + return self._world_objects_client.list_world_objects( + object_types, time_start_point + ) + @try_claim - def velocity_cmd( - self, - v_x: float, - v_y: float, - v_rot: float, - cmd_duration: float = 0.7, - ) -> typing.Tuple[bool, str]: + def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125): """Send a velocity motion command to the robot. Args: @@ -1159,14 +1427,14 @@ def velocity_cmd( @try_claim def trajectory_cmd( self, - goal_x: float, - goal_y: float, - goal_heading: float, - cmd_duration: float, - frame_name: str = "odom", - precise_position: bool = False, - mobility_params: spot_command_pb2.MobilityParams = None, - ) -> typing.Tuple[bool, str]: + goal_x, + goal_y, + goal_heading, + cmd_duration, + frame_name="odom", + precise_position=False, + mobility_params=None, + ): """Send a trajectory motion command to the robot. Args: @@ -1231,9 +1499,7 @@ def trajectory_cmd( self._last_trajectory_command = response[2] return response[0], response[1] - def robot_command( - self, robot_command: robot_command_pb2.RobotCommand - ) -> typing.Tuple[bool, str]: + def robot_command(self, robot_command): end_time = time.time() + MAX_COMMAND_DURATION return self._robot_command( robot_command, @@ -1241,33 +1507,1019 @@ def robot_command( timesync_endpoint=self._robot.time_sync.endpoint, ) - def get_robot_command_feedback( - self, cmd_id: int - ) -> robot_command_pb2.RobotCommandFeedbackResponse: + def manipulation_command(self, request): + end_time = time.time() + MAX_COMMAND_DURATION + return self._manipulation_request( + request, + end_time_secs=end_time, + timesync_endpoint=self._robot.time_sync.endpoint, + ) + + def get_robot_command_feedback(self, cmd_id): return self._robot_command_client.robot_command_feedback(cmd_id) - def check_is_powered_on(self): - """Determine if the robot is powered on or off.""" - power_state = self._robot_state_client.get_robot_state().power_state - self._powered_on = power_state.motor_power_state == power_state.STATE_ON - return self._powered_on + def get_manipulation_command_feedback(self, cmd_id): + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_id + ) - @try_claim - def execute_dance(self, data): - if self._is_licensed_for_choreography: - return self._spot_dance.execute_dance(data) - else: - return False, "Spot is not licensed for choreography" + return self._manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) - def upload_animation( - self, animation_name: str, animation_file_content: str - ) -> typing.Tuple[bool, str]: - if self._is_licensed_for_choreography: - return self._spot_dance.upload_animation( - animation_name, animation_file_content + def list_graph(self, upload_path): + """List waypoint ids of garph_nav + Args: + upload_path : Path to the root directory of the map. + """ + ids, eds = self._list_graph_waypoint_and_edge_ids() + # skip waypoint_ for v2.2.1, skip waypiont for < v2.2 + return [ + v + for k, v in sorted( + ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) ) - else: - return False, "Spot is not licensed for choreography" + ] + + @try_claim + def navigate_to( + self, + upload_path, + navigate_to, + initial_localization_fiducial=True, + initial_localization_waypoint=None, + ): + """navigate with graph nav. + + Args: + upload_path : Path to the root directory of the map. + navigate_to : Waypont id string for where to goal + initial_localization_fiducial : Tells the initializer whether to use fiducials + initial_localization_waypoint : Waypoint id string of current robot position (optional) + """ + # Filepath for uploading a saved graph's and snapshots too. + if upload_path[-1] == "/": + upload_filepath = upload_path[:-1] + else: + upload_filepath = upload_path + + # Boolean indicating the robot's power state. + power_state = self._robot_state_client.get_robot_state().power_state + self._started_powered_on = power_state.motor_power_state == power_state.STATE_ON + self._powered_on = self._started_powered_on + + # FIX ME somehow,,,, if the robot is stand, need to sit the robot before starting garph nav + if self.is_standing and not self.is_moving: + self.sit() + + # TODO verify estop / claim / power_on + self._clear_graph() + self._upload_graph_and_snapshots(upload_filepath) + if initial_localization_fiducial: + self._set_initial_localization_fiducial() + if initial_localization_waypoint: + self._set_initial_localization_waypoint([initial_localization_waypoint]) + self._list_graph_waypoint_and_edge_ids() + self._get_localization_state() + resp = self._navigate_to([navigate_to]) + + return resp + + # Arm ############################################ + @try_claim + def ensure_arm_power_and_stand(self): + if not self._robot.has_arm(): + return False, "Spot with an arm is required for this service" + + try: + if not self.check_is_powered_on(): + self._logger.info("Spot is powering on within the timeout of 20 secs") + self._robot.power_on(timeout_sec=20) + assert self._robot.is_powered_on(), "Spot failed to power on" + self._logger.info("Spot is powered on") + except Exception as e: + return ( + False, + f"Exception occured while Spot or its arm were trying to power on: {e}", + ) + + if not self._is_standing: + robot_command.blocking_stand( + command_client=self._robot_command_client, timeout_sec=10.0 + ) + self._logger.info("Spot is standing") + else: + self._logger.info("Spot is already standing") + + return True, "Spot has an arm, is powered on, and standing" + + @try_claim + def arm_stow(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Stow Arm + stow = RobotCommandBuilder.arm_stow_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(stow) + self._logger.info("Command stow issued") + time.sleep(2.0) + + except Exception as e: + return False, f"Exception occured while trying to stow: {e}" + + return True, "Stow arm success" + + @try_claim + def arm_unstow(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Unstow Arm + unstow = RobotCommandBuilder.arm_ready_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(unstow) + self._logger.info("Command unstow issued") + time.sleep(2.0) + + except Exception as e: + return False, f"Exception occured while trying to unstow: {e}" + + return True, "Unstow arm success" + + @try_claim + def arm_carry(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Get Arm in carry mode + carry = RobotCommandBuilder.arm_carry_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(carry) + self._logger.info("Command carry issued") + time.sleep(2.0) + + except Exception as e: + return False, f"Exception occured while carry mode was issued: {e}" + + return True, "Carry mode success" + + def make_arm_trajectory_command(self, arm_joint_trajectory): + """Helper function to create a RobotCommand from an ArmJointTrajectory. + Copy from 'spot-sdk/python/examples/arm_joint_move/arm_joint_move.py'""" + + joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request( + trajectory=arm_joint_trajectory + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_joint_move_command=joint_move_command + ) + sync_arm = synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + arm_sync_robot_cmd = robot_command_pb2.RobotCommand( + synchronized_command=sync_arm + ) + return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd) + + @try_claim + def arm_joint_move(self, joint_targets): + # All perspectives are given when looking at the robot from behind after the unstow service is called + # Joint1: 0.0 arm points to the front. positive: turn left, negative: turn right) + # RANGE: -3.14 -> 3.14 + # Joint2: 0.0 arm points to the front. positive: move down, negative move up + # RANGE: 0.4 -> -3.13 ( + # Joint3: 0.0 arm straight. moves the arm down + # RANGE: 0.0 -> 3.1415 + # Joint4: 0.0 middle position. negative: moves ccw, positive moves cw + # RANGE: -2.79253 -> 2.79253 + # # Joint5: 0.0 gripper points to the front. positive moves the gripper down + # RANGE: -1.8326 -> 1.8326 + # Joint6: 0.0 Gripper is not rolled, positive is ccw + # RANGE: -2.87 -> 2.87 + # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0] + if abs(joint_targets[0]) > 3.14: + msg = "Joint 1 has to be between -3.14 and 3.14" + self._logger.warning(msg) + return False, msg + elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13: + msg = "Joint 2 has to be between -3.13 and 0.4" + self._logger.warning(msg) + return False, msg + elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0: + msg = "Joint 3 has to be between 0.0 and 3.14" + self._logger.warning(msg) + return False, msg + elif abs(joint_targets[3]) > 2.79253: + msg = "Joint 4 has to be between -2.79253 and 2.79253" + self._logger.warning(msg) + return False, msg + elif abs(joint_targets[4]) > 1.8326: + msg = "Joint 5 has to be between -1.8326 and 1.8326" + self._logger.warning(msg) + return False, msg + elif abs(joint_targets[5]) > 2.87: + msg = "Joint 6 has to be between -2.87 and 2.87" + self._logger.warning(msg) + return False, msg + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + trajectory_point = ( + RobotCommandBuilder.create_arm_joint_trajectory_point( + joint_targets[0], + joint_targets[1], + joint_targets[2], + joint_targets[3], + joint_targets[4], + joint_targets[5], + ) + ) + arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory( + points=[trajectory_point] + ) + arm_command = self.make_arm_trajectory_command(arm_joint_trajectory) + + # Send the request + cmd_id = self._robot_command_client.robot_command(arm_command) + + # Query for feedback to determine how long it will take + feedback_resp = self._robot_command_client.robot_command_feedback( + cmd_id + ) + joint_move_feedback = ( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback + ) + time_to_goal: Duration = joint_move_feedback.time_to_goal + time_to_goal_in_seconds: float = time_to_goal.seconds + ( + float(time_to_goal.nanos) / float(10**9) + ) + time.sleep(time_to_goal_in_seconds) + return True, "Spot Arm moved successfully" + + except Exception as e: + return False, f"Exception occured during arm movement: {e}" + + @try_claim + def force_trajectory(self, data): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + + def create_wrench_from_msg(forces, torques): + force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) + torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) + return geometry_pb2.Wrench(force=force, torque=torque) + + # Duration in seconds. + traj_duration = data.duration + + # first point on trajectory + wrench0 = create_wrench_from_msg(data.forces_pt0, data.torques_pt0) + t0 = seconds_to_duration(0) + traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( + wrench=wrench0, time_since_reference=t0 + ) + + # Second point on the trajectory + wrench1 = create_wrench_from_msg(data.forces_pt1, data.torques_pt1) + t1 = seconds_to_duration(traj_duration) + traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( + wrench=wrench1, time_since_reference=t1 + ) + + # Build the trajectory + trajectory = trajectory_pb2.WrenchTrajectory( + points=[traj_point0, traj_point1] + ) + + # Build the trajectory request, putting all axes into force mode + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + root_frame_name=data.frame, + wrench_trajectory_in_task=trajectory, + x_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + y_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + z_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + rx_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + ry_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + rz_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command + ) + synchronized_command = ( + synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + ) + robot_command = robot_command_pb2.RobotCommand( + synchronized_command=synchronized_command + ) + + # Send the request + self._robot_command_client.robot_command(robot_command) + self._logger.info("Force trajectory command sent") + + time.sleep(float(traj_duration) + 1.0) + + except Exception as e: + return False, f"Exception occured during arm movement: {e}" + + return True, "Moved arm successfully" + + @try_claim + def gripper_open(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Open gripper + command = RobotCommandBuilder.claw_gripper_open_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper open sent") + time.sleep(2.0) + + except Exception as e: + return False, f"Exception occured while gripper was moving: {e}" + + return True, "Open gripper success" + + @try_claim + def gripper_close(self): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Close gripper + command = RobotCommandBuilder.claw_gripper_close_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper close sent") + time.sleep(2.0) + + except Exception as e: + return False, f"Exception occured while gripper was moving: {e}" + + return True, "Closed gripper successfully" + + @try_claim + def gripper_angle_open(self, gripper_ang): + # takes an angle between 0 (closed) and 90 (fully opened) and opens the + # gripper at this angle + if gripper_ang > 90 or gripper_ang < 0: + return False, "Gripper angle must be between 0 and 90" + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # The open angle command does not take degrees but the limits + # defined in the urdf, that is why we have to interpolate + closed = 0.349066 + opened = -1.396263 + angle = gripper_ang / 90.0 * (opened - closed) + closed + command = RobotCommandBuilder.claw_gripper_open_angle_command(angle) + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper open angle sent") + time.sleep(2.0) + + except Exception as e: + return False, f"Exception occured while gripper was moving: {e}" + + return True, "Opened gripper successfully" + + @try_claim + def hand_pose(self, data): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + pose_point = data.pose_point + # Move the arm to a spot in front of the robot given a pose for the gripper. + # Build a position to move the arm to (in meters, relative to the body frame origin.) + position = geometry_pb2.Vec3( + x=pose_point.position.x, + y=pose_point.position.y, + z=pose_point.position.z, + ) + + # # Rotation as a quaternion. + rotation = geometry_pb2.Quaternion( + w=pose_point.orientation.w, + x=pose_point.orientation.x, + y=pose_point.orientation.y, + z=pose_point.orientation.z, + ) + + seconds = data.duration + duration = seconds_to_duration(seconds) + + # Build the SE(3) pose of the desired hand position in the moving body frame. + hand_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) + hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose, time_since_reference=duration + ) + hand_trajectory = trajectory_pb2.SE3Trajectory( + points=[hand_pose_traj_point] + ) + + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + root_frame_name=data.frame, + pose_trajectory_in_task=hand_trajectory, + force_remain_near_current_joint_configuration=True, + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command + ) + synchronized_command = ( + synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + ) + + robot_command = robot_command_pb2.RobotCommand( + synchronized_command=synchronized_command + ) + + command = RobotCommandBuilder.build_synchro_command(robot_command) + + # Send the request + self._robot_command_client.robot_command(robot_command) + self._logger.info("Moving arm to position.") + + time.sleep(2.0) + + except Exception as e: + return ( + False, + f"An error occured while trying to move arm \n Exception: {e}", + ) + + return True, "Moved arm successfully" + + @try_claim + def grasp_3d(self, frame, object_rt_frame): + try: + frm = str(frame) + pos = geometry_pb2.Vec3( + x=object_rt_frame[0], y=object_rt_frame[1], z=object_rt_frame[2] + ) + + grasp = manipulation_api_pb2.PickObject(frame_name=frm, object_rt_frame=pos) + + # Ask the robot to pick up the object + grasp_request = manipulation_api_pb2.ManipulationApiRequest( + pick_object=grasp + ) + # Send the request + cmd_response = self._manipulation_api_client.manipulation_api_command( + manipulation_api_request=grasp_request + ) + + # Get feedback from the robot + while True: + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_response.manipulation_cmd_id + ) + + # Send the request + response = ( + self._manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) + ) + + print( + "Current state: ", + manipulation_api_pb2.ManipulationFeedbackState.Name( + response.current_state + ), + ) + + if ( + response.current_state + == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED + or response.current_state + == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED + ): + break + + time.sleep(0.25) + + self._robot.logger.info("Finished grasp.") + + except Exception as e: + return False, f"An error occured while trying to grasp from pose: {e}" + + return True, "Grasped successfully" + + ################################################################### + + def _init_current_graph_nav_state(self): + # Store the most recent knowledge of the state of the robot based on rpc calls. + self._current_graph = None + self._current_edges = dict() # maps to_waypoint to list(from_waypoint) + self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot + self._current_edge_snapshots = dict() # maps id to edge snapshot + self._current_annotation_name_to_wp_id = dict() + self._current_anchored_world_objects = ( + dict() + ) # maps object id to a (wo, waypoint, fiducial) + self._current_anchors = dict() # maps anchor id to anchor + + ## copy from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py + def _get_localization_state(self, *args): + """Get the current localization and state of the robot.""" + state = self._graph_nav_client.get_localization_state() + self._logger.info("Got localization: \n%s" % str(state.localization)) + odom_tform_body = get_odom_tform_body( + state.robot_kinematics.transforms_snapshot + ) + self._logger.info( + "Got robot state in kinematic odometry frame: \n%s" % str(odom_tform_body) + ) + + def _set_initial_localization_fiducial(self, *args): + """Trigger localization when near a fiducial.""" + robot_state = self._robot_state_client.get_robot_state() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() + # Create an empty instance for initial localization since we are asking it to localize + # based on the nearest fiducial. + localization = nav_pb2.Localization() + self._graph_nav_client.set_localization( + initial_guess_localization=localization, + ko_tform_body=current_odom_tform_body, + ) + + def _set_initial_localization_waypoint(self, *args): + """Trigger localization to a waypoint.""" + # Take the first argument as the localization waypoint. + if len(args) < 1: + # If no waypoint id is given as input, then return without initializing. + self._logger.error("No waypoint specified to initialize to.") + return + destination_waypoint = graph_nav_util.find_unique_waypoint_id( + args[0][0], + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not destination_waypoint: + # Failed to find the unique waypoint id. + return + + robot_state = self._robot_state_client.get_robot_state() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() + # Create an initial localization to the specified waypoint as the identity. + localization = nav_pb2.Localization() + localization.waypoint_id = destination_waypoint + localization.waypoint_tform_body.rotation.w = 1.0 + self._graph_nav_client.set_localization( + initial_guess_localization=localization, + # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). + max_distance=0.2, + max_yaw=20.0 * math.pi / 180.0, + fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NO_FIDUCIAL, + ko_tform_body=current_odom_tform_body, + ) + + def _list_graph_waypoint_and_edge_ids(self, *args): + """List the waypoint ids and edge ids of the graph currently on the robot.""" + + # Download current graph + graph = self._graph_nav_client.download_graph() + if graph is None: + self._logger.error("Empty graph.") + return + self._current_graph = graph + + localization_id = ( + self._graph_nav_client.get_localization_state().localization.waypoint_id + ) + + # Update and print waypoints and edges + ( + self._current_annotation_name_to_wp_id, + self._current_edges, + ) = graph_nav_util.update_waypoints_and_edges( + graph, localization_id, self._logger + ) + return self._current_annotation_name_to_wp_id, self._current_edges + + def _upload_graph_and_snapshots(self, upload_filepath): + """Upload the graph and snapshots to the robot.""" + self._logger.info("Loading the graph from disk into local storage...") + with open(os.path.join(upload_filepath, "graph"), "rb") as graph_file: + # Load the graph from disk. + data = graph_file.read() + self._current_graph = map_pb2.Graph() + self._current_graph.ParseFromString(data) + self._logger.info( + "Loaded graph has {} waypoints and {} edges".format( + len(self._current_graph.waypoints), len(self._current_graph.edges) + ) + ) + for waypoint in self._current_graph.waypoints: + # Load the waypoint snapshots from disk. + if len(waypoint.snapshot_id) == 0: + continue + waypoint_filepath = os.path.join( + upload_filepath, "waypoint_snapshots", waypoint.snapshot_id + ) + if not os.path.exists(waypoint_filepath): + continue + with open(waypoint_filepath, "rb") as snapshot_file: + waypoint_snapshot = map_pb2.WaypointSnapshot() + waypoint_snapshot.ParseFromString(snapshot_file.read()) + self._current_waypoint_snapshots[ + waypoint_snapshot.id + ] = waypoint_snapshot + + for fiducial in waypoint_snapshot.objects: + if not fiducial.HasField("apriltag_properties"): + continue + + str_id = str(fiducial.apriltag_properties.tag_id) + if ( + str_id in self._current_anchored_world_objects + and len(self._current_anchored_world_objects[str_id]) == 1 + ): + # Replace the placeholder tuple with a tuple of (wo, waypoint, fiducial). + anchored_wo = self._current_anchored_world_objects[str_id][0] + self._current_anchored_world_objects[str_id] = ( + anchored_wo, + waypoint, + fiducial, + ) + + for edge in self._current_graph.edges: + # Load the edge snapshots from disk. + if len(edge.snapshot_id) == 0: + continue + edge_filepath = os.path.join( + upload_filepath, "edge_snapshots", edge.snapshot_id + ) + if not os.path.exists(edge_filepath): + continue + with open(edge_filepath, "rb") as snapshot_file: + edge_snapshot = map_pb2.EdgeSnapshot() + edge_snapshot.ParseFromString(snapshot_file.read()) + self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot + for anchor in self._current_graph.anchoring.anchors: + self._current_anchors[anchor.id] = anchor + # Upload the graph to the robot. + self._logger.info("Uploading the graph and snapshots to the robot...") + if self._lease is None: + self.getLease() + self._graph_nav_client.upload_graph( + lease=self._lease.lease_proto, graph=self._current_graph + ) + # Upload the snapshots to the robot. + for waypoint_snapshot in self._current_waypoint_snapshots.values(): + self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) + self._logger.info("Uploaded {}".format(waypoint_snapshot.id)) + for edge_snapshot in self._current_edge_snapshots.values(): + self._graph_nav_client.upload_edge_snapshot(edge_snapshot) + self._logger.info("Uploaded {}".format(edge_snapshot.id)) + + # The upload is complete! Check that the robot is localized to the graph, + # and it if is not, prompt the user to localize the robot before attempting + # any navigation commands. + localization_state = self._graph_nav_client.get_localization_state() + if not localization_state.localization.waypoint_id: + # The robot is not localized to the newly uploaded graph. + self._logger.info( + "Upload complete! The robot is currently not localized to the map; " + "please localize the robot" + ) + + @try_claim + def _navigate_to(self, *args): + """Navigate to a specific waypoint.""" + # Take the first argument as the destination waypoint. + if len(args) < 1: + # If no waypoint id is given as input, then return without requesting navigation. + self._logger.info("No waypoint provided as a destination for navigate to.") + return + + self._lease = self._lease_wallet.get_lease() + destination_waypoint = graph_nav_util.find_unique_waypoint_id( + args[0][0], + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not destination_waypoint: + # Failed to find the appropriate unique waypoint id for the navigation command. + return + if not self.toggle_power(should_power_on=True): + self._logger.info( + "Failed to power on the robot, and cannot complete navigate to request." + ) + return + + # Stop the lease keepalive and create a new sublease for graph nav. + self._lease = self._lease_wallet.advance() + sublease = self._lease.create_sublease() + self._lease_keepalive.shutdown() + + # Navigate to the destination waypoint. + is_finished = False + nav_to_cmd_id = -1 + while not is_finished: + # Issue the navigation command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + nav_to_cmd_id = self._graph_nav_client.navigate_to( + destination_waypoint, 1.0, leases=[sublease.lease_proto] + ) + time.sleep(0.5) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the navigation command is complete. Then sit + # the robot down once it is finished. + is_finished = self._check_success(nav_to_cmd_id) + + self._lease = self._lease_wallet.advance() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + + # Update the lease and power off the robot if appropriate. + if self._powered_on and not self._started_powered_on: + # Sit the robot down + power off after the navigation command is complete. + self.toggle_power(should_power_on=False) + + status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): + return True, "Successfully completed the navigation commands!" + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: + return ( + False, + "Robot got lost when navigating the route, the robot will now sit down.", + ) + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: + return ( + False, + "Robot got stuck when navigating the route, the robot will now sit down.", + ) + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): + return False, "Robot is impaired." + else: + return False, "Navigation command is not complete yet." + + @try_claim + def _navigate_route(self, *args): + """Navigate through a specific route of waypoints.""" + if len(args) < 1: + # If no waypoint ids are given as input, then return without requesting navigation. + self._logger.error("No waypoints provided for navigate route.") + return + waypoint_ids = args[0] + for i in range(len(waypoint_ids)): + waypoint_ids[i] = graph_nav_util.find_unique_waypoint_id( + waypoint_ids[i], + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not waypoint_ids[i]: + # Failed to find the unique waypoint id. + return + + edge_ids_list = [] + all_edges_found = True + # Attempt to find edges in the current graph that match the ordered waypoint pairs. + # These are necessary to create a valid route. + for i in range(len(waypoint_ids) - 1): + start_wp = waypoint_ids[i] + end_wp = waypoint_ids[i + 1] + edge_id = self._match_edge(self._current_edges, start_wp, end_wp) + if edge_id is not None: + edge_ids_list.append(edge_id) + else: + all_edges_found = False + self._logger.error( + "Failed to find an edge between waypoints: ", + start_wp, + " and ", + end_wp, + ) + self._logger.error( + "List the graph's waypoints and edges to ensure pairs of waypoints has an edge." + ) + break + + self._lease = self._lease_wallet.get_lease() + if all_edges_found: + if not self.toggle_power(should_power_on=True): + self._logger.error( + "Failed to power on the robot, and cannot complete navigate route request." + ) + return + + # Stop the lease keepalive and create a new sublease for graph nav. + self._lease = self._lease_wallet.advance() + sublease = self._lease.create_sublease() + self._lease_keepalive.shutdown() + + # Navigate a specific route. + route = self._graph_nav_client.build_route(waypoint_ids, edge_ids_list) + is_finished = False + while not is_finished: + # Issue the route command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + nav_route_command_id = self._graph_nav_client.navigate_route( + route, cmd_duration=1.0, leases=[sublease.lease_proto] + ) + time.sleep( + 0.5 + ) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the route is complete. Then sit + # the robot down once it is finished. + is_finished = self._check_success(nav_route_command_id) + + self._lease = self._lease_wallet.advance() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + + # Update the lease and power off the robot if appropriate. + if self._powered_on and not self._started_powered_on: + # Sit the robot down + power off after the navigation command is complete. + self.toggle_power(should_power_on=False) + + def _clear_graph(self, *args): + """Clear the state of the map on the robot, removing all waypoints and edges.""" + result = self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) + self._init_current_graph_nav_state() + return result + + @try_claim + def toggle_power(self, should_power_on): + """Power the robot on/off dependent on the current power state.""" + is_powered_on = self.check_is_powered_on() + if not is_powered_on and should_power_on: + # Power on the robot up before navigating when it is in a powered-off state. + power_on(self._power_client) + motors_on = False + while not motors_on: + future = self._robot_state_client.get_robot_state_async() + state_response = future.result( + timeout=10 + ) # 10 second timeout for waiting for the state response. + if ( + state_response.power_state.motor_power_state + == robot_state_pb2.PowerState.STATE_ON + ): + motors_on = True + else: + # Motors are not yet fully powered on. + time.sleep(0.25) + elif is_powered_on and not should_power_on: + # Safe power off (robot will sit then power down) when it is in a + # powered-on state. + safe_power_off(self._robot_command_client, self._robot_state_client) + else: + # Return the current power state without change. + return is_powered_on + # Update the locally stored power state. + self.check_is_powered_on() + return self._powered_on + + def check_is_powered_on(self): + """Determine if the robot is powered on or off.""" + power_state = self._robot_state_client.get_robot_state().power_state + self._powered_on = power_state.motor_power_state == power_state.STATE_ON + return self._powered_on + + def _check_success(self, command_id=-1): + """Use a navigation command id to get feedback from the robot and sit when command succeeds.""" + if command_id == -1: + # No command, so we have not status to check. + return False + status = self._graph_nav_client.navigation_feedback(command_id) + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): + # Successfully completed the navigation commands! + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: + self._logger.error( + "Robot got lost when navigating the route, the robot will now sit down." + ) + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: + self._logger.error( + "Robot got stuck when navigating the route, the robot will now sit down." + ) + return True + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): + self._logger.error("Robot is impaired.") + return True + else: + # Navigation command is not complete yet. + return False + + def _match_edge(self, current_edges, waypoint1, waypoint2): + """Find an edge in the graph that is between two waypoint ids.""" + # Return the correct edge id as soon as it's found. + for edge_to_id in current_edges: + for edge_from_id in current_edges[edge_to_id]: + if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): + # This edge matches the pair of waypoints! Add it the edge list and continue. + return map_pb2.Edge.Id( + from_waypoint=waypoint2, to_waypoint=waypoint1 + ) + elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): + # This edge matches the pair of waypoints! Add it the edge list and continue. + return map_pb2.Edge.Id( + from_waypoint=waypoint1, to_waypoint=waypoint2 + ) + return None + + @try_claim + def dock(self, dock_id): + """Dock the robot to the docking station with fiducial ID [dock_id].""" + try: + # Make sure we're powered on and standing + self._robot.power_on() + self.stand() + # Dock the robot + self._last_docking_command = dock_id + blocking_dock_robot(self._robot, dock_id) + self._last_docking_command = None + # Necessary to reset this as docking often causes the last stand command to go into an unknown state + self._last_stand_command = None + return True, "Success" + except Exception as e: + return False, f"Exception while trying to dock: {e}" + + @try_claim + def undock(self, timeout=20): + """Power motors on and undock the robot from the station.""" + try: + # Maker sure we're powered on + self._robot.power_on() + # Undock the robot + blocking_undock(self._robot, timeout) + return True, "Success" + except Exception as e: + return False, f"Exception while trying to undock: {e}" + + @try_claim + def execute_dance(self, data): + if self._is_licensed_for_choreography: + return self._spot_dance.execute_dance(data) + else: + return False, "Spot is not licensed for choreography" + + def upload_animation( + self, animation_name: str, animation_file_content: str + ) -> typing.Tuple[bool, str]: + if self._is_licensed_for_choreography: + return self._spot_dance.upload_animation( + animation_name, animation_file_content + ) + else: + return False, "Spot is not licensed for choreography" def list_all_moves(self) -> typing.Tuple[bool, str, typing.List[str]]: if self._is_licensed_for_choreography: @@ -1280,3 +2532,225 @@ def list_all_dances(self) -> typing.Tuple[bool, str, typing.List[str]]: return self._spot_dance.list_all_dances() else: return False, "Spot is not licensed for choreography", [] + + def get_docking_state(self, **kwargs): + """Get docking state of robot.""" + state = self._docking_client.get_docking_state(**kwargs) + return state + + def update_image_tasks(self, image_name): + """Adds an async tasks to retrieve images from the specified image source""" + + task_to_add = self.camera_task_name_to_task_mapping[image_name] + + if task_to_add == self._hand_image_task and not self._robot.has_arm(): + self._logger.warning( + "Robot has no arm, therefore the arm image task can not be added" + ) + return + + if task_to_add in self._async_tasks._tasks: + self._logger.warning( + f"Task {image_name} already in async task list, will not be added again" + ) + return + + self._async_tasks.add_task(self.camera_task_name_to_task_mapping[image_name]) + + def get_frontleft_rgb_image(self): + try: + return self._image_client.get_image( + [ + build_image_request( + "frontleft_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_frontright_rgb_image(self): + try: + return self._image_client.get_image( + [ + build_image_request( + "frontright_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_left_rgb_image(self): + try: + return self._image_client.get_image( + [ + build_image_request( + "left_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_right_rgb_image(self): + try: + return self._image_client.get_image( + [ + build_image_request( + "right_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_back_rgb_image(self): + try: + return self._image_client.get_image( + [ + build_image_request( + "back_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_hand_rgb_image(self): + if not self.has_arm(): + return None + try: + return self._image_client.get_image( + [ + build_image_request( + "hand_color_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_images( + self, image_requests: typing.List[image_pb2.ImageRequest] + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: + try: + image_responses = self._image_client.get_image(image_requests) + except UnsupportedPixelFormatRequestedError as e: + return None + if self.has_arm(): + return ImageWithHandBundle( + frontleft=image_responses[0], + frontright=image_responses[1], + left=image_responses[2], + right=image_responses[3], + back=image_responses[4], + hand=image_responses[5], + ) + else: + return ImageBundle( + frontleft=image_responses[0], + frontright=image_responses[1], + left=image_responses[2], + right=image_responses[3], + back=image_responses[4], + ) + + def get_camera_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: + return self.get_images(self._camera_image_requests) + + def get_depth_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: + return self.get_images(self._depth_image_requests) + + def get_depth_registered_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: + return self.get_images(self._depth_registered_image_requests) + + def get_images_by_cameras( + self, camera_sources: typing.List[CameraSource] + ) -> typing.List[ImageEntry]: + """Calls the GetImage RPC using the image client with requests + corresponding to the given cameras. + + Args: + camera_sources: a list of CameraSource objects. There are two + possibilities for each item in this list. Either it is + CameraSource(camera='front') or + CameraSource(camera='front', image_types=['visual', 'depth_registered') + + - If the former is provided, the image requests will include all + image types for each specified camera. + - If the latter is provided, the image requests will be + limited to the specified image types for each corresponding + camera. + + Note that duplicates of camera names are not allowed. + + Returns: + a list, where each entry is (camera_name, image_type, image_response) + e.g. ('frontleft', 'visual', image_response) + """ + # Build image requests + image_requests = [] + source_types = [] + cameras_specified = set() + for item in camera_sources: + if item.camera_name in cameras_specified: + self._logger.error( + f"Duplicated camera source for camera {item.camera_name}" + ) + return None + image_types = item.image_types + if image_types is None: + image_types = IMAGE_TYPES + for image_type in image_types: + try: + image_requests.append( + self._image_requests_by_camera[item.camera_name][image_type] + ) + except KeyError: + self._logger.error( + f"Unexpected camera name '{item.camera_name}' or image type '{image_type}'" + ) + return None + source_types.append((item.camera_name, image_type)) + cameras_specified.add(item.camera_name) + + # Send image requests + try: + image_responses = self._image_client.get_image(image_requests) + except UnsupportedPixelFormatRequestedError: + self._logger.error( + "UnsupportedPixelFormatRequestedError. " + "Likely pixel_format is set wrong for some image request" + ) + return None + + # Return + result = [] + for i, (camera_name, image_type) in enumerate(source_types): + result.append( + ImageEntry( + camera_name=camera_name, + image_type=image_type, + image_response=image_responses[i], + ) + ) + return result From 5e33275af4c550876d68e29be985422e317f6e68 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 27 Jun 2023 19:27:16 +0100 Subject: [PATCH 59/66] restore cam wrapper to main --- spot_wrapper/cam_wrapper.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/cam_wrapper.py b/spot_wrapper/cam_wrapper.py index ea348f44..00460422 100644 --- a/spot_wrapper/cam_wrapper.py +++ b/spot_wrapper/cam_wrapper.py @@ -10,19 +10,19 @@ import cv2 import numpy as np from aiortc import RTCConfiguration -from bosdyn.api.spot_cam import audio_pb2 -from bosdyn.api.spot_cam.ptz_pb2 import PtzDescription, PtzVelocity, PtzPosition from bosdyn.client import Robot from bosdyn.client import spot_cam -from bosdyn.client.payload import PayloadClient -from bosdyn.client.spot_cam.audio import AudioClient from bosdyn.client.spot_cam.compositor import CompositorClient -from bosdyn.client.spot_cam.health import HealthClient from bosdyn.client.spot_cam.lighting import LightingClient -from bosdyn.client.spot_cam.media_log import MediaLogClient from bosdyn.client.spot_cam.power import PowerClient -from bosdyn.client.spot_cam.ptz import PtzClient +from bosdyn.client.spot_cam.health import HealthClient +from bosdyn.client.spot_cam.audio import AudioClient from bosdyn.client.spot_cam.streamquality import StreamQualityClient +from bosdyn.client.spot_cam.ptz import PtzClient +from bosdyn.client.spot_cam.media_log import MediaLogClient +from bosdyn.client.payload import PayloadClient +from bosdyn.api.spot_cam.ptz_pb2 import PtzDescription, PtzVelocity, PtzPosition +from bosdyn.api.spot_cam import audio_pb2 from spot_wrapper.cam_webrtc_client import WebRTCClient from spot_wrapper.wrapper import SpotWrapper From 664bfccaafb09a94224ced24d49fdb9aa62c2593 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 27 Jun 2023 19:29:33 +0100 Subject: [PATCH 60/66] restore graph nav util to main --- spot_wrapper/graph_nav_util.py | 130 +++++++++++++++++++++++++++++++++ 1 file changed, 130 insertions(+) create mode 100644 spot_wrapper/graph_nav_util.py diff --git a/spot_wrapper/graph_nav_util.py b/spot_wrapper/graph_nav_util.py new file mode 100644 index 00000000..e35357e4 --- /dev/null +++ b/spot_wrapper/graph_nav_util.py @@ -0,0 +1,130 @@ +# Copyright (c) 2020 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +"""Graph nav utility functions""" + + +def id_to_short_code(id): + """Convert a unique id to a 2 letter short code.""" + tokens = id.split("-") + if len(tokens) > 2: + return "%c%c" % (tokens[0][0], tokens[1][0]) + return None + + +def pretty_print_waypoints( + waypoint_id, waypoint_name, short_code_to_count, localization_id, logger +): + short_code = id_to_short_code(waypoint_id) + if short_code is None or short_code_to_count[short_code] != 1: + short_code = " " # If the short code is not valid/unique, don't show it. + + logger.info( + "%s Waypoint name: %s id: %s short code: %s" + % ( + "->" if localization_id == waypoint_id else " ", + waypoint_name, + waypoint_id, + short_code, + ) + ) + + +def find_unique_waypoint_id(short_code, graph, name_to_id, logger): + """Convert either a 2 letter short code or an annotation name into the associated unique id.""" + if len(short_code) != 2: + # Not a short code, check if it is an annotation name (instead of the waypoint id). + if short_code in name_to_id: + # Short code is an waypoint's annotation name. Check if it is paired with a unique waypoint id. + if name_to_id[short_code] is not None: + # Has an associated waypoint id! + return name_to_id[short_code] + else: + logger.error( + "The waypoint name %s is used for multiple different unique waypoints. Please use" + + "the waypoint id." % (short_code) + ) + return None + # Also not an waypoint annotation name, so we will operate under the assumption that it is a + # unique waypoint id. + return short_code + + ret = short_code + for waypoint in graph.waypoints: + if short_code == id_to_short_code(waypoint.id): + if ret != short_code: + return short_code # Multiple waypoints with same short code. + ret = waypoint.id + return ret + + +def update_waypoints_and_edges(graph, localization_id, logger): + """Update and print waypoint ids and edge ids.""" + name_to_id = dict() + edges = dict() + + short_code_to_count = {} + waypoint_to_timestamp = [] + for waypoint in graph.waypoints: + # Determine the timestamp that this waypoint was created at. + timestamp = -1.0 + try: + timestamp = ( + waypoint.annotations.creation_time.seconds + + waypoint.annotations.creation_time.nanos / 1e9 + ) + except: + # Must be operating on an older graph nav map, since the creation_time is not + # available within the waypoint annotations message. + pass + waypoint_to_timestamp.append( + (waypoint.id, timestamp, waypoint.annotations.name) + ) + + # Determine how many waypoints have the same short code. + short_code = id_to_short_code(waypoint.id) + if short_code not in short_code_to_count: + short_code_to_count[short_code] = 0 + short_code_to_count[short_code] += 1 + + # Add the annotation name/id into the current dictionary. + waypoint_name = waypoint.annotations.name + if waypoint_name: + if waypoint_name in name_to_id: + # Waypoint name is used for multiple different waypoints, so set the waypoint id + # in this dictionary to None to avoid confusion between two different waypoints. + name_to_id[waypoint_name] = None + else: + # First time we have seen this waypoint annotation name. Add it into the dictionary + # with the respective waypoint unique id. + name_to_id[waypoint_name] = waypoint.id + + # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, + # fallback to sorting by annotation name. + waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) + + # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from + # when the waypoint was created. + logger.info("%d waypoints:" % len(graph.waypoints)) + for waypoint in waypoint_to_timestamp: + pretty_print_waypoints( + waypoint[0], waypoint[2], short_code_to_count, localization_id, logger + ) + + for edge in graph.edges: + if edge.id.to_waypoint in edges: + if edge.id.from_waypoint not in edges[edge.id.to_waypoint]: + edges[edge.id.to_waypoint].append(edge.id.from_waypoint) + else: + edges[edge.id.to_waypoint] = [edge.id.from_waypoint] + logger.info( + "(Edge) from waypoint id: ", + edge.id.from_waypoint, + " and to waypoint id: ", + edge.id.to_waypoint, + ) + + return name_to_id, edges From 1ffe26a64035d2b515d57c7cebe8ac3a3574cb00 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Wed, 28 Jun 2023 15:55:01 +0100 Subject: [PATCH 61/66] unnecessary change --- spot_wrapper/spot_dance.py | 1 - 1 file changed, 1 deletion(-) diff --git a/spot_wrapper/spot_dance.py b/spot_wrapper/spot_dance.py index add4c8b9..c52c0800 100644 --- a/spot_wrapper/spot_dance.py +++ b/spot_wrapper/spot_dance.py @@ -2,7 +2,6 @@ import tempfile import os -from bosdyn.choreography.client.choreography import ChoreographyClient from bosdyn.choreography.client.choreography import ( load_choreography_sequence_from_txt_file, ChoreographyClient, From 37324066d29618cfbaf2087e3b02ede842fd4725 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Thu, 29 Jun 2023 15:35:29 +0100 Subject: [PATCH 62/66] remove other modules and old graph nav util --- spot_wrapper/graph_nav_util.py | 130 ------- spot_wrapper/spot_arm.py | 687 --------------------------------- spot_wrapper/spot_check.py | 225 ----------- spot_wrapper/spot_docking.py | 60 --- spot_wrapper/spot_eap.py | 72 ---- spot_wrapper/spot_images.py | 376 ------------------ 6 files changed, 1550 deletions(-) delete mode 100644 spot_wrapper/graph_nav_util.py delete mode 100644 spot_wrapper/spot_arm.py delete mode 100644 spot_wrapper/spot_check.py delete mode 100644 spot_wrapper/spot_docking.py delete mode 100644 spot_wrapper/spot_eap.py delete mode 100644 spot_wrapper/spot_images.py diff --git a/spot_wrapper/graph_nav_util.py b/spot_wrapper/graph_nav_util.py deleted file mode 100644 index e35357e4..00000000 --- a/spot_wrapper/graph_nav_util.py +++ /dev/null @@ -1,130 +0,0 @@ -# Copyright (c) 2020 Boston Dynamics, Inc. All rights reserved. -# -# Downloading, reproducing, distributing or otherwise using the SDK Software -# is subject to the terms and conditions of the Boston Dynamics Software -# Development Kit License (20191101-BDSDK-SL). - -"""Graph nav utility functions""" - - -def id_to_short_code(id): - """Convert a unique id to a 2 letter short code.""" - tokens = id.split("-") - if len(tokens) > 2: - return "%c%c" % (tokens[0][0], tokens[1][0]) - return None - - -def pretty_print_waypoints( - waypoint_id, waypoint_name, short_code_to_count, localization_id, logger -): - short_code = id_to_short_code(waypoint_id) - if short_code is None or short_code_to_count[short_code] != 1: - short_code = " " # If the short code is not valid/unique, don't show it. - - logger.info( - "%s Waypoint name: %s id: %s short code: %s" - % ( - "->" if localization_id == waypoint_id else " ", - waypoint_name, - waypoint_id, - short_code, - ) - ) - - -def find_unique_waypoint_id(short_code, graph, name_to_id, logger): - """Convert either a 2 letter short code or an annotation name into the associated unique id.""" - if len(short_code) != 2: - # Not a short code, check if it is an annotation name (instead of the waypoint id). - if short_code in name_to_id: - # Short code is an waypoint's annotation name. Check if it is paired with a unique waypoint id. - if name_to_id[short_code] is not None: - # Has an associated waypoint id! - return name_to_id[short_code] - else: - logger.error( - "The waypoint name %s is used for multiple different unique waypoints. Please use" - + "the waypoint id." % (short_code) - ) - return None - # Also not an waypoint annotation name, so we will operate under the assumption that it is a - # unique waypoint id. - return short_code - - ret = short_code - for waypoint in graph.waypoints: - if short_code == id_to_short_code(waypoint.id): - if ret != short_code: - return short_code # Multiple waypoints with same short code. - ret = waypoint.id - return ret - - -def update_waypoints_and_edges(graph, localization_id, logger): - """Update and print waypoint ids and edge ids.""" - name_to_id = dict() - edges = dict() - - short_code_to_count = {} - waypoint_to_timestamp = [] - for waypoint in graph.waypoints: - # Determine the timestamp that this waypoint was created at. - timestamp = -1.0 - try: - timestamp = ( - waypoint.annotations.creation_time.seconds - + waypoint.annotations.creation_time.nanos / 1e9 - ) - except: - # Must be operating on an older graph nav map, since the creation_time is not - # available within the waypoint annotations message. - pass - waypoint_to_timestamp.append( - (waypoint.id, timestamp, waypoint.annotations.name) - ) - - # Determine how many waypoints have the same short code. - short_code = id_to_short_code(waypoint.id) - if short_code not in short_code_to_count: - short_code_to_count[short_code] = 0 - short_code_to_count[short_code] += 1 - - # Add the annotation name/id into the current dictionary. - waypoint_name = waypoint.annotations.name - if waypoint_name: - if waypoint_name in name_to_id: - # Waypoint name is used for multiple different waypoints, so set the waypoint id - # in this dictionary to None to avoid confusion between two different waypoints. - name_to_id[waypoint_name] = None - else: - # First time we have seen this waypoint annotation name. Add it into the dictionary - # with the respective waypoint unique id. - name_to_id[waypoint_name] = waypoint.id - - # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, - # fallback to sorting by annotation name. - waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) - - # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from - # when the waypoint was created. - logger.info("%d waypoints:" % len(graph.waypoints)) - for waypoint in waypoint_to_timestamp: - pretty_print_waypoints( - waypoint[0], waypoint[2], short_code_to_count, localization_id, logger - ) - - for edge in graph.edges: - if edge.id.to_waypoint in edges: - if edge.id.from_waypoint not in edges[edge.id.to_waypoint]: - edges[edge.id.to_waypoint].append(edge.id.from_waypoint) - else: - edges[edge.id.to_waypoint] = [edge.id.from_waypoint] - logger.info( - "(Edge) from waypoint id: ", - edge.id.from_waypoint, - " and to waypoint id: ", - edge.id.to_waypoint, - ) - - return name_to_id, edges diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py deleted file mode 100644 index 4a599021..00000000 --- a/spot_wrapper/spot_arm.py +++ /dev/null @@ -1,687 +0,0 @@ -import logging -import time -import typing - -from bosdyn.api import arm_command_pb2 -from bosdyn.api import geometry_pb2 -from bosdyn.api import image_pb2 -from bosdyn.api import manipulation_api_pb2 -from bosdyn.api import robot_command_pb2 -from bosdyn.api import synchronized_command_pb2 -from bosdyn.api import trajectory_pb2 -from bosdyn.client import robot_command -from bosdyn.client.async_tasks import AsyncPeriodicQuery -from bosdyn.client.image import ImageClient, build_image_request -from bosdyn.client.manipulation_api_client import ManipulationApiClient -from bosdyn.client.robot import Robot -from bosdyn.client.robot_command import ( - RobotCommandBuilder, - RobotCommandClient, - block_until_arm_arrives, -) -from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.time_sync import TimeSyncEndpoint -from bosdyn.util import seconds_to_duration - -"""List of hand image sources for asynchronous periodic query""" -HAND_IMAGE_SOURCES = [ - "hand_image", - "hand_depth", - "hand_color_image", - "hand_depth_in_hand_color_frame", -] - - -class AsyncImageService(AsyncPeriodicQuery): - """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback, image_requests): - super(AsyncImageService, self).__init__( - "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - self._image_requests = image_requests - - def _start_query(self): - if self._callback: - callback_future = self._client.get_image_async(self._image_requests) - callback_future.add_done_callback(self._callback) - return callback_future - - -class SpotArm: - def __init__( - self, - robot: Robot, - logger: logging.Logger, - robot_params: typing.Dict[str, typing.Any], - robot_clients: typing.Dict[str, typing.Any], - max_command_duration: float, - ): - """ - Constructor for SpotArm class. - Args: - robot: Robot object - logger: Logger object - robot_params: Dictionary of robot parameters - - robot_params['is_standing']: True if robot is standing, False otherwise - robot_clients: Dictionary of robot clients - - robot_clients['robot_command_client']: RobotCommandClient object - - robot_clients['robot_command_method']: RobotCommand method - max_command_duration: Maximum duration for commands when using the manipulation command method - """ - self._robot = robot - self._logger = logger - self._robot_params = robot_params - self._max_command_duration = max_command_duration - self._robot_command_client: RobotCommandClient = robot_clients[ - "robot_command_client" - ] - self._manipulation_api_client: ManipulationApiClient = robot_clients[ - "manipulation_api_client" - ] - self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] - self._image_client: ImageClient = robot_clients["image_client"] - self._robot_command: typing.Callable = robot_clients["robot_command_method"] - self._rates: typing.Dict[str, float] = robot_params["rates"] - self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] - - self._hand_image_requests = [] - self._hand_image_task: AsyncImageService = None - self.initialize_hand_image_service() - - def initialize_hand_image_service(self): - """ - Initialises the hand image task for retrieving the hand image - """ - for source in HAND_IMAGE_SOURCES: - self._hand_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._hand_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("hand_image", 0.0)), - self._callbacks.get("hand_image", None), - self._hand_image_requests, - ) - - @property - def hand_image_task(self) -> AsyncImageService: - """ - Get the hand image task - - Returns: - Async image service for getting the hand image - """ - return self._hand_image_task - - def _manipulation_request( - self, - request_proto: manipulation_api_pb2, - end_time_secs: typing.Optional[float] = None, - timesync_endpoint: typing.Optional[TimeSyncEndpoint] = None, - ): - """Generic function for sending requests to the manipulation api of a robot. - Args: - request_proto: manipulation_api_pb2 object to send to the robot. - """ - try: - command_id = self._manipulation_api_client.manipulation_api_command( - manipulation_api_request=request_proto, - end_time_secs=end_time_secs, - timesync_endpoint=timesync_endpoint, - ).manipulation_cmd_id - - return True, "Success", command_id - except Exception as e: - self._logger.error(f"Unable to execute manipulation command: {e}") - return False, str(e), None - - def manipulation_command(self, request: manipulation_api_pb2): - end_time = time.time() + self._max_command_duration - return self._manipulation_request( - request, - end_time_secs=end_time, - timesync_endpoint=self._robot.time_sync.endpoint, - ) - - def get_manipulation_command_feedback(self, cmd_id): - feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( - manipulation_cmd_id=cmd_id - ) - - return self._manipulation_api_client.manipulation_api_feedback_command( - manipulation_api_feedback_request=feedback_request - ) - - def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: - if not self._robot.has_arm(): - return False, "Spot with an arm is required for this service" - - try: - self._logger.info("Spot is powering on within the timeout of 20 secs") - self._robot.power_on(timeout_sec=20) - assert self._robot.is_powered_on(), "Spot failed to power on" - self._logger.info("Spot is powered on") - except Exception as e: - return ( - False, - f"Exception occured while Spot or its arm were trying to power on: {e}", - ) - - if not self._robot_params["is_standing"]: - robot_command.blocking_stand( - command_client=self._robot_command_client, timeout_sec=10 - ) - self._logger.info("Spot is standing") - else: - self._logger.info("Spot is already standing") - - return True, "Spot has an arm, is powered on, and standing" - - def wait_for_arm_command_to_complete(self, cmd_id, timeout_sec=None): - """ - Wait until a command issued to the arm complets. Wrapper around the SDK function for convenience - - Args: - cmd_id: ID of the command that we are waiting on - timeout_sec: After this time, timeout regardless of what the robot state is - - """ - block_until_arm_arrives( - self._robot_command_client, cmd_id=cmd_id, timeout_sec=timeout_sec - ) - - def arm_stow(self) -> typing.Tuple[bool, str]: - """ - Moves the arm to the stowed position - - Returns: - Boolean success, string message - """ - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Stow Arm - stow = RobotCommandBuilder.arm_stow_command() - - # Command issue with RobotCommandClient - cmd_id = self._robot_command_client.robot_command(stow) - self._logger.info("Command stow issued") - self.wait_for_arm_command_to_complete(cmd_id) - - except Exception as e: - return False, f"Exception occured while trying to stow: {e}" - - return True, "Stow arm success" - - def arm_unstow(self) -> typing.Tuple[bool, str]: - """ - Unstows the arm - - Returns: - Boolean success, string message - """ - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Unstow Arm - unstow = RobotCommandBuilder.arm_ready_command() - - # Command issue with RobotCommandClient - cmd_id = self._robot_command_client.robot_command(unstow) - self._logger.info("Command unstow issued") - self.wait_for_arm_command_to_complete(cmd_id) - - except Exception as e: - return False, f"Exception occured while trying to unstow: {e}" - - return True, "Unstow arm success" - - def arm_carry(self) -> typing.Tuple[bool, str]: - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Get Arm in carry mode - carry = RobotCommandBuilder.arm_carry_command() - - # Command issue with RobotCommandClient - cmd_id = self._robot_command_client.robot_command(carry) - self._logger.info("Command carry issued") - self.wait_for_arm_command_to_complete(cmd_id) - - except Exception as e: - return False, f"Exception occured while carry mode was issued: {e}" - - return True, "Carry mode success" - - def make_arm_trajectory_command( - self, arm_joint_trajectory: arm_command_pb2.ArmJointTrajectory - ) -> robot_command_pb2.RobotCommand: - """Helper function to create a RobotCommand from an ArmJointTrajectory. - Copy from 'spot-sdk/python/examples/arm_joint_move/arm_joint_move.py'""" - - joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request( - trajectory=arm_joint_trajectory - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_joint_move_command=joint_move_command - ) - sync_arm = synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - arm_sync_robot_cmd = robot_command_pb2.RobotCommand( - synchronized_command=sync_arm - ) - return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd) - - def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: - # All perspectives are given when looking at the robot from behind after the unstow service is called - # Joint1: 0.0 arm points to the front. positive: turn left, negative: turn right) - # RANGE: -3.14 -> 3.14 - # Joint2: 0.0 arm points to the front. positive: move down, negative move up - # RANGE: 0.4 -> -3.13 ( - # Joint3: 0.0 arm straight. moves the arm down - # RANGE: 0.0 -> 3.1415 - # Joint4: 0.0 middle position. negative: moves ccw, positive moves cw - # RANGE: -2.79253 -> 2.79253 - # # Joint5: 0.0 gripper points to the front. positive moves the gripper down - # RANGE: -1.8326 -> 1.8326 - # Joint6: 0.0 Gripper is not rolled, positive is ccw - # RANGE: -2.87 -> 2.87 - # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0] - if abs(joint_targets[0]) > 3.14: - msg = "Joint 1 has to be between -3.14 and 3.14" - self._logger.warning(msg) - return False, msg - elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13: - msg = "Joint 2 has to be between -3.13 and 0.4" - self._logger.warning(msg) - return False, msg - elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0: - msg = "Joint 3 has to be between 0.0 and 3.14" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[3]) > 2.79253: - msg = "Joint 4 has to be between -2.79253 and 2.79253" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[4]) > 1.8326: - msg = "Joint 5 has to be between -1.8326 and 1.8326" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[5]) > 2.87: - msg = "Joint 6 has to be between -2.87 and 2.87" - self._logger.warning(msg) - return False, msg - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - trajectory_point = ( - RobotCommandBuilder.create_arm_joint_trajectory_point( - joint_targets[0], - joint_targets[1], - joint_targets[2], - joint_targets[3], - joint_targets[4], - joint_targets[5], - ) - ) - arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory( - points=[trajectory_point] - ) - arm_command = self.make_arm_trajectory_command(arm_joint_trajectory) - - # Send the request - cmd_id = self._robot_command_client.robot_command(arm_command) - self.wait_for_arm_command_to_complete(cmd_id) - return True, "Spot Arm moved successfully" - - except Exception as e: - return False, f"Exception occured during arm movement: {e}" - - def create_wrench_from_forces_and_torques(self, forces, torques): - force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) - torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) - return geometry_pb2.Wrench(force=force, torque=torque) - - def force_trajectory(self, data) -> typing.Tuple[bool, str]: - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Duration in seconds. - traj_duration = data.duration - - # first point on trajectory - wrench0 = self.create_wrench_from_forces_and_torques( - data.forces_pt0, data.torques_pt0 - ) - t0 = seconds_to_duration(0) - traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( - wrench=wrench0, time_since_reference=t0 - ) - - # Second point on the trajectory - wrench1 = self.create_wrench_from_forces_and_torques( - data.forces_pt1, data.torques_pt1 - ) - t1 = seconds_to_duration(traj_duration) - traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( - wrench=wrench1, time_since_reference=t1 - ) - - # Build the trajectory - trajectory = trajectory_pb2.WrenchTrajectory( - points=[traj_point0, traj_point1] - ) - - # Build the trajectory request, putting all axes into force mode - arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( - root_frame_name=data.frame, - wrench_trajectory_in_task=trajectory, - x_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - y_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - z_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - rx_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - ry_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - rz_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_cartesian_command=arm_cartesian_command - ) - synchronized_command = ( - synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - ) - robot_command = robot_command_pb2.RobotCommand( - synchronized_command=synchronized_command - ) - - # Send the request - cmd_id = self._robot_command_client.robot_command(robot_command) - self._logger.info("Force trajectory command sent") - self.wait_for_arm_command_to_complete(cmd_id) - except Exception as e: - return False, f"Exception occured during arm movement: {e}" - - return True, "Moved arm successfully" - - def gripper_open(self) -> typing.Tuple[bool, str]: - """ - Fully opens the gripper - - Returns: - Boolean success, string message - """ - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Open gripper - command = RobotCommandBuilder.claw_gripper_open_command() - - # Command issue with RobotCommandClient - cmd_id = self._robot_command_client.robot_command(command) - self._logger.info("Command gripper open sent") - self.wait_for_arm_command_to_complete(cmd_id) - - except Exception as e: - return False, f"Exception occured while gripper was moving: {e}" - - return True, "Open gripper success" - - def gripper_close(self) -> typing.Tuple[bool, str]: - """ - Closes the gripper - - Returns: - Boolean success, string message - """ - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Close gripper - command = RobotCommandBuilder.claw_gripper_close_command() - - # Command issue with RobotCommandClient - cmd_id = self._robot_command_client.robot_command(command) - self._logger.info("Command gripper close sent") - self.wait_for_arm_command_to_complete(cmd_id) - - except Exception as e: - return False, f"Exception occured while gripper was moving: {e}" - - return True, "Closed gripper successfully" - - def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: - """ - Takes an angle between 0 (closed) and 90 (fully opened) and opens the gripper at this angle - - Args: - gripper_ang: Angle to which the gripper should be opened - - Returns: - Boolean success, string message - """ - if gripper_ang > 90 or gripper_ang < 0: - return False, "Gripper angle must be between 0 and 90" - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # The open angle command does not take degrees but the limits - # defined in the urdf, that is why we have to interpolate - closed = 0.349066 - opened = -1.396263 - angle = gripper_ang / 90.0 * (opened - closed) + closed - command = RobotCommandBuilder.claw_gripper_open_angle_command(angle) - - # Command issue with RobotCommandClient - cmd_id = self._robot_command_client.robot_command(command) - self._logger.info("Command gripper open angle sent") - self.wait_for_arm_command_to_complete(cmd_id) - - except Exception as e: - return False, f"Exception occured while gripper was moving: {e}" - - return True, "Opened gripper successfully" - - def hand_pose(self, data) -> typing.Tuple[bool, str]: - """ - Set the pose of the hand - - Args: - data: - - Returns: - Boolean success, string message - """ - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - pose_point = data.pose_point - # Move the arm to a spot in front of the robot given a pose for the gripper. - # Build a position to move the arm to (in meters, relative to the body frame origin.) - position = geometry_pb2.Vec3( - x=pose_point.position.x, - y=pose_point.position.y, - z=pose_point.position.z, - ) - - # # Rotation as a quaternion. - rotation = geometry_pb2.Quaternion( - w=pose_point.orientation.w, - x=pose_point.orientation.x, - y=pose_point.orientation.y, - z=pose_point.orientation.z, - ) - - seconds = data.duration - duration = seconds_to_duration(seconds) - - # Build the SE(3) pose of the desired hand position in the moving body frame. - hand_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) - hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint( - pose=hand_pose, time_since_reference=duration - ) - hand_trajectory = trajectory_pb2.SE3Trajectory( - points=[hand_pose_traj_point] - ) - - arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( - root_frame_name=frame, - pose_trajectory_in_task=hand_trajectory, - force_remain_near_current_joint_configuration=True, - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_cartesian_command=arm_cartesian_command - ) - synchronized_command = ( - synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - ) - - robot_command = robot_command_pb2.RobotCommand( - synchronized_command=synchronized_command - ) - - command = RobotCommandBuilder.build_synchro_command(robot_command) - - # Send the request - cmd_id = self._robot_command_client.robot_command(robot_command) - self._logger.info("Moving arm to position.") - self.wait_for_arm_command_to_complete(cmd_id) - - except Exception as e: - return ( - False, - f"An error occured while trying to move arm \n Exception: {e}", - ) - - return True, "Moved arm successfully" - - @staticmethod - def block_until_manipulation_completes( - manipulation_client: ManipulationApiClient, - cmd_id: int, - timeout_sec: float = None, - ) -> bool: - """ - Helper that blocks until the arm achieves a finishing state for the specific manipulation command. - - Args: - manipulation_client: manipulation client, used to request feedback - cmd_id: command ID returned by the robot when the arm movement command was sent. - timeout_sec: optional number of seconds after which we'll return no matter what - the robot's state is. - - Returns: - True if successfully completed the grasp, False if the grasp failed - """ - if timeout_sec is not None: - start_time = time.time() - end_time = start_time + timeout_sec - now = time.time() - - while timeout_sec is None or now < end_time: - feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( - manipulation_cmd_id=cmd_id - ) - - # Send the request - response = manipulation_client.manipulation_api_feedback_command( - manipulation_api_feedback_request=feedback_request - ) - manipulation_state = response.current_state - # TODO: Incorporate more of the feedback states if needed for different grasp commands. - if manipulation_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED: - return True - elif manipulation_state == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED: - return False - - time.sleep(0.1) - now = time.time() - return False - - def grasp_3d( - self, frame: str, object_rt_frame: typing.List[float] - ) -> typing.Tuple[bool, str]: - """ - Attempt to grasp an object - - Args: - frame: Frame in the which the object_rt_frame vector is given - object_rt_frame: xyz position of the object in the given frame - - Returns: - Bool indicating success, and a message with information. - """ - try: - frm = str(frame) - pos = geometry_pb2.Vec3( - x=object_rt_frame[0], y=object_rt_frame[1], z=object_rt_frame[2] - ) - - grasp = manipulation_api_pb2.PickObject(frame_name=frm, object_rt_frame=pos) - - # Ask the robot to pick up the object - grasp_request = manipulation_api_pb2.ManipulationApiRequest( - pick_object=grasp - ) - # Send the request - cmd_response = self._manipulation_api_client.manipulation_api_command( - manipulation_api_request=grasp_request - ) - - success = self.block_until_manipulation_completes( - self._manipulation_api_client, cmd_response.cmd_id - ) - - if success: - msg = "Grasped successfully" - self._logger.info(msg) - return True, msg - else: - msg = "Grasp failed." - self._logger.info(msg) - return False, msg - except Exception as e: - return False, f"An error occured while trying to grasp from pose {e}" diff --git a/spot_wrapper/spot_check.py b/spot_wrapper/spot_check.py deleted file mode 100644 index 0a5eec1e..00000000 --- a/spot_wrapper/spot_check.py +++ /dev/null @@ -1,225 +0,0 @@ -import logging -import time -import typing - -from bosdyn.api import header_pb2 -from bosdyn.client import robot_command -from bosdyn.client.lease import LeaseClient, LeaseWallet, Lease -from bosdyn.client.robot import Robot -from bosdyn.client.spot_check import SpotCheckClient, run_spot_check -from bosdyn.client.spot_check import spot_check_pb2 -from google.protobuf.timestamp_pb2 import Timestamp - - -class SpotCheck: - def __init__( - self, - robot: Robot, - logger: logging.Logger, - robot_params: typing.Dict[str, typing.Any], - robot_clients: typing.Dict[str, typing.Any], - ): - self._robot = robot - self._logger = logger - self._spot_check_client: SpotCheckClient = robot_clients["spot_check_client"] - self._robot_command_client: robot_command.RobotCommandClient = robot_clients[ - "robot_command_client" - ] - self._lease_client: LeaseClient = robot_clients["lease_client"] - self._robot_params = robot_params - self._spot_check_resp = None - self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet - - @property - def spot_check_resp(self) -> spot_check_pb2.SpotCheckFeedbackResponse: - return self._spot_check_resp - - def _get_lease(self) -> Lease: - self._lease = self._lease_wallet.get_lease() - return self._lease - - def _feedback_error_check( - self, resp: spot_check_pb2.SpotCheckFeedbackResponse - ) -> typing.Tuple[bool, str]: - """Check for errors in the feedback response""" - - # Save results from Spot Check - self._spot_check_resp = resp - - errorcode_mapping = { - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_UNEXPECTED_POWER_CHANGE: "Unexpected power change", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_IMU_CHECK: "Robot body is not flat on the ground", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_NOT_SITTING: "Robot body is not close to a sitting pose", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_LOADCELL_TIMEOUT: "Timeout during loadcell calibration", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_ON_FAILURE: "Error enabling motor power", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ENDSTOP_TIMEOUT: "Timeout during endstop calibration", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FAILED_STAND: "Robot failed to stand", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_CAMERA_TIMEOUT: "Timeout during camera check", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GROUND_CHECK: "Flat ground check failed", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_OFF_FAILURE: "Robot failed to power off", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_REVERT_FAILURE: "Robot failed to revert calibration", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FGKC_FAILURE: "Robot failed to do flat ground kinematic calibration", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GRIPPER_CAL_TIMEOUT: "Timeout during gripper calibration", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ARM_CHECK_COLLISION: "Arm motion would cause collisions", - spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ARM_CHECK_TIMEOUT: "Timeout during arm joint check", - } - # Check for common errors - if resp.header.error.code in (2, 3): - return False, str(resp.header.error.message) - if resp.error > 1: - return False, errorcode_mapping[resp.error] - - return True, "Successfully ran Spot Check" - - def _req_feedback(self) -> spot_check_pb2.SpotCheckFeedbackResponse: - start_time_seconds, start_time_ns = int(time.time()), int(time.time_ns() % 1e9) - req = spot_check_pb2.SpotCheckFeedbackRequest( - header=header_pb2.RequestHeader( - request_timestamp=Timestamp( - seconds=start_time_seconds, nanos=start_time_ns - ), - client_name="spot-check", - disable_rpc_logging=False, - ) - ) - resp: spot_check_pb2.SpotCheckFeedbackResponse = ( - self._spot_check_client.spot_check_feedback(req) - ) - - self._spot_check_resp = resp - - return resp - - def _spot_check_cmd(self, command: spot_check_pb2.SpotCheckCommandRequest): - """Send a Spot Check command""" - start_time_seconds, start_time_ns = int(time.time()), int(time.time_ns() % 1e9) - req = spot_check_pb2.SpotCheckCommandRequest( - header=header_pb2.RequestHeader( - request_timestamp=Timestamp( - seconds=start_time_seconds, nanos=start_time_ns - ), - client_name="spot-check", - disable_rpc_logging=False, - ), - lease=self._get_lease().lease_proto, - command=command, - ) - self._spot_check_client.spot_check_command(req) - - def stop_check(self) -> typing.Tuple[bool, str]: - """Stop the Spot Check - Note: This may cause the robot to enter a FaultState. Use only in emergencies. - """ - self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_ABORT) - - # Get feedback - resp = self._req_feedback() - - # Check for errors - success, status = self._feedback_error_check(resp) - - if success: - status = "Successfully stopped Spot Check" - self._logger.info(status) - else: - self._logger.error("Failed to stop Spot Check") - - return success, status - - def revert_calibration(self) -> typing.Tuple[bool, str]: - """Revert calibration for Spot Check""" - self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_REVERT_CAL) - - # Get feedback - resp = self._req_feedback() - - # Check for errors - success, status = self._feedback_error_check(resp) - - if success: - status = "Successfully reverted calibration" - self._logger.info(status) - else: - self._logger.error("Failed to revert calibration") - - return success, status - - def start_check(self) -> typing.Tuple[bool, str]: - """Start the Spot Check""" - # Make sure we're powered on and sitting - try: - self._robot.power_on() - if not self._robot_params["is_sitting"]: - robot_command.blocking_sit( - command_client=self._robot_command_client, timeout_sec=10 - ) - self._logger.info("Spot is sitting") - else: - self._logger.info("Spot is already sitting") - - self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_START) - - # Get feedback - resp = self._req_feedback() - - # Check for errors - success, status = self._feedback_error_check(resp) - - if success: - status = "Successfully started Spot Check" - self._logger.info(status) - else: - self._logger.error("Failed to start Spot Check") - return success, status - - except Exception as e: - return False, str(e) - - def blocking_check( - self, - timeout_sec: int = 360, - update_freq: float = 0.25, - verbose: bool = False, - ) -> typing.Tuple[bool, str]: - """Check the robot - Args: - timeout_sec: Timeout for the blocking check - update_freq: Frequency to update the check - verbose: Whether to print the check status - Returns: - Tuple of (success, message) - """ - try: - # Make sure we're powered on and sitting - self._robot.power_on() - if not self._robot_params["is_sitting"]: - robot_command.blocking_sit( - command_client=self._robot_command_client, timeout_sec=10 - ) - self._logger.info("Spot is sitting") - else: - self._logger.info("Spot is already sitting") - - # Check the robot and block for timeout_sec - self._logger.info("Blocking Spot Check is starting!") - resp: spot_check_pb2.SpotCheckFeedbackResponse = run_spot_check( - self._spot_check_client, - self._get_lease(), - timeout_sec, - update_freq, - verbose, - ) - - self._logger.info("Blocking Spot Check ran successfully!") - success, status = self._feedback_error_check(resp) - - return success, status - - except Exception as e: - self._logger.error("Exception thrown: {}".format(e)) - return False, str(e) - - def get_feedback(self) -> spot_check_pb2.SpotCheckFeedbackResponse: - """Get feedback from Spot Check""" - resp = self._req_feedback() - return resp[0], "Got only feedback from Spot Check" diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py deleted file mode 100644 index 1eaf91f7..00000000 --- a/spot_wrapper/spot_docking.py +++ /dev/null @@ -1,60 +0,0 @@ -import logging -import typing - -from bosdyn.api.docking import docking_pb2 -from bosdyn.client import robot_command -from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock -from bosdyn.client.robot import Robot - - -class SpotDocking: - def __init__( - self, - robot: Robot, - logger: logging.Logger, - robot_params: typing.Dict[str, typing.Any], - robot_clients: typing.Dict[str, typing.Any], - ): - self._robot = robot - self._logger = logger - self._docking_client: DockingClient = robot_clients["docking_client"] - self._robot_command_client: robot_command.RobotCommandClient = robot_clients[ - "robot_command_client" - ] - self._robot_params = robot_params - - def dock(self, dock_id: int) -> typing.Tuple[bool, str]: - """Dock the robot to the docking station with fiducial ID [dock_id].""" - try: - # Make sure we're powered on and standing - self._robot.power_on() - if not self._robot_params["is_standing"]: - robot_command.blocking_stand( - command_client=self._robot_command_client, timeout_sec=10 - ) - self._logger.info("Spot is standing") - else: - self._logger.info("Spot is already standing") - # Dock the robot - self.last_docking_command = dock_id - blocking_dock_robot(self._robot, dock_id) - self.last_docking_command = None - return True, "Success" - except Exception as e: - return False, f"Exception while trying to dock: {e}" - - def undock(self, timeout: int = 20) -> typing.Tuple[bool, str]: - """Power motors on and undock the robot from the station.""" - try: - # Maker sure we're powered on - self._robot.power_on() - # Undock the robot - blocking_undock(self._robot, timeout) - return True, "Success" - except Exception as e: - return False, f"Exception while trying to undock: {e}" - - def get_docking_state(self, **kwargs) -> docking_pb2.DockState: - """Get docking state of robot.""" - state = self._docking_client.get_docking_state(**kwargs) - return state diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py deleted file mode 100644 index 548d6ff1..00000000 --- a/spot_wrapper/spot_eap.py +++ /dev/null @@ -1,72 +0,0 @@ -import logging -import typing - -from bosdyn.client.async_tasks import AsyncPeriodicQuery -from bosdyn.client.point_cloud import PointCloudClient, build_pc_request -from bosdyn.client.robot import Robot - -"""List of point cloud sources""" -point_cloud_sources = ["velodyne-point-cloud"] - - -class AsyncPointCloudService(AsyncPeriodicQuery): - """ - Class to get point cloud at regular intervals. get_point_cloud_from_sources_async query sent to the robot at - every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback, point_cloud_requests): - super(AsyncPointCloudService, self).__init__( - "robot_point_cloud_service", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - self._point_cloud_requests = point_cloud_requests - - def _start_query(self): - if self._callback and self._point_cloud_requests: - callback_future = self._client.get_point_cloud_async( - self._point_cloud_requests - ) - callback_future.add_done_callback(self._callback) - return callback_future - - -class SpotEAP: - def __init__( - self, - robot: Robot, - logger: logging.Logger, - robot_params: typing.Dict[str, typing.Any], - robot_clients: typing.Dict[str, typing.Any], - ): - self._robot = robot - self._logger = logger - self._robot_params = robot_params - self._rates: typing.Dict[str, float] = robot_params["rates"] - self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] - self._point_cloud_client: PointCloudClient = robot_clients["point_cloud_client"] - - self._point_cloud_requests = [] - for source in point_cloud_sources: - self._point_cloud_requests.append(build_pc_request(source)) - - self._point_cloud_task = AsyncPointCloudService( - self._point_cloud_client, - self._logger, - max(0.0, self._rates.get("point_cloud", 0.0)), - self._callbacks.get("lidar_points", None), - self._point_cloud_requests, - ) - - @property - def async_task(self) -> AsyncPeriodicQuery: - """Returns the async PointCloudService task for the robot""" - return self._point_cloud_task diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py deleted file mode 100644 index dd6059a3..00000000 --- a/spot_wrapper/spot_images.py +++ /dev/null @@ -1,376 +0,0 @@ -import logging -import typing -from collections import namedtuple -from dataclasses import dataclass - -from bosdyn.api import image_pb2 -from bosdyn.client.image import ( - ImageClient, - build_image_request, - UnsupportedPixelFormatRequestedError, -) -from bosdyn.client.robot import Robot - -"""List of body image sources for periodic query""" -CAMERA_IMAGE_SOURCES = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "left_fisheye_image", - "right_fisheye_image", - "back_fisheye_image", -] -DEPTH_IMAGE_SOURCES = [ - "frontleft_depth", - "frontright_depth", - "left_depth", - "right_depth", - "back_depth", -] -DEPTH_REGISTERED_IMAGE_SOURCES = [ - "frontleft_depth_in_visual_frame", - "frontright_depth_in_visual_frame", - "right_depth_in_visual_frame", - "left_depth_in_visual_frame", - "back_depth_in_visual_frame", -] -ImageBundle = namedtuple( - "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] -) -ImageWithHandBundle = namedtuple( - "ImageBundle", ["frontleft", "frontright", "left", "right", "back", "hand"] -) - -IMAGE_SOURCES_BY_CAMERA = { - "frontleft": { - "visual": "frontleft_fisheye_image", - "depth": "frontleft_depth", - "depth_registered": "frontleft_depth_in_visual_frame", - }, - "frontright": { - "visual": "frontright_fisheye_image", - "depth": "frontright_depth", - "depth_registered": "frontright_depth_in_visual_frame", - }, - "left": { - "visual": "left_fisheye_image", - "depth": "left_depth", - "depth_registered": "left_depth_in_visual_frame", - }, - "right": { - "visual": "right_fisheye_image", - "depth": "right_depth", - "depth_registered": "right_depth_in_visual_frame", - }, - "back": { - "visual": "back_fisheye_image", - "depth": "back_depth", - "depth_registered": "back_depth_in_visual_frame", - }, - "hand": { - "visual": "hand_color_image", - "depth": "hand_depth", - "depth_registered": "hand_depth_in_hand_color_frame", - }, -} - -IMAGE_TYPES = {"visual", "depth", "depth_registered"} - - -@dataclass(frozen=True, eq=True) -class CameraSource: - camera_name: str - image_types: typing.List[str] - - -@dataclass(frozen=True) -class ImageEntry: - camera_name: str - image_type: str - image_response: image_pb2.ImageResponse - - -class SpotImages: - def __init__( - self, - robot: Robot, - logger: logging.Logger, - robot_params: typing.Dict[str, typing.Any], - robot_clients: typing.Dict[str, typing.Any], - rgb_cameras: bool = True, - ): - self._robot = robot - self._logger = logger - self._rgb_cameras = rgb_cameras - self._robot_params = robot_params - self._image_client: ImageClient = robot_clients["image_client"] - - ############################################ - self._camera_image_requests = [] - for camera_source in CAMERA_IMAGE_SOURCES: - self._camera_image_requests.append( - build_image_request( - camera_source, - image_format=image_pb2.Image.FORMAT_RAW, - ) - ) - - self._depth_image_requests = [] - for camera_source in DEPTH_IMAGE_SOURCES: - self._depth_image_requests.append( - build_image_request( - camera_source, image_format=image_pb2.Image.FORMAT_RAW - ) - ) - - self._depth_registered_image_requests = [] - for camera_source in DEPTH_REGISTERED_IMAGE_SOURCES: - self._depth_registered_image_requests.append( - build_image_request( - camera_source, image_format=image_pb2.Image.FORMAT_RAW - ) - ) - - if self._robot.has_arm(): - self._camera_image_requests.append( - build_image_request( - "hand_color_image", - image_format=image_pb2.Image.FORMAT_JPEG, - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ) - self._depth_image_requests.append( - build_image_request( - "hand_depth", - pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, - ) - ) - self._depth_registered_image_requests.append( - build_image_request( - "hand_depth_in_hand_color_frame", - pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, - ) - ) - - # Build image requests by camera - self._image_requests_by_camera = {} - for camera in IMAGE_SOURCES_BY_CAMERA: - if camera == "hand" and not self._robot.has_arm(): - continue - self._image_requests_by_camera[camera] = {} - image_types = IMAGE_SOURCES_BY_CAMERA[camera] - for image_type in image_types: - if image_type.startswith("depth"): - image_format = image_pb2.Image.FORMAT_RAW - pixel_format = image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 - else: - image_format = image_pb2.Image.FORMAT_JPEG - if camera == "hand" or self._rgb_cameras: - pixel_format = image_pb2.Image.PIXEL_FORMAT_RGB_U8 - elif camera != "hand": - self._logger.info( - f"Switching {camera}:{image_type} to greyscale image format." - ) - pixel_format = image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8 - - source = IMAGE_SOURCES_BY_CAMERA[camera][image_type] - self._image_requests_by_camera[camera][ - image_type - ] = build_image_request( - source, - image_format=image_format, - pixel_format=pixel_format, - quality_percent=75, - ) - - def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: - try: - return self._image_client.get_image( - [ - build_image_request( - "frontleft_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - self._logger.error(e) - return None - - def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: - try: - return self._image_client.get_image( - [ - build_image_request( - "frontright_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - self._logger.error(e) - return None - - def get_left_rgb_image(self) -> image_pb2.ImageResponse: - try: - return self._image_client.get_image( - [ - build_image_request( - "left_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - self._logger.error(e) - return None - - def get_right_rgb_image(self) -> image_pb2.ImageResponse: - try: - return self._image_client.get_image( - [ - build_image_request( - "right_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - self._logger.error(e) - return None - - def get_back_rgb_image(self) -> image_pb2.ImageResponse: - try: - return self._image_client.get_image( - [ - build_image_request( - "back_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - self._logger.error(e) - return None - - def get_hand_rgb_image(self): - if not self._robot.has_arm(): - return None - try: - return self._image_client.get_image( - [ - build_image_request( - "hand_color_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_images( - self, image_requests: typing.List[image_pb2.ImageRequest] - ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: - try: - image_responses = self._image_client.get_image(image_requests) - except UnsupportedPixelFormatRequestedError as e: - self._logger.error(e) - return None - if self._robot.has_arm(): - return ImageWithHandBundle( - frontleft=image_responses[0], - frontright=image_responses[1], - left=image_responses[2], - right=image_responses[3], - back=image_responses[4], - hand=image_responses[5], - ) - else: - return ImageBundle( - frontleft=image_responses[0], - frontright=image_responses[1], - left=image_responses[2], - right=image_responses[3], - back=image_responses[4], - ) - - def get_camera_images( - self, - ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: - return self.get_images(self._camera_image_requests) - - def get_depth_images( - self, - ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: - return self.get_images(self._depth_image_requests) - - def get_depth_registered_images( - self, - ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: - return self.get_images(self._depth_registered_image_requests) - - def get_images_by_cameras( - self, camera_sources: typing.List[CameraSource] - ) -> typing.Optional[typing.List[ImageEntry]]: - """Calls the GetImage RPC using the image client with requests - corresponding to the given cameras. - Args: - camera_sources: a list of CameraSource objects. There are two - possibilities for each item in this list. Either it is - CameraSource(camera='front') or - CameraSource(camera='front', image_types=['visual', 'depth_registered') - - If the former is provided, the image requests will include all - image types for each specified camera. - - If the latter is provided, the image requests will be - limited to the specified image types for each corresponding - camera. - Note that duplicates of camera names are not allowed. - Returns: - a list, where each entry is (camera_name, image_type, image_response) - e.g. ('frontleft', 'visual', image_response), or none if there was an error - """ - # Build image requests - image_requests = [] - source_types = [] - cameras_specified = set() - for item in camera_sources: - if item.camera_name in cameras_specified: - self._logger.error( - f"Duplicated camera source for camera {item.camera_name}" - ) - return None - image_types = item.image_types - if image_types is None: - image_types = IMAGE_TYPES - for image_type in image_types: - try: - image_requests.append( - self._image_requests_by_camera[item.camera_name][image_type] - ) - except KeyError: - self._logger.error( - f"Unexpected camera name '{item.camera_name}' or image type '{image_type}'" - ) - return None - source_types.append((item.camera_name, image_type)) - cameras_specified.add(item.camera_name) - - # Send image requests - try: - image_responses = self._image_client.get_image(image_requests) - except UnsupportedPixelFormatRequestedError: - self._logger.error( - "UnsupportedPixelFormatRequestedError. " - "Likely pixel_format is set wrong for some image request" - ) - return None - - # Return - result = [] - for i, (camera_name, image_type) in enumerate(source_types): - result.append( - ImageEntry( - camera_name=camera_name, - image_type=image_type, - image_response=image_responses[i], - ) - ) - return result From c03db9e78fa430b895e77b34afb17f31749d7011 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Thu, 29 Jun 2023 16:33:06 +0100 Subject: [PATCH 63/66] wrapper changes and explicit clients --- spot_wrapper/spot_graph_nav.py | 15 +- spot_wrapper/wrapper.py | 441 ++------------------------------- 2 files changed, 28 insertions(+), 428 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index ff383900..9b2c1584 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -23,16 +23,17 @@ def __init__( robot: Robot, logger: logging.Logger, robot_params: typing.Dict[str, typing.Any], - robot_clients: typing.Dict[str, typing.Any], + graph_nav_client: GraphNavClient, + map_processing_client: MapProcessingServiceClient, + robot_state_client: RobotStateClient, + lease_client: LeaseClient, ): self._robot = robot self._logger = logger - self._graph_nav_client: GraphNavClient = robot_clients["graph_nav_client"] - self._map_processing_client: MapProcessingServiceClient = robot_clients[ - "map_processing_client" - ] - self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] - self._lease_client: LeaseClient = robot_clients["lease_client"] + self._graph_nav_client = graph_nav_client + self._map_processing_client = map_processing_client + self._robot_state_client = robot_state_client + self._lease_client = lease_client self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet self._robot_params = robot_params diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 6c0d4aba..d700b886 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -46,6 +46,7 @@ ) from bosdyn.client.lease import LeaseClient, LeaseKeepAlive from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.point_cloud import build_pc_request from bosdyn.client.power import safe_power_off, PowerClient, power_on from bosdyn.client.robot import UnregisteredServiceError, Robot @@ -83,6 +84,7 @@ from bosdyn.api import basic_command_pb2 from google.protobuf.timestamp_pb2 import Timestamp +from .spot_graph_nav import SpotGraphNav from .spot_world_objects import SpotWorldObjects @@ -725,6 +727,9 @@ def __init__( self._graph_nav_client = self._robot.ensure_client( GraphNavClient.default_service_name ) + self._map_processing_client = self._robot.ensure_client( + MapProcessingServiceClient.default_service_name + ) self._power_client = self._robot.ensure_client( PowerClient.default_service_name ) @@ -845,9 +850,6 @@ def __init__( quality_percent=75, ) - # Store the most recent knowledge of the state of the robot based on rpc calls. - self._init_current_graph_nav_state() - # Async Tasks self._async_task_list = [] self._robot_state_task = AsyncRobotState( @@ -916,6 +918,16 @@ def __init__( self._estop_monitor, ] + self._spot_graph_nav = SpotGraphNav( + self._robot, + self._logger, + self._robot_params, + self._graph_nav_client, + self._map_processing_client, + self._robot_state_client, + self._lease_client, + ) + if self._point_cloud_client: self._point_cloud_task = AsyncPointCloudService( self._point_cloud_client, @@ -1000,6 +1012,11 @@ def robot_name(self) -> str: def frame_prefix(self) -> str: return self._frame_prefix + @property + def spot_graph_nav(self) -> SpotGraphNav: + """Return SpotGraphNav instance""" + return self._spot_graph_nav + @property def logger(self) -> logging.Logger: """Return logger instance of the SpotWrapper""" @@ -1581,64 +1598,6 @@ def get_manipulation_command_feedback(self, cmd_id): manipulation_api_feedback_request=feedback_request ) - def list_graph(self, upload_path=None): - """List waypoint ids of garph_nav - Args: - upload_path : Path to the root directory of the map. - """ - ids, eds = self._list_graph_waypoint_and_edge_ids() - # skip waypoint_ for v2.2.1, skip waypiont for < v2.2 - return [ - v - for k, v in sorted( - ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) - ) - ] - - @try_claim - def navigate_to( - self, - upload_path, - navigate_to, - initial_localization_fiducial=True, - initial_localization_waypoint=None, - ): - """navigate with graph nav. - - Args: - upload_path : Path to the root directory of the map. - navigate_to : Waypont id string for where to goal - initial_localization_fiducial : Tells the initializer whether to use fiducials - initial_localization_waypoint : Waypoint id string of current robot position (optional) - """ - # Filepath for uploading a saved graph's and snapshots too. - if upload_path[-1] == "/": - upload_filepath = upload_path[:-1] - else: - upload_filepath = upload_path - - # Boolean indicating the robot's power state. - power_state = self._robot_state_client.get_robot_state().power_state - self._started_powered_on = power_state.motor_power_state == power_state.STATE_ON - self._powered_on = self._started_powered_on - - # FIX ME somehow,,,, if the robot is stand, need to sit the robot before starting garph nav - if self.is_standing and not self.is_moving: - self.sit() - - # TODO verify estop / claim / power_on - self._clear_graph() - self._upload_graph_and_snapshots(upload_filepath) - if initial_localization_fiducial: - self._set_initial_localization_fiducial() - if initial_localization_waypoint: - self._set_initial_localization_waypoint([initial_localization_waypoint]) - self._list_graph_waypoint_and_edge_ids() - self._get_localization_state() - resp = self._navigate_to([navigate_to]) - - return resp - # Arm ############################################ @try_claim def ensure_arm_power_and_stand(self): @@ -2099,349 +2058,6 @@ def grasp_3d(self, frame, object_rt_frame): ################################################################### - def _init_current_graph_nav_state(self): - # Store the most recent knowledge of the state of the robot based on rpc calls. - self._current_graph = None - self._current_edges = dict() # maps to_waypoint to list(from_waypoint) - self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot - self._current_edge_snapshots = dict() # maps id to edge snapshot - self._current_annotation_name_to_wp_id = dict() - self._current_anchored_world_objects = ( - dict() - ) # maps object id to a (wo, waypoint, fiducial) - self._current_anchors = dict() # maps anchor id to anchor - - ## copy from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py - def _get_localization_state(self, *args): - """Get the current localization and state of the robot.""" - state = self._graph_nav_client.get_localization_state() - self._logger.info("Got localization: \n%s" % str(state.localization)) - odom_tform_body = get_odom_tform_body( - state.robot_kinematics.transforms_snapshot - ) - self._logger.info( - "Got robot state in kinematic odometry frame: \n%s" % str(odom_tform_body) - ) - - def _set_initial_localization_fiducial(self, *args): - """Trigger localization when near a fiducial.""" - robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() - # Create an empty instance for initial localization since we are asking it to localize - # based on the nearest fiducial. - localization = nav_pb2.Localization() - self._graph_nav_client.set_localization( - initial_guess_localization=localization, - ko_tform_body=current_odom_tform_body, - ) - - def _set_initial_localization_waypoint(self, *args): - """Trigger localization to a waypoint.""" - # Take the first argument as the localization waypoint. - if len(args) < 1: - # If no waypoint id is given as input, then return without initializing. - self._logger.error("No waypoint specified to initialize to.") - return - destination_waypoint = graph_nav_util.find_unique_waypoint_id( - args[0][0], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not destination_waypoint: - # Failed to find the unique waypoint id. - return - - robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() - # Create an initial localization to the specified waypoint as the identity. - localization = nav_pb2.Localization() - localization.waypoint_id = destination_waypoint - localization.waypoint_tform_body.rotation.w = 1.0 - self._graph_nav_client.set_localization( - initial_guess_localization=localization, - # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). - max_distance=0.2, - max_yaw=20.0 * math.pi / 180.0, - fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NO_FIDUCIAL, - ko_tform_body=current_odom_tform_body, - ) - - def _list_graph_waypoint_and_edge_ids(self, *args): - """List the waypoint ids and edge ids of the graph currently on the robot.""" - - # Download current graph - graph = self._graph_nav_client.download_graph() - if graph is None: - self._logger.error("Empty graph.") - return - self._current_graph = graph - - localization_id = ( - self._graph_nav_client.get_localization_state().localization.waypoint_id - ) - - # Update and print waypoints and edges - ( - self._current_annotation_name_to_wp_id, - self._current_edges, - ) = graph_nav_util.update_waypoints_and_edges( - graph, localization_id, self._logger - ) - return self._current_annotation_name_to_wp_id, self._current_edges - - def _upload_graph_and_snapshots(self, upload_filepath): - """Upload the graph and snapshots to the robot.""" - self._logger.info("Loading the graph from disk into local storage...") - with open(os.path.join(upload_filepath, "graph"), "rb") as graph_file: - # Load the graph from disk. - data = graph_file.read() - self._current_graph = map_pb2.Graph() - self._current_graph.ParseFromString(data) - self._logger.info( - "Loaded graph has {} waypoints and {} edges".format( - len(self._current_graph.waypoints), len(self._current_graph.edges) - ) - ) - for waypoint in self._current_graph.waypoints: - # Load the waypoint snapshots from disk. - if len(waypoint.snapshot_id) == 0: - continue - waypoint_filepath = os.path.join( - upload_filepath, "waypoint_snapshots", waypoint.snapshot_id - ) - if not os.path.exists(waypoint_filepath): - continue - with open(waypoint_filepath, "rb") as snapshot_file: - waypoint_snapshot = map_pb2.WaypointSnapshot() - waypoint_snapshot.ParseFromString(snapshot_file.read()) - self._current_waypoint_snapshots[ - waypoint_snapshot.id - ] = waypoint_snapshot - - for fiducial in waypoint_snapshot.objects: - if not fiducial.HasField("apriltag_properties"): - continue - - str_id = str(fiducial.apriltag_properties.tag_id) - if ( - str_id in self._current_anchored_world_objects - and len(self._current_anchored_world_objects[str_id]) == 1 - ): - # Replace the placeholder tuple with a tuple of (wo, waypoint, fiducial). - anchored_wo = self._current_anchored_world_objects[str_id][0] - self._current_anchored_world_objects[str_id] = ( - anchored_wo, - waypoint, - fiducial, - ) - - for edge in self._current_graph.edges: - # Load the edge snapshots from disk. - if len(edge.snapshot_id) == 0: - continue - edge_filepath = os.path.join( - upload_filepath, "edge_snapshots", edge.snapshot_id - ) - if not os.path.exists(edge_filepath): - continue - with open(edge_filepath, "rb") as snapshot_file: - edge_snapshot = map_pb2.EdgeSnapshot() - edge_snapshot.ParseFromString(snapshot_file.read()) - self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot - for anchor in self._current_graph.anchoring.anchors: - self._current_anchors[anchor.id] = anchor - # Upload the graph to the robot. - self._logger.info("Uploading the graph and snapshots to the robot...") - if self._lease is None: - self.getLease() - self._graph_nav_client.upload_graph( - lease=self._lease.lease_proto, graph=self._current_graph - ) - # Upload the snapshots to the robot. - for waypoint_snapshot in self._current_waypoint_snapshots.values(): - self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) - self._logger.info("Uploaded {}".format(waypoint_snapshot.id)) - for edge_snapshot in self._current_edge_snapshots.values(): - self._graph_nav_client.upload_edge_snapshot(edge_snapshot) - self._logger.info("Uploaded {}".format(edge_snapshot.id)) - - # The upload is complete! Check that the robot is localized to the graph, - # and it if is not, prompt the user to localize the robot before attempting - # any navigation commands. - localization_state = self._graph_nav_client.get_localization_state() - if not localization_state.localization.waypoint_id: - # The robot is not localized to the newly uploaded graph. - self._logger.info( - "Upload complete! The robot is currently not localized to the map; " - "please localize the robot" - ) - - @try_claim - def _navigate_to(self, *args): - """Navigate to a specific waypoint.""" - # Take the first argument as the destination waypoint. - if len(args) < 1: - # If no waypoint id is given as input, then return without requesting navigation. - self._logger.info("No waypoint provided as a destination for navigate to.") - return - - self._lease = self._lease_wallet.get_lease() - destination_waypoint = graph_nav_util.find_unique_waypoint_id( - args[0][0], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not destination_waypoint: - # Failed to find the appropriate unique waypoint id for the navigation command. - return - if not self.toggle_power(should_power_on=True): - self._logger.info( - "Failed to power on the robot, and cannot complete navigate to request." - ) - return - - # Stop the lease keepalive and create a new sublease for graph nav. - self._lease = self._lease_wallet.advance() - sublease = self._lease.create_sublease() - self._lease_keepalive.shutdown() - - # Navigate to the destination waypoint. - is_finished = False - nav_to_cmd_id = -1 - while not is_finished: - # Issue the navigation command about twice a second such that it is easy to terminate the - # navigation command (with estop or killing the program). - nav_to_cmd_id = self._graph_nav_client.navigate_to( - destination_waypoint, 1.0, leases=[sublease.lease_proto] - ) - time.sleep(0.5) # Sleep for half a second to allow for command execution. - # Poll the robot for feedback to determine if the navigation command is complete. Then sit - # the robot down once it is finished. - is_finished = self._check_success(nav_to_cmd_id) - - self._lease = self._lease_wallet.advance() - self._lease_keepalive = LeaseKeepAlive(self._lease_client) - - # Update the lease and power off the robot if appropriate. - if self._powered_on and not self._started_powered_on: - # Sit the robot down + power off after the navigation command is complete. - self.toggle_power(should_power_on=False) - - status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): - return True, "Successfully completed the navigation commands!" - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - return ( - False, - "Robot got lost when navigating the route, the robot will now sit down.", - ) - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - return ( - False, - "Robot got stuck when navigating the route, the robot will now sit down.", - ) - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): - return False, "Robot is impaired." - else: - return False, "Navigation command is not complete yet." - - @try_claim - def _navigate_route(self, *args): - """Navigate through a specific route of waypoints.""" - if len(args) < 1: - # If no waypoint ids are given as input, then return without requesting navigation. - self._logger.error("No waypoints provided for navigate route.") - return - waypoint_ids = args[0] - for i in range(len(waypoint_ids)): - waypoint_ids[i] = graph_nav_util.find_unique_waypoint_id( - waypoint_ids[i], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not waypoint_ids[i]: - # Failed to find the unique waypoint id. - return - - edge_ids_list = [] - all_edges_found = True - # Attempt to find edges in the current graph that match the ordered waypoint pairs. - # These are necessary to create a valid route. - for i in range(len(waypoint_ids) - 1): - start_wp = waypoint_ids[i] - end_wp = waypoint_ids[i + 1] - edge_id = self._match_edge(self._current_edges, start_wp, end_wp) - if edge_id is not None: - edge_ids_list.append(edge_id) - else: - all_edges_found = False - self._logger.error( - "Failed to find an edge between waypoints: ", - start_wp, - " and ", - end_wp, - ) - self._logger.error( - "List the graph's waypoints and edges to ensure pairs of waypoints has an edge." - ) - break - - self._lease = self._lease_wallet.get_lease() - if all_edges_found: - if not self.toggle_power(should_power_on=True): - self._logger.error( - "Failed to power on the robot, and cannot complete navigate route request." - ) - return - - # Stop the lease keepalive and create a new sublease for graph nav. - self._lease = self._lease_wallet.advance() - sublease = self._lease.create_sublease() - self._lease_keepalive.shutdown() - - # Navigate a specific route. - route = self._graph_nav_client.build_route(waypoint_ids, edge_ids_list) - is_finished = False - while not is_finished: - # Issue the route command about twice a second such that it is easy to terminate the - # navigation command (with estop or killing the program). - nav_route_command_id = self._graph_nav_client.navigate_route( - route, cmd_duration=1.0, leases=[sublease.lease_proto] - ) - time.sleep( - 0.5 - ) # Sleep for half a second to allow for command execution. - # Poll the robot for feedback to determine if the route is complete. Then sit - # the robot down once it is finished. - is_finished = self._check_success(nav_route_command_id) - - self._lease = self._lease_wallet.advance() - self._lease_keepalive = LeaseKeepAlive(self._lease_client) - - # Update the lease and power off the robot if appropriate. - if self._powered_on and not self._started_powered_on: - # Sit the robot down + power off after the navigation command is complete. - self.toggle_power(should_power_on=False) - - def _clear_graph(self, *args): - """Clear the state of the map on the robot, removing all waypoints and edges.""" - result = self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) - self._init_current_graph_nav_state() - return result - @try_claim def toggle_power(self, should_power_on): """Power the robot on/off dependent on the current power state.""" @@ -2512,23 +2128,6 @@ def _check_success(self, command_id=-1): # Navigation command is not complete yet. return False - def _match_edge(self, current_edges, waypoint1, waypoint2): - """Find an edge in the graph that is between two waypoint ids.""" - # Return the correct edge id as soon as it's found. - for edge_to_id in current_edges: - for edge_from_id in current_edges[edge_to_id]: - if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): - # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint2, to_waypoint=waypoint1 - ) - elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): - # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint1, to_waypoint=waypoint2 - ) - return None - @try_claim def dock(self, dock_id): """Dock the robot to the docking station with fiducial ID [dock_id].""" From 3efe9edccdfa442cfb38830f6361a060c5e9b3ff Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Thu, 29 Jun 2023 16:35:33 +0100 Subject: [PATCH 64/66] doesn't actually need robot_params --- spot_wrapper/spot_graph_nav.py | 14 ++++++-------- spot_wrapper/wrapper.py | 1 - 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 9b2c1584..764c3cbc 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -22,7 +22,6 @@ def __init__( self, robot: Robot, logger: logging.Logger, - robot_params: typing.Dict[str, typing.Any], graph_nav_client: GraphNavClient, map_processing_client: MapProcessingServiceClient, robot_state_client: RobotStateClient, @@ -35,7 +34,6 @@ def __init__( self._robot_state_client = robot_state_client self._lease_client = lease_client self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet - self._robot_params = robot_params self._init_current_graph_nav_state() @@ -46,14 +44,14 @@ def _get_lease(self) -> Lease: def _init_current_graph_nav_state(self): # Store the most recent knowledge of the state of the robot based on rpc calls. self._current_graph = None - self._current_edges = dict() # maps to_waypoint to list(from_waypoint) - self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot - self._current_edge_snapshots = dict() # maps id to edge snapshot - self._current_annotation_name_to_wp_id = dict() + self._current_edges = {} # maps to_waypoint to list(from_waypoint) + self._current_waypoint_snapshots = {} # maps id to waypoint snapshot + self._current_edge_snapshots = {} # maps id to edge snapshot + self._current_annotation_name_to_wp_id = {} self._current_anchored_world_objects = ( - dict() + {} ) # maps object id to a (wo, waypoint, fiducial) - self._current_anchors = dict() # maps anchor id to anchor + self._current_anchors = {} # maps anchor id to anchor def list_graph(self) -> typing.List[str]: """List waypoint ids of graph_nav diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index d700b886..feeb0d94 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -921,7 +921,6 @@ def __init__( self._spot_graph_nav = SpotGraphNav( self._robot, self._logger, - self._robot_params, self._graph_nav_client, self._map_processing_client, self._robot_state_client, From 1ce13ffeb112d0bf4a0a10c2194b3a0dc3fdd7d0 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Wed, 5 Jul 2023 19:16:48 +0100 Subject: [PATCH 65/66] Remove import of graph nav utils --- spot_wrapper/wrapper.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index f81ae1e3..61f31ba6 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -75,12 +75,6 @@ SPOT_CLIENT_NAME = "ros_spot" MAX_COMMAND_DURATION = 1e5 -### Release -from . import graph_nav_util - -### Debug -# import graph_nav_util - from bosdyn.api import basic_command_pb2 from google.protobuf.timestamp_pb2 import Timestamp From c85fe4ad56df55f04cc8cca1cbd6e7824478cd51 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Mon, 31 Jul 2023 20:40:40 +0100 Subject: [PATCH 66/66] Try-claim decorator converted to an object (#55) * change the try-claim decorator to an object * claim decorator takes functions it needs, no longer relies on power being decorated Previously the claim decorator assumed that the power_on function was decorated with the same decorator. If power on was requested, the power on function would do the claiming, and then power on the system. That is kind of confusing, so now the decorator will call claim first, and then optionally power on. Also takes the power on and claim functions, rather than calling the ones on the wrapper itself. * formatting * move decorator object to helper file, rename and make functions better, decorate docking functions * formatting * remove mistakenly merged robot command data and state * decorate arm commands * Fixed bugs with decorated functions * black --------- Co-authored-by: Andrew Messing <129519955+amessing-bdai@users.noreply.github.com> Co-authored-by: Andrew Messing --- spot_wrapper/spot_arm.py | 25 +++++++- spot_wrapper/spot_docking.py | 13 +++- spot_wrapper/wrapper.py | 90 ++++++++++++-------------- spot_wrapper/wrapper_helpers.py | 110 ++++++++++++++++++++++++++++++++ 4 files changed, 187 insertions(+), 51 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index c671b881..b1ba2263 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -21,7 +21,7 @@ from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.util import seconds_to_duration -from spot_wrapper.wrapper_helpers import RobotState +from spot_wrapper.wrapper_helpers import RobotState, ClaimAndPowerDecorator class SpotArm: @@ -34,13 +34,19 @@ def __init__( manipulation_api_client: ManipulationApiClient, robot_state_client: RobotStateClient, max_command_duration: float, + claim_and_power_decorator: ClaimAndPowerDecorator, ) -> None: """ Constructor for SpotArm class. Args: robot: Robot object logger: Logger object + robot_state: Object containing the robot's state as controlled by the wrapper + robot_command_client: Command client to use to send commands to the robot + manipulation_api_client: Command client to send manipulation commands to the robot + robot_state_client: Client to retrieve state of the robot max_command_duration: Maximum duration for commands when using the manipulation command method + claim_and_power_decorator: Object to use to decorate the functions on this object """ self._robot = robot self._logger = logger @@ -49,6 +55,23 @@ def __init__( self._robot_command_client = robot_command_client self._manipulation_api_client = manipulation_api_client self._robot_state_client = robot_state_client + self._claim_and_power_decorator = claim_and_power_decorator + self._claim_and_power_decorator.decorate_functions( + self, + decorated_funcs=[ + self.ensure_arm_power_and_stand, + self.arm_stow, + self.arm_unstow, + self.arm_carry, + self.arm_joint_move, + self.force_trajectory, + self.gripper_open, + self.gripper_close, + self.gripper_angle_open, + self.hand_pose, + self.grasp_3d, + ], + ) def _manipulation_request( self, diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py index 63a88f83..085152cf 100644 --- a/spot_wrapper/spot_docking.py +++ b/spot_wrapper/spot_docking.py @@ -6,7 +6,11 @@ from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock from bosdyn.client.robot import Robot -from spot_wrapper.wrapper_helpers import RobotState, RobotCommandData +from spot_wrapper.wrapper_helpers import ( + RobotState, + RobotCommandData, + ClaimAndPowerDecorator, +) class SpotDocking: @@ -22,6 +26,7 @@ def __init__( command_data: RobotCommandData, docking_client: DockingClient, robot_command_client: robot_command.RobotCommandClient, + claim_and_power_decorator: ClaimAndPowerDecorator, ) -> None: self._robot = robot self._logger = logger @@ -29,6 +34,12 @@ def __init__( self._docking_client: DockingClient = docking_client self._robot_command_client = robot_command_client self._robot_state = robot_state + self._claim_and_power_decorator = claim_and_power_decorator + # Decorate the functions so that they take the lease. Dock function needs to power on because it might have + # to move the robot, the undock + self._claim_and_power_decorator.decorate_functions( + self, decorated_funcs=[self.dock, self.undock] + ) def dock(self, dock_id: int) -> typing.Tuple[bool, str]: """Dock the robot to the docking station with fiducial ID [dock_id].""" diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 42777b03..ae2329a2 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -84,7 +84,7 @@ from .spot_images import SpotImages from .spot_world_objects import SpotWorldObjects -from .wrapper_helpers import RobotCommandData, RobotState +from .wrapper_helpers import RobotCommandData, RobotState, ClaimAndPowerDecorator """Service name for getting pointcloud of VLP16 connected to Spot Core""" point_cloud_sources = ["velodyne-point-cloud"] @@ -379,38 +379,6 @@ def _start_query(self): pass -def try_claim(func=None, *, power_on=False): - """ - Decorator which tries to acquire the lease before executing the wrapped function - - the _func=None and * args are required to allow this decorator to be used with or without arguments - - Args: - func: Function that is being wrapped - power_on: If true, power on after claiming the lease - - Returns: - Decorator which will wrap the decorated function - """ - # If this decorator is being used without the power_on arg, return it as if it was called with that arg specified - if func is None: - return functools.partial(try_claim, power_on=power_on) - - @functools.wraps(func) - def wrapper_try_claim(self, *args, **kwargs): - if self._get_lease_on_action: - if power_on: - # Power on is also wrapped by this decorator so if we request power on the lease will also be claimed - response = self.power_on() - else: - response = self.claim() - if not response[0]: - return response - return func(self, *args, **kwargs) - - return wrapper_try_claim - - class SpotWrapper: """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically""" @@ -459,7 +427,10 @@ def __init__( self._rates = rates or {} self._callbacks = callbacks or {} self._use_take_lease = use_take_lease - self._get_lease_on_action = get_lease_on_action + self._claim_decorator = ClaimAndPowerDecorator( + self.power_on, self.claim, get_lease_on_action + ) + self.decorate_functions() self._continually_try_stand = continually_try_stand self._rgb_cameras = rgb_cameras self._frame_prefix = "" @@ -653,6 +624,7 @@ def __init__( self._manipulation_api_client, self._robot_state_client, MAX_COMMAND_DURATION, + self._claim_decorator, ) else: self._spot_arm = None @@ -666,6 +638,7 @@ def __init__( self._command_data, self._docking_client, self._robot_command_client, + self._claim_decorator, ) if self._point_cloud_client: @@ -701,6 +674,40 @@ def __init__( self._robot_id = None self._lease = None + def decorate_functions(self): + """ + Many of the functions in the wrapper need to have the lease claimed and the robot powered on before they will + function. The TryClaimDecorator object includes a decorator which is the mechanism we use to make sure that + is the case, assuming the get_lease_on_action variable is true. Otherwise, it is up to the user to ensure + that the lease is claimed and the power is on before running commands, otherwise the commands will fail. + """ + decorated_funcs = [ + self.stop, + self.self_right, + self.sit, + self.simple_stand, + self.stand, + self.battery_change_pose, + self.velocity_cmd, + self.trajectory_cmd, + self.navigate_to, + self._navigate_to, + self._navigate_route, + self.execute_dance, + self._robot_command, + self._manipulation_request, + ] + decorated_funcs_no_power = [ + self.stop, + self.power_on, + self.safe_power_off, + self.toggle_power, + ] + + self._claim_decorator.decorate_functions( + self, decorated_funcs, decorated_funcs_no_power + ) + @staticmethod def authenticate( robot: Robot, username: str, password: str, logger: logging.Logger @@ -1136,7 +1143,6 @@ def _manipulation_request( self._logger.error(f"Unable to execute manipulation command: {e}") return False, str(e), None - @try_claim def stop(self) -> typing.Tuple[bool, str]: """ Stop any action the robot is currently doing. @@ -1148,7 +1154,6 @@ def stop(self) -> typing.Tuple[bool, str]: response = self._robot_command(RobotCommandBuilder.stop_command()) return response[0], response[1] - @try_claim(power_on=True) def self_right(self) -> typing.Tuple[bool, str]: """ Have the robot self-right. @@ -1159,7 +1164,6 @@ def self_right(self) -> typing.Tuple[bool, str]: response = self._robot_command(RobotCommandBuilder.selfright_command()) return response[0], response[1] - @try_claim(power_on=True) def sit(self) -> typing.Tuple[bool, str]: """ Stop the robot's motion and sit down if able. @@ -1172,7 +1176,6 @@ def sit(self) -> typing.Tuple[bool, str]: self.last_sit_command = response[2] return response[0], response[1] - @try_claim(power_on=True) def simple_stand(self, monitor_command: bool = True) -> typing.Tuple[bool, str]: """ If the e-stop is enabled, and the motor power is enabled, stand the robot up. @@ -1187,7 +1190,6 @@ def simple_stand(self, monitor_command: bool = True) -> typing.Tuple[bool, str]: self.last_stand_command = response[2] return response[0], response[1] - @try_claim(power_on=True) def stand( self, monitor_command: bool = True, @@ -1231,7 +1233,6 @@ def stand( self.last_stand_command = response[2] return response[0], response[1] - @try_claim(power_on=True) def battery_change_pose(self, dir_hint: int = 1) -> typing.Tuple[bool, str]: """ Put the robot into the battery change pose @@ -1249,7 +1250,6 @@ def battery_change_pose(self, dir_hint: int = 1) -> typing.Tuple[bool, str]: return response[0], response[1] return False, "Call sit before trying to roll over" - @try_claim def safe_power_off(self) -> typing.Tuple[bool, str]: """ Stop the robot's motion and sit if possible. Once sitting, disable motor power. @@ -1277,7 +1277,6 @@ def clear_behavior_fault( except Exception as e: return False, f"Exception while clearing behavior fault: {e}", None - @try_claim def power_on(self) -> typing.Tuple[bool, str]: """ Enable the motor power if e-stop is enabled. @@ -1314,7 +1313,6 @@ def get_mobility_params(self) -> spot_command_pb2.MobilityParams: """Get mobility params""" return self._mobility_params - @try_claim def velocity_cmd( self, v_x: float, v_y: float, v_rot: float, cmd_duration: float = 0.125 ) -> typing.Tuple[bool, str]: @@ -1342,7 +1340,6 @@ def velocity_cmd( self.last_velocity_command_time = end_time return response[0], response[1] - @try_claim def trajectory_cmd( self, goal_x: float, @@ -1530,7 +1527,6 @@ def download_graph(self, download_path: str) -> typing.Tuple[bool, str]: f"Got an error during downloading graph and snapshots from the robot: {e}", ) - @try_claim def navigate_to( self, upload_path, @@ -1817,7 +1813,6 @@ def _download_graph_and_snapshots( ) return True, "Success" - @try_claim def _navigate_to(self, *args): """Navigate to a specific waypoint.""" # Take the first argument as the destination waypoint. @@ -1893,7 +1888,6 @@ def _navigate_to(self, *args): else: return False, "Navigation command is not complete yet." - @try_claim def _navigate_route(self, *args): """Navigate through a specific route of waypoints.""" if len(args) < 1: @@ -1978,7 +1972,6 @@ def _clear_graph(self, *args): self._init_current_graph_nav_state() return result - @try_claim def toggle_power(self, should_power_on): """Power the robot on/off dependent on the current power state.""" is_powered_on = self.check_is_powered_on() @@ -2065,7 +2058,6 @@ def _match_edge(self, current_edges, waypoint1, waypoint2): ) return None - @try_claim def execute_dance(self, data): if self._is_licensed_for_choreography: return self._spot_dance.execute_dance(data) diff --git a/spot_wrapper/wrapper_helpers.py b/spot_wrapper/wrapper_helpers.py index c824cba4..bf2f816b 100644 --- a/spot_wrapper/wrapper_helpers.py +++ b/spot_wrapper/wrapper_helpers.py @@ -1,9 +1,119 @@ """Helper classes for the wrapper. This file is necessary to prevent circular imports caused by the modules also using these classes""" import typing +import functools from dataclasses import dataclass +class ClaimAndPowerDecorator: + """ + Some functions in the wrapper require the lease to be claimed and the robot to be powered on before they can + function. + + This class provides a portable way of wrapping the functions of the wrapper or modules to enable them to do + that. It can be passed around to modules which can then decorate their functions with it, allowing them to claim + and power on as needed. + + Note that this decorator is not intended to be applied using the @ syntax. It should be applied during or after + object instantiation. + """ + + def __init__( + self, + power_on_function: typing.Callable[[], typing.Tuple[bool, str]], + claim_function: typing.Callable[[], typing.Tuple[bool, str]], + get_lease_on_action: bool = False, + ) -> None: + self.power_on = power_on_function + self.claim = claim_function + self._get_lease_on_action = get_lease_on_action + + def _make_function_take_lease_and_power_on( + self, func: typing.Callable, power_on: bool = True + ) -> typing.Callable: + """ + Decorator which tries to acquire the lease before executing the wrapped function + + Args: + func: Function that is being wrapped + power_on: If true, power on after claiming the lease. For the vast majority of cases this is needed + + Returns: + Decorator which will wrap the decorated function + """ + + @functools.wraps(func) + def wrapper_try_claim(*args, **kwargs) -> typing.Callable: + # Note that because we are assuming that this decorator is used only on instantiated classes, + # this function does not take a self arg. The self arg is necessary when using the @ syntax because at + # that point the class has not yet been instantiated. In this case, the func we receive is already a bound + # method, as opposed to an unbound one. A bound function has the "self" instance built in. + if self._get_lease_on_action: + # Ignore success or failure of these functions. If they fail, then the function that is being wrapped + # will fail and the caller will be able to handle from there. + self.claim() + if power_on: + self.power_on() + return func(*args, **kwargs) + + return wrapper_try_claim + + def make_function_take_lease_and_power_on( + self, decorated_object, function: typing.Callable, power_on: bool = True + ) -> None: + """ + Decorate a function of an object with this class's decorator. After being decorated, when the function is + called, it will forcefully take the lease, then power on the robot if that option is specified. + + Args: + decorated_object: The object whose function is to be decorated + function: The function to be decorated + power_on: If true, power on the robot after claiming + + Raises: + AttributeError: if the object passed does not have a function with the same name as the given function + """ + function_name = function.__name__ + if not hasattr(decorated_object, function_name): + raise AttributeError( + f"Requested decoration of function {function_name} of object {decorated_object}, but the object does " + f"not have a function by that name." + ) + + setattr( + decorated_object, + function_name, + self._make_function_take_lease_and_power_on(function, power_on=power_on), + ) + + def decorate_functions( + self, + decorated_object, + decorated_funcs: typing.List[typing.Callable], + decorated_funcs_no_power: typing.Optional[typing.List[typing.Callable]] = None, + ) -> None: + """ + Decorate the specified functions of the given object with this class's decorator. + + Args: + decorated_object: Object which contains the functions to be decorated + decorated_funcs: List of the functions of the object which should be decorated. When called, + these functions will forcefully take the lease and power on the robot + decorated_funcs_no_power: Same as decorated_funcs, but will calling these will not power on the robot. + """ + + for func in decorated_funcs: + self.make_function_take_lease_and_power_on(decorated_object, func) + + if decorated_funcs_no_power is None: + decorated_funcs_no_power = [] + + for func in decorated_funcs_no_power: + self.make_function_take_lease_and_power_on( + decorated_object, func, power_on=False + ) + + @dataclass() class RobotState: """