diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml new file mode 100644 index 00000000..08636ac9 --- /dev/null +++ b/.github/workflows/graph_nav_util_test.yml @@ -0,0 +1,53 @@ +name: graph_nav_util unit tests + +# Run on every push +on: push + +jobs: + test: + runs-on: ubuntu-latest + defaults: + run: + shell: bash + + steps: + - name: Make directory + run: mkdir -p ~/spot_wrapper + + - name: Checkout repository + uses: actions/checkout@v3 + with: + path: spot_wrapper + + - name: Check files + run: ls spot_wrapper + + - name: Set up Python + uses: actions/setup-python@v2 + with: + python-version: "3.8" + + - name: Install dependencies + run: | + pip install -r spot_wrapper/requirements.txt + pip install -e spot_wrapper + + - name: Run 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 0e57211a..2c4b4fde 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,4 @@ bosdyn_core==3.2.3 protobuf==4.22.1 setuptools==45.2.0 +pytest==7.3.1 \ No newline at end of file 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/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..4a599021 --- /dev/null +++ b/spot_wrapper/spot_arm.py @@ -0,0 +1,687 @@ +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 new file mode 100644 index 00000000..0a5eec1e --- /dev/null +++ b/spot_wrapper/spot_check.py @@ -0,0 +1,225 @@ +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_dance.py b/spot_wrapper/spot_dance.py index c52c0800..add4c8b9 100644 --- a/spot_wrapper/spot_dance.py +++ b/spot_wrapper/spot_dance.py @@ -2,6 +2,7 @@ import tempfile import os +from bosdyn.choreography.client.choreography import ChoreographyClient from bosdyn.choreography.client.choreography import ( load_choreography_sequence_from_txt_file, ChoreographyClient, diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py new file mode 100644 index 00000000..1eaf91f7 --- /dev/null +++ b/spot_wrapper/spot_docking.py @@ -0,0 +1,60 @@ +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 new file mode 100644 index 00000000..548d6ff1 --- /dev/null +++ b/spot_wrapper/spot_eap.py @@ -0,0 +1,72 @@ +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_graph_nav.py b/spot_wrapper/spot_graph_nav.py new file mode 100644 index 00000000..ff383900 --- /dev/null +++ b/spot_wrapper/spot_graph_nav.py @@ -0,0 +1,811 @@ +import logging +import math +import os +import time +import typing + +from bosdyn.api.graph_nav import graph_nav_pb2 +from bosdyn.api.graph_nav import map_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, Lease +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 + + +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 + + 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 + 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 + 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._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 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 + # 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(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._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 + ) + # 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._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.""" + 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 + + 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)) + 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.""" + tokens = id.split("-") + if len(tokens) > 2: + return f"{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..dd6059a3 --- /dev/null +++ b/spot_wrapper/spot_images.py @@ -0,0 +1,376 @@ +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 diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py new file mode 100644 index 00000000..ae228729 --- /dev/null +++ b/spot_wrapper/spot_world_objects.py @@ -0,0 +1,65 @@ +import logging +import typing + +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.robot import Robot +from bosdyn.client.world_object import WorldObjectClient + + +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..18ba04d2 --- /dev/null +++ b/spot_wrapper/tests/test_graph_nav_util.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 +import pytest +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: + def test_id_to_short_code(self): + assert ( + graph_nav_util._id_to_short_code("ebony-pug-mUzxLNq.TkGlVIxga+UKAQ==") + == "ep" + ) + assert ( + graph_nav_util._id_to_short_code("erose-simian-sug9xpxhCxgft7Mtbhr98A==") + == "es" + ) + + +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"} + # Test normal short code + assert ( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ) + == "AC" + ) + # Test annotation name that is known + assert ( + graph_nav_util._find_unique_waypoint_id( + "ABCDE", self.graph, self.name_to_id, self.logger + ) + == "Node1" + ) + # Test annotation name that is unknown + assert ( + graph_nav_util._find_unique_waypoint_id( + "ABCDEF", self.graph, self.name_to_id, self.logger + ) + == "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") + assert ( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ) + == "AB-CD-EF" + ) + # Test short code that is not in graph + assert ( + graph_nav_util._find_unique_waypoint_id( + "AD", self.graph, self.name_to_id, self.logger + ) + == "AD" + ) + # Test multiple waypoints with the same short code + self.graph.waypoints.add(id="AB-CD-EF-1") + assert ( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ) + == "AC" + ) + + +class TestGraphNavUtilUpdateWaypointsEdges: + def test_empty_graph(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + + # 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 + ) + 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() + 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 + ) + 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() + 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 + ) + 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() + 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 + ) + 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" diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9d6a34c7..e4668932 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1,56 +1,40 @@ 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 manipulation_api_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 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.api import world_object_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.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.image import ( - ImageClient, - build_image_request, - UnsupportedPixelFormatRequestedError, -) +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.point_cloud import build_pc_request -from bosdyn.client.power import safe_power_off, PowerClient, power_on +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.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 ( @@ -58,130 +42,28 @@ ) from .spot_dance import SpotDance - HAVE_CHOREOGRAPHY = True + HAVE_CHOREOGRAPHY_MODULE = True except ModuleNotFoundError: - HAVE_CHOREOGRAPHY = False + HAVE_CHOREOGRAPHY_MODULE = False 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"] -) -ImageWithHandBundle = namedtuple( - "ImageBundle", ["frontleft", "frontright", "left", "right", "back", "hand"] -) +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 + +SPOT_CLIENT_NAME = "ros_spot" +MAX_COMMAND_DURATION = 1e5 -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 +"""Service name for getting pointcloud of VLP16 connected to Spot Core""" +VELODYNE_SERVICE_NAME = "velodyne-point-cloud" def robotToLocalTime(timestamp, robot): @@ -210,6 +92,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. @@ -285,62 +175,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 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 @@ -365,24 +199,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 ) @@ -390,10 +224,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 @@ -425,7 +259,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 self._spot_wrapper._trajectory_status_unknown = False @@ -439,7 +273,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 @@ -457,14 +291,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 @@ -514,31 +348,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 @@ -574,8 +383,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, @@ -585,8 +392,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.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, @@ -616,6 +423,8 @@ 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 @@ -624,14 +433,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 @@ -653,71 +454,15 @@ 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(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._logger.error(f"Error creating SDK object: {e}") self._valid = False return - if HAVE_CHOREOGRAPHY: + if HAVE_CHOREOGRAPHY_MODULE: self._sdk.register_service_client(ChoreographyClient) + self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) @@ -728,250 +473,226 @@ def __init__( self._valid = False return - if self._robot: - # Clients - self._logger.info("Creating clients...") - initialised = False - while not initialised: + 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 + ) 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 - ) - self._license_client = self._robot.ensure_client( - LicenseClient.default_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.") - 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 + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) - try: - self._point_cloud_client = self._robot.ensure_client( - VELODYNE_SERVICE_NAME + 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 ) - except UnregisteredServiceError: - self._point_cloud_client = None + self._is_licensed_for_choreography = True + else: self._logger.info( - "No velodyne point cloud service is available." + 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._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 - ) - 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 - ) - ) - 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, + if self._robot.has_arm(): + self._manipulation_api_client = self._robot.ensure_client( + ManipulationApiClient.default_service_name ) - ) - self._depth_registered_image_requests.append( - build_image_request( - "hand_depth_in_hand_color_frame", - pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, + 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) - # 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, - ) + # 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 + ) - # Store the most recent knowledge of the state of the robot based on rpc calls. - self._init_current_graph_nav_state() + 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, + ] - # 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, - ) + # 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 + ) - 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, + if self._robot.has_arm(): + self._spot_arm = SpotArm( + self._robot, self._logger, - 10.0, - self._callbacks.get("world_objects", None), + 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) + else: + self._spot_arm = None - self._estop_endpoint = None - self._estop_keepalive = 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_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._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) - if self._is_licensed_for_choreography: - self._spot_dance = SpotDance( - self._robot, self._choreography_client, self._logger - ) + if self._is_licensed_for_choreography: + self._spot_dance = SpotDance( + self._robot, self._choreography_client, self._logger + ) - self._robot_id = None - self._lease = None + self._async_tasks = AsyncTasks(robot_tasks) @staticmethod def authenticate(robot, username, password, logger): @@ -1012,104 +733,127 @@ def authenticate(robot, username, password, logger): return authenticated @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_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: """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 - - @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 + return self.spot_world_objects.async_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 + return self.spot_arm.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) def has_arm(self, timeout=None): return self._robot.has_arm(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 @@ -1120,7 +864,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: @@ -1130,13 +874,13 @@ def robotToLocalTime(self, timestamp): """ return robotToLocalTime(timestamp, self._robot) - def claim(self): + 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 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" @@ -1154,7 +898,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): @@ -1162,12 +906,12 @@ 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""" 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) @@ -1234,7 +978,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: @@ -1254,24 +1003,6 @@ def _robot_command(self, command_proto, end_time_secs=None, timesync_endpoint=No 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.""" @@ -1386,7 +1117,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: @@ -1394,17 +1125,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: @@ -1427,14 +1159,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: @@ -1499,7 +1231,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, @@ -1507,1009 +1241,23 @@ def robot_command(self, robot_command): timesync_endpoint=self._robot.time_sync.endpoint, ) - 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): + def get_robot_command_feedback( + self, cmd_id: int + ) -> robot_command_pb2.RobotCommandFeedbackResponse: return self._robot_command_client.robot_command_feedback(cmd_id) - 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 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_", "")) - ) - ] + 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 @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] + def execute_dance(self, data): + if self._is_licensed_for_choreography: + return self._spot_dance.execute_dance(data) 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" + return False, "Spot is not licensed for choreography" def upload_animation( self, animation_name: str, animation_file_content: str @@ -2532,225 +1280,3 @@ 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