From adfb0b174c484685220634f58c3e559065a939f4 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 18:48:13 +0000 Subject: [PATCH 01/57] refactored code into modules --- spot_wrapper/graph_nav_util.py | 130 -- spot_wrapper/spot_arm.py | 512 +++++++ spot_wrapper/spot_check.py | 260 ++++ spot_wrapper/spot_config.py | 66 + spot_wrapper/spot_docking.py | 60 + spot_wrapper/spot_eap.py | 73 + spot_wrapper/spot_graph_nav.py | 796 ++++++++++ spot_wrapper/spot_images.py | 273 ++++ spot_wrapper/spot_world_objects.py | 68 + spot_wrapper/tests/test_graph_nav_util.py | 222 +++ spot_wrapper/wrapper.py | 1695 ++++----------------- 11 files changed, 2607 insertions(+), 1548 deletions(-) delete mode 100644 spot_wrapper/graph_nav_util.py create mode 100644 spot_wrapper/spot_arm.py create mode 100644 spot_wrapper/spot_check.py create mode 100644 spot_wrapper/spot_config.py create mode 100644 spot_wrapper/spot_docking.py create mode 100644 spot_wrapper/spot_eap.py create mode 100644 spot_wrapper/spot_graph_nav.py create mode 100644 spot_wrapper/spot_images.py create mode 100644 spot_wrapper/spot_world_objects.py create mode 100755 spot_wrapper/tests/test_graph_nav_util.py diff --git a/spot_wrapper/graph_nav_util.py b/spot_wrapper/graph_nav_util.py deleted file mode 100644 index e35357e4..00000000 --- a/spot_wrapper/graph_nav_util.py +++ /dev/null @@ -1,130 +0,0 @@ -# Copyright (c) 2020 Boston Dynamics, Inc. All rights reserved. -# -# Downloading, reproducing, distributing or otherwise using the SDK Software -# is subject to the terms and conditions of the Boston Dynamics Software -# Development Kit License (20191101-BDSDK-SL). - -"""Graph nav utility functions""" - - -def id_to_short_code(id): - """Convert a unique id to a 2 letter short code.""" - tokens = id.split("-") - if len(tokens) > 2: - return "%c%c" % (tokens[0][0], tokens[1][0]) - return None - - -def pretty_print_waypoints( - waypoint_id, waypoint_name, short_code_to_count, localization_id, logger -): - short_code = id_to_short_code(waypoint_id) - if short_code is None or short_code_to_count[short_code] != 1: - short_code = " " # If the short code is not valid/unique, don't show it. - - logger.info( - "%s Waypoint name: %s id: %s short code: %s" - % ( - "->" if localization_id == waypoint_id else " ", - waypoint_name, - waypoint_id, - short_code, - ) - ) - - -def find_unique_waypoint_id(short_code, graph, name_to_id, logger): - """Convert either a 2 letter short code or an annotation name into the associated unique id.""" - if len(short_code) != 2: - # Not a short code, check if it is an annotation name (instead of the waypoint id). - if short_code in name_to_id: - # Short code is an waypoint's annotation name. Check if it is paired with a unique waypoint id. - if name_to_id[short_code] is not None: - # Has an associated waypoint id! - return name_to_id[short_code] - else: - logger.error( - "The waypoint name %s is used for multiple different unique waypoints. Please use" - + "the waypoint id." % (short_code) - ) - return None - # Also not an waypoint annotation name, so we will operate under the assumption that it is a - # unique waypoint id. - return short_code - - ret = short_code - for waypoint in graph.waypoints: - if short_code == id_to_short_code(waypoint.id): - if ret != short_code: - return short_code # Multiple waypoints with same short code. - ret = waypoint.id - return ret - - -def update_waypoints_and_edges(graph, localization_id, logger): - """Update and print waypoint ids and edge ids.""" - name_to_id = dict() - edges = dict() - - short_code_to_count = {} - waypoint_to_timestamp = [] - for waypoint in graph.waypoints: - # Determine the timestamp that this waypoint was created at. - timestamp = -1.0 - try: - timestamp = ( - waypoint.annotations.creation_time.seconds - + waypoint.annotations.creation_time.nanos / 1e9 - ) - except: - # Must be operating on an older graph nav map, since the creation_time is not - # available within the waypoint annotations message. - pass - waypoint_to_timestamp.append( - (waypoint.id, timestamp, waypoint.annotations.name) - ) - - # Determine how many waypoints have the same short code. - short_code = id_to_short_code(waypoint.id) - if short_code not in short_code_to_count: - short_code_to_count[short_code] = 0 - short_code_to_count[short_code] += 1 - - # Add the annotation name/id into the current dictionary. - waypoint_name = waypoint.annotations.name - if waypoint_name: - if waypoint_name in name_to_id: - # Waypoint name is used for multiple different waypoints, so set the waypoint id - # in this dictionary to None to avoid confusion between two different waypoints. - name_to_id[waypoint_name] = None - else: - # First time we have seen this waypoint annotation name. Add it into the dictionary - # with the respective waypoint unique id. - name_to_id[waypoint_name] = waypoint.id - - # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, - # fallback to sorting by annotation name. - waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) - - # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from - # when the waypoint was created. - logger.info("%d waypoints:" % len(graph.waypoints)) - for waypoint in waypoint_to_timestamp: - pretty_print_waypoints( - waypoint[0], waypoint[2], short_code_to_count, localization_id, logger - ) - - for edge in graph.edges: - if edge.id.to_waypoint in edges: - if edge.id.from_waypoint not in edges[edge.id.to_waypoint]: - edges[edge.id.to_waypoint].append(edge.id.from_waypoint) - else: - edges[edge.id.to_waypoint] = [edge.id.from_waypoint] - logger.info( - "(Edge) from waypoint id: ", - edge.id.from_waypoint, - " and to waypoint id: ", - edge.id.to_waypoint, - ) - - return name_to_id, edges diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py new file mode 100644 index 00000000..a06e3fab --- /dev/null +++ b/spot_wrapper/spot_arm.py @@ -0,0 +1,512 @@ +import time +import typing +import logging + +from bosdyn.util import seconds_to_duration +from bosdyn.client.frame_helpers import ( + ODOM_FRAME_NAME, + GRAV_ALIGNED_BODY_FRAME_NAME, + get_a_tform_b, +) +from bosdyn.client import robot_command, math_helpers +from bosdyn.client.robot import Robot +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client.robot_command import ( + RobotCommandBuilder, + RobotCommandClient, + block_until_arm_arrives, +) +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.api import robot_command_pb2 +from bosdyn.api import arm_command_pb2 +from bosdyn.api import synchronized_command_pb2 +from bosdyn.api import geometry_pb2 +from bosdyn.api import trajectory_pb2 +from bosdyn.api import manipulation_api_pb2 +from google.protobuf.duration_pb2 import Duration + +from geometry_msgs.msg import Pose +from spot_msgs.srv import HandPose, HandPoseResponse, HandPoseRequest +from spot_msgs.srv import ( + ArmForceTrajectory, + ArmForceTrajectoryResponse, + ArmForceTrajectoryRequest, +) + + +class SpotArm: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + """ + Constructor for SpotArm class. + :param robot: Robot object + :param logger: Logger object + :param robot_params: Dictionary of robot parameters + - robot_params['is_standing']: True if robot is standing, False otherwise + :param robot_clients: Dictionary of robot clients + - robot_clients['robot_command_client']: RobotCommandClient object + - robot_clients['robot_command_method']: RobotCommand method + """ + self._robot = robot + self._logger = logger + self._robot_params = robot_params + self._robot_command_client: RobotCommandClient = robot_clients[ + "robot_command_client" + ] + self._manipulation_client: ManipulationApiClient = robot_clients[ + "manipulation_client" + ] + self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] + self._robot_command: typing.Callable = robot_clients["robot_command_method"] + + def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: + if not self._robot.has_arm(): + return False, "Spot with an arm is required for this service" + + try: + self._logger.info("Spot is powering on within the timeout of 20 secs") + self._robot.power_on(timeout_sec=20) + assert self._robot.is_powered_on(), "Spot failed to power on" + self._logger.info("Spot is powered on") + except Exception as e: + return ( + False, + "Exception occured while Spot or its arm were trying to power on", + ) + + if not self._robot_params["is_standing"]: + robot_command.blocking_stand( + command_client=self._robot_command_client, timeout_sec=10 + ) + self._logger.info("Spot is standing") + else: + self._logger.info("Spot is already standing") + + return True, "Spot has an arm, is powered on, and standing" + + def arm_stow(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Stow Arm + stow = RobotCommandBuilder.arm_stow_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(stow) + self._logger.info("Command stow issued") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while trying to stow" + + return True, "Stow arm success" + + def arm_unstow(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Unstow Arm + unstow = RobotCommandBuilder.arm_ready_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(unstow) + self._logger.info("Command unstow issued") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while trying to unstow" + + return True, "Unstow arm success" + + def arm_carry(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Get Arm in carry mode + carry = RobotCommandBuilder.arm_carry_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(carry) + self._logger.info("Command carry issued") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while carry mode was issued" + + return True, "Carry mode success" + + def make_arm_trajectory_command( + self, arm_joint_trajectory: arm_command_pb2.ArmJointTrajectory + ) -> robot_command_pb2.RobotCommand: + """Helper function to create a RobotCommand from an ArmJointTrajectory. + Copy from 'spot-sdk/python/examples/arm_joint_move/arm_joint_move.py'""" + + joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request( + trajectory=arm_joint_trajectory + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_joint_move_command=joint_move_command + ) + sync_arm = synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + arm_sync_robot_cmd = robot_command_pb2.RobotCommand( + synchronized_command=sync_arm + ) + return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd) + + def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: + # All perspectives are given when looking at the robot from behind after the unstow service is called + # Joint1: 0.0 arm points to the front. positive: turn left, negative: turn right) + # RANGE: -3.14 -> 3.14 + # Joint2: 0.0 arm points to the front. positive: move down, negative move up + # RANGE: 0.4 -> -3.13 ( + # Joint3: 0.0 arm straight. moves the arm down + # RANGE: 0.0 -> 3.1415 + # Joint4: 0.0 middle position. negative: moves ccw, positive moves cw + # RANGE: -2.79253 -> 2.79253 + # # Joint5: 0.0 gripper points to the front. positive moves the gripper down + # RANGE: -1.8326 -> 1.8326 + # Joint6: 0.0 Gripper is not rolled, positive is ccw + # RANGE: -2.87 -> 2.87 + # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0] + if abs(joint_targets[0]) > 3.14: + msg = "Joint 1 has to be between -3.14 and 3.14" + self._logger.warn(msg) + return False, msg + elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13: + msg = "Joint 2 has to be between -3.13 and 0.4" + self._logger.warn(msg) + return False, msg + elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0: + msg = "Joint 3 has to be between 0.0 and 3.14" + self._logger.warn(msg) + return False, msg + elif abs(joint_targets[3]) > 2.79253: + msg = "Joint 4 has to be between -2.79253 and 2.79253" + self._logger.warn(msg) + return False, msg + elif abs(joint_targets[4]) > 1.8326: + msg = "Joint 5 has to be between -1.8326 and 1.8326" + self._logger.warn(msg) + return False, msg + elif abs(joint_targets[5]) > 2.87: + msg = "Joint 6 has to be between -2.87 and 2.87" + self._logger.warn(msg) + return False, msg + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + trajectory_point = ( + RobotCommandBuilder.create_arm_joint_trajectory_point( + joint_targets[0], + joint_targets[1], + joint_targets[2], + joint_targets[3], + joint_targets[4], + joint_targets[5], + ) + ) + arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory( + points=[trajectory_point] + ) + arm_command = self.make_arm_trajectory_command(arm_joint_trajectory) + + # Send the request + cmd_id = self._robot_command_client.robot_command(arm_command) + + # Query for feedback to determine how long it will take + feedback_resp = self._robot_command_client.robot_command_feedback( + cmd_id + ) + joint_move_feedback = ( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback + ) + time_to_goal: Duration = joint_move_feedback.time_to_goal + time_to_goal_in_seconds: float = time_to_goal.seconds + ( + float(time_to_goal.nanos) / float(10**9) + ) + time.sleep(time_to_goal_in_seconds) + return True, "Spot Arm moved successfully" + + except Exception as e: + return False, "Exception occured during arm movement: " + str(e) + + def force_trajectory( + self, data: ArmForceTrajectoryRequest + ) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + + def create_wrench_from_msg(forces, torques): + force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) + torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) + return geometry_pb2.Wrench(force=force, torque=torque) + + # Duration in seconds. + traj_duration = data.duration + + # first point on trajectory + wrench0 = create_wrench_from_msg(data.forces_pt0, data.torques_pt0) + t0 = seconds_to_duration(0) + traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( + wrench=wrench0, time_since_reference=t0 + ) + + # Second point on the trajectory + wrench1 = create_wrench_from_msg(data.forces_pt1, data.torques_pt1) + t1 = seconds_to_duration(traj_duration) + traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( + wrench=wrench1, time_since_reference=t1 + ) + + # Build the trajectory + trajectory = trajectory_pb2.WrenchTrajectory( + points=[traj_point0, traj_point1] + ) + + # Build the trajectory request, putting all axes into force mode + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + root_frame_name=data.frame, + wrench_trajectory_in_task=trajectory, + x_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + y_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + z_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + rx_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + ry_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + rz_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command + ) + synchronized_command = ( + synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + ) + robot_command = robot_command_pb2.RobotCommand( + synchronized_command=synchronized_command + ) + + # Send the request + self._robot_command_client.robot_command(robot_command) + self._logger.info("Force trajectory command sent") + + time.sleep(float(traj_duration) + 1.0) + + except Exception as e: + return False, "Exception occured during arm movement" + + return True, "Moved arm successfully" + + def gripper_open(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Open gripper + command = RobotCommandBuilder.claw_gripper_open_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper open sent") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while gripper was moving" + + return True, "Open gripper success" + + def gripper_close(self) -> typing.Tuple[bool, str]: + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # Close gripper + command = RobotCommandBuilder.claw_gripper_close_command() + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper close sent") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while gripper was moving" + + return True, "Closed gripper successfully" + + def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: + # takes an angle between 0 (closed) and 90 (fully opened) and opens the + # gripper at this angle + if gripper_ang > 90 or gripper_ang < 0: + return False, "Gripper angle must be between 0 and 90" + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + # The open angle command does not take degrees but the limits + # defined in the urdf, that is why we have to interpolate + closed = 0.349066 + opened = -1.396263 + angle = gripper_ang / 90.0 * (opened - closed) + closed + command = RobotCommandBuilder.claw_gripper_open_angle_command(angle) + + # Command issue with RobotCommandClient + self._robot_command_client.robot_command(command) + self._logger.info("Command gripper open angle sent") + time.sleep(2.0) + + except Exception as e: + return False, "Exception occured while gripper was moving" + + return True, "Opened gripper successfully" + + def hand_pose(self, data: HandPoseRequest): + try: + success, msg = self.ensure_arm_power_and_stand() + if not success: + self._logger.info(msg) + return False, msg + else: + pose_point = data.pose_point + # Move the arm to a spot in front of the robot given a pose for the gripper. + # Build a position to move the arm to (in meters, relative to the body frame origin.) + position = geometry_pb2.Vec3( + x=pose_point.position.x, + y=pose_point.position.y, + z=pose_point.position.z, + ) + + # # Rotation as a quaternion. + rotation = geometry_pb2.Quaternion( + w=pose_point.orientation.w, + x=pose_point.orientation.x, + y=pose_point.orientation.y, + z=pose_point.orientation.z, + ) + + seconds = data.duration + duration = seconds_to_duration(seconds) + + # Build the SE(3) pose of the desired hand position in the moving body frame. + hand_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) + hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose, time_since_reference=duration + ) + hand_trajectory = trajectory_pb2.SE3Trajectory( + points=[hand_pose_traj_point] + ) + + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + root_frame_name=data.frame, + pose_trajectory_in_task=hand_trajectory, + force_remain_near_current_joint_configuration=True, + ) + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command + ) + synchronized_command = ( + synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command + ) + ) + + robot_command = robot_command_pb2.RobotCommand( + synchronized_command=synchronized_command + ) + + command = RobotCommandBuilder.build_synchro_command(robot_command) + + # Send the request + self._robot_command_client.robot_command(robot_command) + self._logger.info("Moving arm to position.") + + time.sleep(2.0) + + except Exception as e: + return ( + False, + "An error occured while trying to move arm \n Exception:" + str(e), + ) + + return True, "Moved arm successfully" + + def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): + try: + frm = str(frame) + pos = geometry_pb2.Vec3( + x=object_rt_frame[0], y=object_rt_frame[1], z=object_rt_frame[2] + ) + + grasp = manipulation_api_pb2.PickObject(frame_name=frm, object_rt_frame=pos) + + # Ask the robot to pick up the object + grasp_request = manipulation_api_pb2.ManipulationApiRequest( + pick_object=grasp + ) + # Send the request + cmd_response = self._manipulation_client.manipulation_api_command( + manipulation_api_request=grasp_request + ) + + # Get feedback from the robot + while True: + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_response.manipulation_cmd_id + ) + + # Send the request + response = self._manipulation_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) + + print( + "Current state: ", + manipulation_api_pb2.ManipulationFeedbackState.Name( + response.current_state + ), + ) + + if ( + response.current_state + == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED + or response.current_state + == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED + ): + break + + time.sleep(0.25) + + self._robot.logger.info("Finished grasp.") + + except Exception as e: + return False, "An error occured while trying to grasp from pose" + + return True, "Grasped successfully" diff --git a/spot_wrapper/spot_check.py b/spot_wrapper/spot_check.py new file mode 100644 index 00000000..992d3aa3 --- /dev/null +++ b/spot_wrapper/spot_check.py @@ -0,0 +1,260 @@ +import typing +import logging +import time + +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command +from bosdyn.client.spot_check import SpotCheckClient, run_spot_check +from bosdyn.client.spot_check import spot_check_pb2 +from bosdyn.api import header_pb2 +from google.protobuf.timestamp_pb2 import Timestamp +from bosdyn.client.lease import LeaseClient, LeaseWallet, Lease + + +class SpotCheck: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._spot_check_client: SpotCheckClient = robot_clients["spot_check_client"] + self._robot_command_client: robot_command.RobotCommandClient = robot_clients[ + "robot_command_client" + ] + self._lease_client: LeaseClient = robot_clients["lease_client"] + self._robot_params = robot_params + self._spot_check_resp = None + self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet + + @property + def spot_check_resp(self) -> spot_check_pb2.SpotCheckFeedbackResponse: + return self._spot_check_resp + + def _get_lease(self) -> Lease: + self._lease = self._lease_wallet.get_lease() + return self._lease + + def _feedback_error_check( + self, resp: spot_check_pb2.SpotCheckFeedbackResponse + ) -> typing.Tuple[bool, str]: + """Check for errors in the feedback response""" + + # Save results from Spot Check + self._spot_check_resp = resp + + # Check for common errors + if resp.header.error.code in (2, 3): + return False, str(resp.header.error.message) + + # Check for other errors + if ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_UNEXPECTED_POWER_CHANGE + ): + return False, "Unexpected power change" + elif ( + resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_IMU_CHECK + ): + return False, "Robot body is not flat on the ground" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_NOT_SITTING + ): + return False, "Robot body is not close to a sitting pose" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_LOADCELL_TIMEOUT + ): + return False, "Timeout during loadcell calibration" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_ON_FAILURE + ): + return False, "Error enabling motor power" + elif ( + resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ENDSTOP_TIMEOUT + ): + return False, "Timeout during endstop calibration" + elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FAILED_STAND: + return False, "Robot failed to stand" + elif ( + resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_CAMERA_TIMEOUT + ): + return False, "Timeout during camera check" + elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GROUND_CHECK: + return False, "Flat ground check failed" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_OFF_FAILURE + ): + return False, "Robot failed to power off" + elif ( + resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_REVERT_FAILURE + ): + return False, "Robot failed to revert calibration" + elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FGKC_FAILURE: + return False, "Robot failed to do flat ground kinematic calibration" + elif ( + resp.error + == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GRIPPER_CAL_TIMEOUT + ): + return False, "Timeout during gripper calibration" + + return True, "Successfully ran Spot Check" + + def _req_feedback(self) -> spot_check_pb2.SpotCheckFeedbackResponse: + start_time_seconds, start_time_ns = int(time.time()), int(time.time_ns() % 1e9) + req = spot_check_pb2.SpotCheckFeedbackRequest( + header=header_pb2.RequestHeader( + request_timestamp=Timestamp( + seconds=start_time_seconds, nanos=start_time_ns + ), + client_name="spot-check", + disable_rpc_logging=False, + ) + ) + resp: spot_check_pb2.SpotCheckFeedbackResponse = ( + self._spot_check_client.spot_check_feedback(req) + ) + + self._spot_check_resp = resp + + return resp + + def _spot_check_cmd(self, command: spot_check_pb2.SpotCheckCommandRequest): + """Send a Spot Check command""" + start_time_seconds, start_time_ns = int(time.time()), int(time.time_ns() % 1e9) + req = spot_check_pb2.SpotCheckCommandRequest( + header=header_pb2.RequestHeader( + request_timestamp=Timestamp( + seconds=start_time_seconds, nanos=start_time_ns + ), + client_name="spot-check", + disable_rpc_logging=False, + ), + lease=self._get_lease().lease_proto, + command=command, + ) + self._spot_check_client.spot_check_command(req) + + def stop_check(self) -> typing.Tuple[bool, str]: + """Stop the Spot Check + Note: This may cause the robot to enter a FaultState. Use only in emergencies. + """ + self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_ABORT) + + # Get feedback + resp = self._req_feedback() + + # Check for errors + success, status = self._feedback_error_check(resp) + + if success: + status = "Successfully stopped Spot Check" + self._logger.info(status) + else: + self._logger.error("Failed to stop Spot Check") + + return success, status + + def revert_calibration(self) -> typing.Tuple[bool, str]: + """Revert calibration for Spot Check""" + self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_REVERT_CAL) + + # Get feedback + resp = self._req_feedback() + + # Check for errors + success, status = self._feedback_error_check(resp) + + if success: + status = "Successfully reverted calibration" + self._logger.info(status) + else: + self._logger.error("Failed to revert calibration") + + return success, status + + def start_check(self) -> typing.Tuple[bool, str]: + """Start the Spot Check""" + # Make sure we're powered on and sitting + try: + self._robot.power_on() + if not self._robot_params["is_sitting"]: + robot_command.blocking_sit( + command_client=self._robot_command_client, timeout_sec=10 + ) + self._logger.info("Spot is sitting") + else: + self._logger.info("Spot is already sitting") + + self._spot_check_cmd(spot_check_pb2.SpotCheckCommandRequest.COMMAND_START) + + # Get feedback + resp = self._req_feedback() + + # Check for errors + success, status = self._feedback_error_check(resp) + + if success: + status = "Successfully started Spot Check" + self._logger.info(status) + else: + self._logger.error("Failed to start Spot Check") + return success, status + + except Exception as e: + return False, str(e) + + def blocking_check( + self, + timeout_sec: int = 360, + update_freq: float = 0.25, + verbose: bool = False, + ) -> typing.Tuple[bool, str]: + """Check the robot + Args: + timeout_sec: Timeout for the blocking check + update_freq: Frequency to update the check + verbose: Whether to print the check status + Returns: + Tuple of (success, message) + """ + try: + # Make sure we're powered on and sitting + self._robot.power_on() + if not self._robot_params["is_sitting"]: + robot_command.blocking_sit( + command_client=self._robot_command_client, timeout_sec=10 + ) + self._logger.info("Spot is sitting") + else: + self._logger.info("Spot is already sitting") + + # Check the robot and block for timeout_sec + self._logger.info("Blocking Spot Check is starting!") + resp: spot_check_pb2.SpotCheckFeedbackResponse = run_spot_check( + self._spot_check_client, + self._get_lease(), + timeout_sec, + update_freq, + verbose, + ) + + self._logger.info("Blocking Spot Check ran successfully!") + success, status = self._feedback_error_check(resp) + + return success, status + + except Exception as e: + self._logger.error("Exception thrown: {}".format(e)) + return False, str(e) + + def get_feedback(self) -> spot_check_pb2.SpotCheckFeedbackResponse: + """Get feedback from Spot Check""" + resp = self._req_feedback() + return resp[0], "Got only feedback from Spot Check" diff --git a/spot_wrapper/spot_config.py b/spot_wrapper/spot_config.py new file mode 100644 index 00000000..0aa325b4 --- /dev/null +++ b/spot_wrapper/spot_config.py @@ -0,0 +1,66 @@ +""" +Global variables used for configuration. These will not change at runtime. +""" +from collections import namedtuple + +SPOT_CLIENT_NAME = "ros_spot" +MAX_COMMAND_DURATION = 1e5 + +"""List of image sources for front image periodic query""" +front_image_sources = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "frontleft_depth", + "frontright_depth", +] + +"""List of image sources for side image periodic query""" +side_image_sources = [ + "left_fisheye_image", + "right_fisheye_image", + "left_depth", + "right_depth", +] + +"""List of image sources for rear image periodic query""" +rear_image_sources = ["back_fisheye_image", "back_depth"] + +"""Service name for getting pointcloud of VLP16 connected to Spot Core""" +VELODYNE_SERVICE_NAME = "velodyne-point-cloud" + +"""List of point cloud sources""" +point_cloud_sources = ["velodyne-point-cloud"] + +"""List of image sources for hand image periodic query""" +hand_image_sources = [ + "hand_image", + "hand_depth", + "hand_color_image", + "hand_depth_in_hand_color_frame", +] + +# TODO: Missing Hand images +CAMERA_IMAGE_SOURCES = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "left_fisheye_image", + "right_fisheye_image", + "back_fisheye_image", +] +DEPTH_IMAGE_SOURCES = [ + "frontleft_depth", + "frontright_depth", + "left_depth", + "right_depth", + "back_depth", +] +DEPTH_REGISTERED_IMAGE_SOURCES = [ + "frontleft_depth_in_visual_frame", + "frontright_depth_in_visual_frame", + "right_depth_in_visual_frame", + "left_depth_in_visual_frame", + "back_depth_in_visual_frame", +] +ImageBundle = namedtuple( + "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] +) diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py new file mode 100644 index 00000000..d1cd14d3 --- /dev/null +++ b/spot_wrapper/spot_docking.py @@ -0,0 +1,60 @@ +import typing +import logging + +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command +from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock +from bosdyn.api.docking import docking_pb2 + + +class SpotDocking: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._docking_client: DockingClient = robot_clients["docking_client"] + self._robot_command_client: robot_command.RobotCommandClient = robot_clients[ + "robot_command_client" + ] + self._robot_params = robot_params + + def dock(self, dock_id: int) -> typing.Tuple[bool, str]: + """Dock the robot to the docking station with fiducial ID [dock_id].""" + try: + # Make sure we're powered on and standing + self._robot.power_on() + if not self._robot_params["is_standing"]: + robot_command.blocking_stand( + command_client=self._robot_command_client, timeout_sec=10 + ) + self._logger.info("Spot is standing") + else: + self._logger.info("Spot is already standing") + # Dock the robot + self.last_docking_command = dock_id + blocking_dock_robot(self._robot, dock_id) + self.last_docking_command = None + return True, "Success" + except Exception as e: + return False, str(e) + + def undock(self, timeout: int = 20) -> typing.Tuple[bool, str]: + """Power motors on and undock the robot from the station.""" + try: + # Maker sure we're powered on + self._robot.power_on() + # Undock the robot + blocking_undock(self._robot, timeout) + return True, "Success" + except Exception as e: + return False, str(e) + + def get_docking_state(self, **kwargs) -> docking_pb2.DockState: + """Get docking state of robot.""" + state = self._docking_client.get_docking_state(**kwargs) + return state diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py new file mode 100644 index 00000000..b3347a75 --- /dev/null +++ b/spot_wrapper/spot_eap.py @@ -0,0 +1,73 @@ +import typing +import logging + +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.point_cloud import PointCloudClient, build_pc_request +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command + + +from .spot_config import * + + +class AsyncPointCloudService(AsyncPeriodicQuery): + """ + Class to get point cloud at regular intervals. get_point_cloud_from_sources_async query sent to the robot at + every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback, point_cloud_requests): + super(AsyncPointCloudService, self).__init__( + "robot_point_cloud_service", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + self._point_cloud_requests = point_cloud_requests + + def _start_query(self): + if self._callback and self._point_cloud_requests: + callback_future = self._client.get_point_cloud_async( + self._point_cloud_requests + ) + callback_future.add_done_callback(self._callback) + return callback_future + + +class SpotEAP: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._robot_params = robot_params + self._rates: typing.Dict[str, float] = robot_params["rates"] + self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + self._point_cloud_client: PointCloudClient = robot_clients["point_cloud_client"] + + self._point_cloud_requests = [] + for source in point_cloud_sources: + self._point_cloud_requests.append(build_pc_request(source)) + + self._point_cloud_task = AsyncPointCloudService( + self._point_cloud_client, + self._logger, + max(0.0, self._rates.get("point_cloud", 0.0)), + self._callbacks.get("lidar_points", None), + self._point_cloud_requests, + ) + + @property + def async_task(self) -> AsyncPeriodicQuery: + """Returns the async PointCloudService task for the robot""" + return self._point_cloud_task diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py new file mode 100644 index 00000000..305dfa33 --- /dev/null +++ b/spot_wrapper/spot_graph_nav.py @@ -0,0 +1,796 @@ +import typing +import logging +import math +import time +import os + +from bosdyn.client.robot import Robot +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client.lease import LeaseClient, LeaseWallet, LeaseKeepAlive +from bosdyn.client.graph_nav import GraphNavClient +from bosdyn.client.map_processing import MapProcessingServiceClient +from bosdyn.client.frame_helpers import get_odom_tform_body +from bosdyn.api.graph_nav import graph_nav_pb2 +from bosdyn.api.graph_nav import map_pb2 +from bosdyn.api.graph_nav import nav_pb2 +from bosdyn.api.graph_nav import map_processing_pb2 + +from google.protobuf import wrappers_pb2 + + +class SpotGraphNav: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._graph_nav_client: GraphNavClient = robot_clients["graph_nav_client"] + self._map_processing_client: MapProcessingServiceClient = robot_clients[ + "map_processing_client" + ] + self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] + self._lease_client: LeaseClient = robot_clients["lease_client"] + self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet + self._robot_params = robot_params + + # Store the most recent knowledge of the state of the robot based on rpc calls. + self._current_graph = None + self._current_edges = dict() # maps to_waypoint to list(from_waypoint) + self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot + self._current_edge_snapshots = dict() # maps id to edge snapshot + self._current_annotation_name_to_wp_id = dict() + + def list_graph(self) -> typing.List[str]: + """List waypoint ids of graph_nav + Args: + upload_path : Path to the root directory of the map. + """ + ids, eds = self._list_graph_waypoint_and_edge_ids() + + return [ + v + for k, v in sorted( + ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) + ) + ] + + def navigate_initial_localization( + self, + upload_path: str, + initial_localization_fiducial: bool = True, + initial_localization_waypoint: typing.Optional[str] = None, + ): + """Navigate with graphnav. + + Args: + upload_path : Path to the root directory of the map. + navigate_to : Waypont id string for where to goal + initial_localization_fiducial : Tells the initializer whether to use fiducials + initial_localization_waypoint : Waypoint id string of current robot position (optional) + """ + # Filepath for uploading a saved graph's and snapshots too. + if upload_path and upload_path[-1] == "/": + upload_filepath = upload_path[:-1] + else: + upload_filepath = upload_path + + # Boolean indicating the robot's power state. + power_state = self._robot_state_client.get_robot_state().power_state + self._started_powered_on = power_state.motor_power_state == power_state.STATE_ON + self._powered_on = self._started_powered_on + + # Claim lease, power on robot, start graphnav. + self._lease = self._lease_wallet.get_lease() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + if upload_filepath: + self._clear_graph() + self._upload_graph_and_snapshots(upload_filepath) + else: + self._download_current_graph() + self._logger.info( + "Re-using existing graph on robot. Check that the correct graph is loaded!" + ) + if initial_localization_fiducial: + self._set_initial_localization_fiducial() + if initial_localization_waypoint: + self._set_initial_localization_waypoint([initial_localization_waypoint]) + self._list_graph_waypoint_and_edge_ids() + self._get_localization_state() + + return True, "Localization done!" + + def navigate_to_existing_waypoint(self, waypoint_id: str): + """Navigate to an existing waypoint. + Args: + waypoint_id : Waypoint id string for where to go + """ + self._get_localization_state() + resp = self._navigate_to(waypoint_id) + return resp + + def navigate_through_route(self, waypoint_ids: typing.List[str]): + """ + Args: + waypoint_ids: List[str] of waypoints to follow + """ + self._get_localization_state() + self._logger.info(f"Waypoint ids: {','.join(waypoint_ids)}") + resp = self._navigate_route(waypoint_ids) + return resp + + def download_navigation_graph(self, download_path: str) -> typing.List[str]: + """Download the navigation graph. + Args: + download_path : Path to the root directory of the map. + """ + self._download_filepath = download_path + self._download_full_graph() + return self.list_graph() + + def navigation_close_loops( + self, close_fiducial_loops: bool, close_odometry_loops: bool + ) -> typing.Tuple[bool, str]: + return self._auto_close_loops(close_fiducial_loops, close_odometry_loops) + + def optmize_anchoring(self) -> typing.Tuple[bool, str]: + return self._optimize_anchoring() + + ## Copied from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py + # Copyright (c) 2020 Boston Dynamics, Inc. All rights reserved. + # + # Downloading, reproducing, distributing or otherwise using the SDK Software + # is subject to the terms and conditions of the Boston Dynamics Software + # Development Kit License (20191101-BDSDK-SL). + def _get_localization_state(self, *args): + """Get the current localization and state of the robot.""" + state = self._graph_nav_client.get_localization_state() + self._logger.info(f"Got localization: \n{str(state.localization)}") + odom_tform_body = get_odom_tform_body( + state.robot_kinematics.transforms_snapshot + ) + self._logger.info( + f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}" + ) + + def _set_initial_localization_fiducial(self, *args): + """Trigger localization when near a fiducial.""" + robot_state = self._robot_state_client.get_robot_state() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() + # Create an empty instance for initial localization since we are asking it to localize + # based on the nearest fiducial. + localization = nav_pb2.Localization() + self._graph_nav_client.set_localization( + initial_guess_localization=localization, + ko_tform_body=current_odom_tform_body, + ) + + def _set_initial_localization_waypoint(self, *args): + """Trigger localization to a waypoint.""" + # Take the first argument as the localization waypoint. + if len(args) < 1: + # If no waypoint id is given as input, then return without initializing. + self._logger.error("No waypoint specified to initialize to.") + return + destination_waypoint = self._find_unique_waypoint_id( + args[0][0], + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not destination_waypoint: + self._logger.error("Failed to find waypoint id.") + return + + robot_state = self._robot_state_client.get_robot_state() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() + # Create an initial localization to the specified waypoint as the identity. + localization = nav_pb2.Localization() + localization.waypoint_id = destination_waypoint + localization.waypoint_tform_body.rotation.w = 1.0 + self._graph_nav_client.set_localization( + initial_guess_localization=localization, + # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). + max_distance=0.2, + max_yaw=20.0 * math.pi / 180.0, + fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NO_FIDUCIAL, + ko_tform_body=current_odom_tform_body, + ) + + def _download_current_graph(self): + graph = self._graph_nav_client.download_graph() + if graph is None: + self._logger.error("Empty graph.") + return + self._current_graph = graph + return graph + + def _download_full_graph(self, *args): + """Download the graph and snapshots from the robot.""" + graph = self._graph_nav_client.download_graph() + if graph is None: + self._logger.info("Failed to download the graph.") + return + self._write_full_graph(graph) + self._logger.info( + "Graph downloaded with {} waypoints and {} edges".format( + len(graph.waypoints), len(graph.edges) + ) + ) + # Download the waypoint and edge snapshots. + self._download_and_write_waypoint_snapshots(graph.waypoints) + self._download_and_write_edge_snapshots(graph.edges) + + def _write_full_graph(self, graph): + """Download the graph from robot to the specified, local filepath location.""" + graph_bytes = graph.SerializeToString() + self._write_bytes(self._download_filepath, "/graph", graph_bytes) + + def _download_and_write_waypoint_snapshots(self, waypoints): + """Download the waypoint snapshots from robot to the specified, local filepath location.""" + num_waypoint_snapshots_downloaded = 0 + for waypoint in waypoints: + if len(waypoint.snapshot_id) == 0: + continue + try: + waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot( + waypoint.snapshot_id + ) + except Exception: + # Failure in downloading waypoint snapshot. Continue to next snapshot. + self._logger.error( + "Failed to download waypoint snapshot: " + waypoint.snapshot_id + ) + continue + self._write_bytes( + self._download_filepath + "/waypoint_snapshots", + "/" + waypoint.snapshot_id, + waypoint_snapshot.SerializeToString(), + ) + num_waypoint_snapshots_downloaded += 1 + self._logger.info( + "Downloaded {} of the total {} waypoint snapshots.".format( + num_waypoint_snapshots_downloaded, len(waypoints) + ) + ) + + def _download_and_write_edge_snapshots(self, edges): + """Download the edge snapshots from robot to the specified, local filepath location.""" + num_edge_snapshots_downloaded = 0 + num_to_download = 0 + for edge in edges: + if len(edge.snapshot_id) == 0: + continue + num_to_download += 1 + try: + edge_snapshot = self._graph_nav_client.download_edge_snapshot( + edge.snapshot_id + ) + except Exception: + # Failure in downloading edge snapshot. Continue to next snapshot. + self._logger.error( + "Failed to download edge snapshot: " + edge.snapshot_id + ) + continue + self._write_bytes( + self._download_filepath + "/edge_snapshots", + "/" + edge.snapshot_id, + edge_snapshot.SerializeToString(), + ) + num_edge_snapshots_downloaded += 1 + self._logger.info( + "Downloaded {} of the total {} edge snapshots.".format( + num_edge_snapshots_downloaded, num_to_download + ) + ) + + def _write_bytes(self, filepath: str, filename: str, data): + """Write data to a file.""" + os.makedirs(filepath, exist_ok=True) + with open(filepath + filename, "wb+") as f: + f.write(data) + f.close() + + def _list_graph_waypoint_and_edge_ids(self, *args): + """List the waypoint ids and edge ids of the graph currently on the robot.""" + + # Download current graph + graph = self._download_current_graph() + + localization_id = ( + self._graph_nav_client.get_localization_state().localization.waypoint_id + ) + + # Update and print waypoints and edges + ( + self._current_annotation_name_to_wp_id, + self._current_edges, + ) = self._update_waypoints_and_edges(graph, localization_id, self._logger) + return self._current_annotation_name_to_wp_id, self._current_edges + + def _upload_graph_and_snapshots(self, upload_filepath: str): + """Upload the graph and snapshots to the robot.""" + self._logger.info("Loading the graph from disk into local storage...") + with open(upload_filepath + "/graph", "rb") as graph_file: + # Load the graph from disk. + data = graph_file.read() + self._current_graph = map_pb2.Graph() + self._current_graph.ParseFromString(data) + self._logger.info( + "Loaded graph has {} waypoints and {} edges".format( + len(self._current_graph.waypoints), len(self._current_graph.edges) + ) + ) + for waypoint in self._current_graph.waypoints: + # Load the waypoint snapshots from disk. + with open( + upload_filepath + "/waypoint_snapshots/{}".format(waypoint.snapshot_id), + "rb", + ) as snapshot_file: + waypoint_snapshot = map_pb2.WaypointSnapshot() + waypoint_snapshot.ParseFromString(snapshot_file.read()) + self._current_waypoint_snapshots[ + waypoint_snapshot.id + ] = waypoint_snapshot + for edge in self._current_graph.edges: + # Load the edge snapshots from disk. + self._logger.info(f"Trying edge: {edge.snapshot_id}") + if not edge.snapshot_id: + continue + with open( + upload_filepath + "/edge_snapshots/{}".format(edge.snapshot_id), "rb" + ) as snapshot_file: + edge_snapshot = map_pb2.EdgeSnapshot() + edge_snapshot.ParseFromString(snapshot_file.read()) + self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot + # Upload the graph to the robot. + self._logger.info("Uploading the graph and snapshots to the robot...") + self._graph_nav_client.upload_graph( + lease=self._lease.lease_proto, graph=self._current_graph + ) + # Upload the snapshots to the robot. + for waypoint_snapshot in self._current_waypoint_snapshots.values(): + self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) + self._logger.info("Uploaded {}".format(waypoint_snapshot.id)) + for edge_snapshot in self._current_edge_snapshots.values(): + self._graph_nav_client.upload_edge_snapshot(edge_snapshot) + self._logger.info("Uploaded {}".format(edge_snapshot.id)) + + # The upload is complete! Check that the robot is localized to the graph, + # and it if is not, prompt the user to localize the robot before attempting + # any navigation commands. + localization_state = self._graph_nav_client.get_localization_state() + if not localization_state.localization.waypoint_id: + # The robot is not localized to the newly uploaded graph. + self._logger.info( + "Upload complete! The robot is currently not localized to the map; please localize the robot using a fiducial before attempting a navigation command." + ) + + def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: + """Navigate to a specific waypoint.""" + self._lease = self._lease_wallet.get_lease() + destination_waypoint = self._find_unique_waypoint_id( + waypoint_id, + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not destination_waypoint: + self._logger.error( + "Failed to find the appropriate unique waypoint id for the navigation command." + ) + return ( + False, + "Failed to find the appropriate unique waypoint id for the navigation command.", + ) + + # Stop the lease keepalive and create a new sublease for graphnav. + self._lease = self._lease_wallet.advance() + sublease = self._lease.create_sublease() + self._lease_keepalive.shutdown() + + # Navigate to the destination waypoint. + is_finished = False + nav_to_cmd_id = -1 + while not is_finished: + # Issue the navigation command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + nav_to_cmd_id = self._graph_nav_client.navigate_to( + destination_waypoint, 1.0, leases=[sublease.lease_proto] + ) + time.sleep(0.5) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the navigation command is complete. + is_finished = self._check_success(nav_to_cmd_id) + + self._lease = self._lease_wallet.advance() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + + status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): + return True, "Successfully completed the navigation commands!" + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: + return ( + False, + "Robot got lost when navigating the route, the robot will now sit down.", + ) + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: + return ( + False, + "Robot got stuck when navigating the route, the robot will now sit down.", + ) + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): + return False, "Robot is impaired." + else: + return False, "Navigation command is not complete yet." + + def _navigate_route( + self, waypoint_ids: typing.List[str] + ) -> typing.Tuple[bool, str]: + """Navigate through a specific route of waypoints. + Note that each waypoint must have an edge between them, aka be adjacent. + """ + for i in range(len(waypoint_ids)): + waypoint_ids[i] = self._find_unique_waypoint_id( + waypoint_ids[i], + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + if not waypoint_ids[i]: + self._logger.error( + "navigate_route: Failed to find the unique waypoint id." + ) + return False, "Failed to find the unique waypoint id." + + edge_ids_list = [] + # Attempt to find edges in the current graph that match the ordered waypoint pairs. + # These are necessary to create a valid route. + for i in range(len(waypoint_ids) - 1): + start_wp = waypoint_ids[i] + end_wp = waypoint_ids[i + 1] + edge_id = self._match_edge(self._current_edges, start_wp, end_wp) + if edge_id is not None: + edge_ids_list.append(edge_id) + else: + self._logger.error( + f"Failed to find an edge between waypoints: {start_wp} and {end_wp}" + ) + return ( + False, + f"Failed to find an edge between waypoints: {start_wp} and {end_wp}", + ) + + # Stop the lease keepalive and create a new sublease for graphnav. + self._lease = self._lease_wallet.advance() + sublease = self._lease.create_sublease() + self._lease_keepalive.shutdown() + + # Navigate a specific route. + route = self._graph_nav_client.build_route(waypoint_ids, edge_ids_list) + is_finished = False + while not is_finished: + # Issue the route command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + nav_route_command_id = self._graph_nav_client.navigate_route( + route, cmd_duration=1.0, leases=[sublease.lease_proto] + ) + time.sleep(0.5) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the route is complete. + is_finished = self._check_success(nav_route_command_id) + + self._lease = self._lease_wallet.advance() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + + return True, "Finished navigating route!" + + def _clear_graph(self, *args) -> bool: + """Clear the state of the map on the robot, removing all waypoints and edges.""" + return self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) + + def _check_success(self, command_id: int = -1) -> bool: + """Use a navigation command id to get feedback from the robot and sit when command succeeds.""" + if command_id == -1: + # No command, so we have not status to check. + return False + status = self._graph_nav_client.navigation_feedback(command_id) + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): + # Successfully completed the navigation commands! + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: + self._logger.error( + "Robot got lost when navigating the route, the robot will now sit down." + ) + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: + self._logger.error( + "Robot got stuck when navigating the route, the robot will now sit down." + ) + return True + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): + self._logger.error("Robot is impaired.") + return True + else: + # Navigation command is not complete yet. + return False + + def _match_edge( + self, + current_edges: typing.Dict[str, typing.List[str]], + waypoint1: str, + waypoint2: str, + ) -> typing.Optional[map_pb2.Edge.Id]: + """Find an edge in the graph that is between two waypoint ids.""" + # Return the correct edge id as soon as it's found. + for edge_to_id in current_edges: + for edge_from_id in current_edges[edge_to_id]: + if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): + # This edge matches the pair of waypoints! Add it the edge list and continue. + return map_pb2.Edge.Id( + from_waypoint=waypoint2, to_waypoint=waypoint1 + ) + elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): + # This edge matches the pair of waypoints! Add it the edge list and continue. + return map_pb2.Edge.Id( + from_waypoint=waypoint1, to_waypoint=waypoint2 + ) + return None + + def _auto_close_loops( + self, close_fiducial_loops: bool, close_odometry_loops: bool, *args + ): + """Automatically find and close all loops in the graph.""" + response: map_processing_pb2.ProcessTopologyResponse = ( + self._map_processing_client.process_topology( + params=map_processing_pb2.ProcessTopologyRequest.Params( + do_fiducial_loop_closure=wrappers_pb2.BoolValue( + value=close_fiducial_loops + ), + do_odometry_loop_closure=wrappers_pb2.BoolValue( + value=close_odometry_loops + ), + ), + modify_map_on_server=True, + ) + ) + self._logger.info( + "Created {} new edge(s).".format(len(response.new_subgraph.edges)) + ) + if response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_OK: + return True, "Successfully closed loops." + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS + ): + return False, "Missing waypoint snapshots." + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH + ): + return False, "Invalid graph." + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING + ): + return False, "Map modified during processing." + else: + return False, "Unknown error during map processing." + + def _optimize_anchoring(self, *args): + """Call anchoring optimization on the server, producing a globally optimal reference frame for waypoints to be expressed in.""" + response: map_processing_pb2.ProcessAnchoringResponse = ( + self._map_processing_client.process_anchoring( + params=map_processing_pb2.ProcessAnchoringRequest.Params(), + modify_anchoring_on_server=True, + stream_intermediate_results=False, + ) + ) + if response.status == map_processing_pb2.ProcessAnchoringResponse.STATUS_OK: + self._logger.info( + "Optimized anchoring after {} iteration(s).".format(response.iteration) + ) + return True, "Successfully optimized anchoring." + else: + self._logger.error("Error optimizing {}".format(response)) + if ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS + ): + return False, "Missing waypoint snapshots." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAPH + ): + return False, "Invalid graph." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_OPTIMIZATION_FAILURE + ): + return False, "Optimization failure." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_PARAMS + ): + return False, "Invalid parameters." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_CONSTRAINT_VIOLATION + ): + return False, "Constraint violation." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_ITERATIONS + ): + return False, "Max iterations, timeout." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_TIME + ): + return False, "Max time reached, timeout." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_HINTS + ): + return False, "Invalid hints." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING + ): + return False, "Map modified during processing." + elif ( + response.status + == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT + ): + return False, "Invalid gravity alignment." + else: + return False, "Unknown error during anchoring optimization." + + def _id_to_short_code(self, id: str): + """Convert a unique id to a 2 letter short code.""" + tokens = id.split("-") + if len(tokens) > 2: + return "%c%c" % (tokens[0][0], tokens[1][0]) + return None + + def _pretty_print_waypoints( + self, + waypoint_id: str, + waypoint_name: str, + short_code_to_count: typing.Dict[str, int], + localization_id: str, + logger: logging.Logger, + ): + short_code = self._id_to_short_code(waypoint_id) + if short_code is None or short_code_to_count[short_code] != 1: + # If the short code is not valid/unique, don't show it. + short_code = " " + + logger.info( + "%s Waypoint name: %s id: %s short code: %s" + % ( + "->" if localization_id == waypoint_id else " ", + waypoint_name, + waypoint_id, + short_code, + ) + ) + + def _find_unique_waypoint_id( + self, + short_code: str, + graph: map_pb2.Graph, + name_to_id: typing.Dict[str, str], + logger: logging.Logger, + ): + """Convert either a 2 letter short code or an annotation name into the associated unique id.""" + if len(short_code) != 2: + # Not a short code, check if it is an annotation name (instead of the waypoint id). + if short_code in name_to_id: + # Short code is an waypoint's annotation name. Check if it is paired with a unique waypoint id. + if name_to_id[short_code] is not None: + # Has an associated waypoint id! + return name_to_id[short_code] + else: + logger.error( + "The waypoint name %s is used for multiple different unique waypoints. Please use" + + "the waypoint id." % (short_code) + ) + return None + # Also not an waypoint annotation name, so we will operate under the assumption that it is a + # unique waypoint id. + return short_code + + ret = short_code + for waypoint in graph.waypoints: + if short_code == self._id_to_short_code(waypoint.id): + if ret != short_code: + return short_code # Multiple waypoints with same short code. + ret = waypoint.id + return ret + + def _update_waypoints_and_edges( + self, graph: map_pb2.Graph, localization_id: str, logger: logging.Logger + ) -> typing.Tuple[typing.Dict[str, str], typing.Dict[str, str]]: + """Update and print waypoint ids and edge ids.""" + name_to_id = dict() + edges = dict() + + short_code_to_count = {} + waypoint_to_timestamp = [] + for waypoint in graph.waypoints: + # Determine the timestamp that this waypoint was created at. + timestamp = -1.0 + try: + timestamp = ( + waypoint.annotations.creation_time.seconds + + waypoint.annotations.creation_time.nanos / 1e9 + ) + except: + # Must be operating on an older graph nav map, since the creation_time is not + # available within the waypoint annotations message. + pass + waypoint_to_timestamp.append( + (waypoint.id, timestamp, waypoint.annotations.name) + ) + + # Determine how many waypoints have the same short code. + short_code = self._id_to_short_code(waypoint.id) + if short_code not in short_code_to_count: + short_code_to_count[short_code] = 0 + short_code_to_count[short_code] += 1 + + # Add the annotation name/id into the current dictionary. + waypoint_name = waypoint.annotations.name + if waypoint_name: + if waypoint_name in name_to_id: + # Waypoint name is used for multiple different waypoints, so set the waypoint id + # in this dictionary to None to avoid confusion between two different waypoints. + name_to_id[waypoint_name] = None + else: + # First time we have seen this waypoint annotation name. Add it into the dictionary + # with the respective waypoint unique id. + name_to_id[waypoint_name] = waypoint.id + + # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, + # fallback to sorting by annotation name. + waypoint_to_timestamp = sorted( + waypoint_to_timestamp, key=lambda x: (x[1], x[2]) + ) + + # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from + # when the waypoint was created. + logger.info("%d waypoints:" % len(graph.waypoints)) + for waypoint in waypoint_to_timestamp: + self._pretty_print_waypoints( + waypoint[0], waypoint[2], short_code_to_count, localization_id, logger + ) + + for edge in graph.edges: + if edge.id.to_waypoint in edges: + if edge.id.from_waypoint not in edges[edge.id.to_waypoint]: + edges[edge.id.to_waypoint].append(edge.id.from_waypoint) + else: + edges[edge.id.to_waypoint] = [edge.id.from_waypoint] + logger.info( + f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}" + ) + + return name_to_id, edges diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py new file mode 100644 index 00000000..d8444a6e --- /dev/null +++ b/spot_wrapper/spot_images.py @@ -0,0 +1,273 @@ +import typing +import logging + +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command +from bosdyn.client.image import ( + ImageClient, + build_image_request, + UnsupportedPixelFormatRequestedError, +) +from bosdyn.api import image_pb2 + +from .spot_config import * + + +class AsyncImageService(AsyncPeriodicQuery): + """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback, image_requests): + super(AsyncImageService, self).__init__( + "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + self._image_requests = image_requests + + def _start_query(self): + if self._callback: + callback_future = self._client.get_image_async(self._image_requests) + callback_future.add_done_callback(self._callback) + return callback_future + + +class SpotImages: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._robot_params = robot_params + self._rates: typing.Dict[str, float] = robot_params["rates"] + self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + self._image_client: ImageClient = robot_clients["image_client"] + + self._front_image_requests = [] + for source in front_image_sources: + self._front_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._side_image_requests = [] + for source in side_image_sources: + self._side_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._rear_image_requests = [] + for source in rear_image_sources: + self._rear_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._hand_image_requests = [] + for source in hand_image_sources: + self._hand_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._front_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("front_image", 0.0)), + self._callbacks.get("front_image", None), + self._front_image_requests, + ) + self._side_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("side_image", 0.0)), + self._callbacks.get("side_image", None), + self._side_image_requests, + ) + self._rear_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("rear_image", 0.0)), + self._callbacks.get("rear_image", None), + self._rear_image_requests, + ) + self._hand_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("hand_image", 0.0)), + self._callbacks.get("hand_image", None), + self._hand_image_requests, + ) + + self.camera_task_name_to_task_mapping: typing.Dict[str, AsyncImageService] = { + "hand_image": self._hand_image_task, + "side_image": self._side_image_task, + "rear_image": self._rear_image_task, + "front_image": self._front_image_task, + } + + ############################################ + # TODO: Sort out double publishing of images + self._camera_image_requests = [] + for camera_source in CAMERA_IMAGE_SOURCES: + self._camera_image_requests.append( + build_image_request( + camera_source, + image_format=image_pb2.Image.FORMAT_JPEG, + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ) + + self._depth_image_requests = [] + for camera_source in DEPTH_IMAGE_SOURCES: + self._depth_image_requests.append( + build_image_request( + camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + ) + ) + + self._depth_registered_image_requests = [] + for camera_source in DEPTH_REGISTERED_IMAGE_SOURCES: + self._depth_registered_image_requests.append( + build_image_request( + camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + ) + ) + + @property + def front_image_task(self) -> AsyncImageService: + return self._front_image_task + + @property + def side_image_task(self) -> AsyncImageService: + return self._side_image_task + + @property + def rear_image_task(self) -> AsyncImageService: + return self._rear_image_task + + @property + def hand_image_task(self) -> AsyncImageService: + return self._hand_image_task + + def update_image_tasks(self, image_name: str): + """Adds an async tasks to retrieve images from the specified image source""" + task_to_add = self.camera_task_name_to_task_mapping[image_name] + + if task_to_add == self._hand_image_task and not self._robot.has_arm(): + self._logger.warning( + "Robot has no arm, therefore the arm image task can not be added" + ) + return + + if task_to_add in self._async_tasks._tasks: + self._logger.warning( + f"Task {image_name} already in async task list, will not be added again" + ) + return + + self._async_tasks.add_task(self.camera_task_name_to_task_mapping[image_name]) + + def get_frontleft_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "frontleft_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_frontright_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "frontright_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_left_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "left_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_right_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "right_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_back_rgb_image(self) -> image_pb2.Image: + try: + return self._image_client.get_image( + [ + build_image_request( + "back_fisheye_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + + def get_images( + self, image_requests: typing.List[image_pb2.ImageRequest] + ) -> typing.Optional[ImageBundle]: + try: + image_responses = self._image_client.get_image(image_requests) + except UnsupportedPixelFormatRequestedError as e: + return None + return ImageBundle( + frontleft=image_responses[0], + frontright=image_responses[1], + left=image_responses[2], + right=image_responses[3], + back=image_responses[4], + ) + + def get_camera_images(self) -> ImageBundle: + return self.get_images(self._camera_image_requests) + + def get_depth_images(self) -> ImageBundle: + return self.get_images(self._depth_image_requests) + + def get_depth_registered_images(self) -> ImageBundle: + return self.get_images(self._depth_registered_image_requests) diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py new file mode 100644 index 00000000..067801d2 --- /dev/null +++ b/spot_wrapper/spot_world_objects.py @@ -0,0 +1,68 @@ +import typing +import logging + +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.robot import Robot +from bosdyn.client import robot_command +from bosdyn.client.world_object import WorldObjectClient + +from .spot_config import * + + +class AsyncWorldObjects(AsyncPeriodicQuery): + """Class to get world objects. list_world_objects_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback): + super(AsyncWorldObjects, self).__init__( + "world-objects", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + + def _start_query(self): + if self._callback: + callback_future = self._client.list_world_objects_async() + callback_future.add_done_callback(self._callback) + return callback_future + + +class SpotWorldObjects: + def __init__( + self, + robot: Robot, + logger: logging.Logger, + robot_params: typing.Dict[str, typing.Any], + robot_clients: typing.Dict[str, typing.Any], + ): + self._robot = robot + self._logger = logger + self._robot_params = robot_params + self._rates: typing.Dict[str, float] = robot_params["rates"] + self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + self._world_objects_client: WorldObjectClient = robot_clients[ + "world_objects_client" + ] + + self._world_objects_task = AsyncWorldObjects( + self._world_objects_client, + self._logger, + self._rates.get("world_objects", 10.0), + self._callbacks.get("world_objects", None), + ) + + @property + def async_task(self): + return self._world_objects_task + + def list_world_objects(self, object_types, time_start_point): + return self._world_objects_client.list_world_objects( + object_types, time_start_point + ) diff --git a/spot_wrapper/tests/test_graph_nav_util.py b/spot_wrapper/tests/test_graph_nav_util.py new file mode 100755 index 00000000..1dc78556 --- /dev/null +++ b/spot_wrapper/tests/test_graph_nav_util.py @@ -0,0 +1,222 @@ +#!/usr/bin/env python3 +PKG = "graph_nav_util" +NAME = "test_graph_nav_util" +SUITE = "test_graph_nav_util.TestSuiteGraphNavUtil" + +import unittest +import logging + +from bosdyn.api.graph_nav import map_pb2 + +from spot_wrapper.spot_graph_nav import SpotGraphNav + + +class MockSpotGraphNav(SpotGraphNav): + def __init__(self) -> None: + pass + + +# Create a mock SpotGraphNav object to access utility methods +graph_nav_util = MockSpotGraphNav() + + +class TestGraphNavUtilShortCode(unittest.TestCase): + def test_id_to_short_code(self): + self.assertEqual( + graph_nav_util._id_to_short_code("ebony-pug-mUzxLNq.TkGlVIxga+UKAQ=="), "ep" + ) + self.assertEqual( + graph_nav_util._id_to_short_code("erose-simian-sug9xpxhCxgft7Mtbhr98A=="), + "es", + ) + + +class TestGraphNavUtilFindUniqueWaypointId(unittest.TestCase): + def setUp(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + self.graph = map_pb2.Graph() + self.name_to_id = {"ABCDE": "Node1"} + + def test_short_code(self): + # Test normal short code + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ), + "AC", + "AC!=AC, normal short code", + ) + # Test annotation name that is known + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "ABCDE", self.graph, self.name_to_id, self.logger + ), + "Node1", + "ABCDE!=Node1, known annotation name", + ) + # Test annotation name that is unknown + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "ABCDEF", self.graph, self.name_to_id, self.logger + ), + "ABCDEF", + "ABCDEF!=ABCDEF, unknown annotation name", + ) + + def test_short_code_with_graph(self): + # Test short code that is in graph + self.graph.waypoints.add(id="AB-CD-EF") + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ), + "AB-CD-EF", + "AC!=AB-CD-EF, short code in graph", + ) + # Test short code that is not in graph + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "AD", self.graph, self.name_to_id, self.logger + ), + "AD", + "AD!=AD, short code not in graph", + ) + # Test multiple waypoints with the same short code + self.graph.waypoints.add(id="AB-CD-EF-1") + self.assertEqual( + graph_nav_util._find_unique_waypoint_id( + "AC", self.graph, self.name_to_id, self.logger + ), + "AC", + "AC!=AC, multiple waypoints with same short code", + ) + + +class TestGraphNavUtilUpdateWaypointsEdges(unittest.TestCase): + def setUp(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + + def test_empty_graph(self): + # Test empty graph + self.graph = map_pb2.Graph() + self.localization_id = "" + graph_nav_util._update_waypoints_and_edges( + self.graph, self.localization_id, self.logger + ) + self.assertEqual( + len(self.graph.waypoints), 0, "Empty graph should have 0 waypoints" + ) + self.assertEqual(len(self.graph.edges), 0, "Empty graph should have 0 edges") + + def test_one_waypoint(self): + # Test graph with 1 waypoint + self.localization_id = "" + self.graph = map_pb2.Graph() + new_waypoint = map_pb2.Waypoint() + new_waypoint.id = "ABCDE" + new_waypoint.annotations.name = "Node1" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( + self.graph, self.localization_id, self.logger + ) + self.assertEqual( + len(self.graph.waypoints), 1, "Graph with 1 waypoint should have 1 waypoint" + ) + self.assertEqual( + len(self.graph.edges), 0, "Graph with 1 waypoint should have 0 edges" + ) + self.assertEqual(len(self.edges), 0, "Edges should have 0 entries") + self.assertEqual(len(self.name_to_id), 1, "Name to id should have 1 entry") + self.assertEqual( + self.name_to_id["Node1"], "ABCDE", "Name to id should have 1 entry" + ) + + def test_two_waypoints_with_edge(self): + # Test graph with 2 waypoints and an edge between them + self.localization_id = "" + self.graph = map_pb2.Graph() + new_waypoint = map_pb2.Waypoint() + new_waypoint.id = "ABCDE" + new_waypoint.annotations.name = "Node1" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + new_waypoint.id = "DE" + new_waypoint.annotations.name = "Node2" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + new_edge = map_pb2.Edge.Id(from_waypoint="ABCDE", to_waypoint="DE") + + self.graph.edges.add(id=new_edge) + self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( + self.graph, self.localization_id, self.logger + ) + self.assertEqual( + len(self.graph.waypoints), + 2, + "Graph with 2 waypoints should have 2 waypoints", + ) + self.assertEqual( + len(self.graph.edges), 1, "Graph with 2 waypoints should have 1 edge" + ) + self.assertEqual(len(self.edges), 1, "Edges should have 1 entry") + self.assertEqual( + self.edges["DE"][0], "ABCDE", "Edges should point to the correct waypoint" + ) + self.assertEqual(len(self.name_to_id), 2, "Name to id should have 2 entries") + self.assertEqual(self.name_to_id["Node1"], "ABCDE", "Name to id entry, ABCDE") + self.assertEqual(self.name_to_id["Node2"], "DE", "Name to id entry, DE") + + def test_two_waypoints_with_edge_and_localization(self): + # Test graph with 2 waypoints and an edge between them. Mainly affects the pretty print. + self.localization_id = "ABCDE" + self.graph = map_pb2.Graph() + new_waypoint = map_pb2.Waypoint() + new_waypoint.id = "ABCDE" + new_waypoint.annotations.name = "Node1" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + new_waypoint.id = "DE" + new_waypoint.annotations.name = "Node2" + self.graph.waypoints.add( + id=new_waypoint.id, annotations=new_waypoint.annotations + ) + new_edge = map_pb2.Edge.Id(from_waypoint="ABCDE", to_waypoint="DE") + + self.graph.edges.add(id=new_edge) + self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( + self.graph, self.localization_id, self.logger + ) + self.assertEqual( + len(self.graph.waypoints), + 2, + "Graph with 2 waypoints should have 2 waypoints", + ) + self.assertEqual( + len(self.graph.edges), 1, "Graph with 2 waypoints should have 1 edge" + ) + self.assertEqual(len(self.edges), 1, "Edges should have 1 entry") + self.assertEqual( + self.edges["DE"][0], "ABCDE", "Edges should point to the correct waypoint" + ) + self.assertEqual(len(self.name_to_id), 2, "Name to id should have 2 entries") + self.assertEqual(self.name_to_id["Node1"], "ABCDE", "Name to id entry, ABCDE") + self.assertEqual(self.name_to_id["Node2"], "DE", "Name to id entry, DE") + + +class TestSuiteGraphNavUtil(unittest.TestSuite): + def __init__(self) -> None: + super(TestSuiteGraphNavUtil, self).__init__() + + self.loader = unittest.TestLoader() + self.addTest(self.loader.loadTestsFromTestCase(TestGraphNavUtilShortCode)) + self.addTest( + self.loader.loadTestsFromTestCase(TestGraphNavUtilFindUniqueWaypointId) + ) + self.addTest( + self.loader.loadTestsFromTestCase(TestGraphNavUtilUpdateWaypointsEdges) + ) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9b019101..ba85d8ca 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -4,7 +4,6 @@ import time import traceback import typing -from collections import namedtuple import bosdyn.client.auth from bosdyn.api import arm_command_pb2 @@ -15,6 +14,10 @@ from bosdyn.api import robot_state_pb2 from bosdyn.api import synchronized_command_pb2 from bosdyn.api import trajectory_pb2 +from bosdyn.api import world_object_pb2 +from bosdyn.api import lease_pb2 +from bosdyn.api import point_cloud_pb2 +from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 from bosdyn.api.graph_nav import graph_nav_pb2 from bosdyn.api.graph_nav import map_pb2 from bosdyn.api.graph_nav import nav_pb2 @@ -22,6 +25,7 @@ from bosdyn.client import frame_helpers from bosdyn.client import math_helpers from bosdyn.client import robot_command +from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock from bosdyn.client.estop import ( @@ -32,6 +36,7 @@ ) from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.client.graph_nav import GraphNavClient +from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.image import ( ImageClient, build_image_request, @@ -44,75 +49,22 @@ from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder from bosdyn.client.robot_state import RobotStateClient from bosdyn.client.world_object import WorldObjectClient +from bosdyn.client.spot_check import SpotCheckClient from bosdyn.geometry import EulerZXY from bosdyn.util import seconds_to_duration from google.protobuf.duration_pb2 import Duration -MAX_COMMAND_DURATION = 1e5 - -### Release -from . import graph_nav_util - -### Debug -# import graph_nav_util - from bosdyn.api import basic_command_pb2 from google.protobuf.timestamp_pb2 import Timestamp -front_image_sources = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "frontleft_depth", - "frontright_depth", -] -"""List of image sources for front image periodic query""" -side_image_sources = [ - "left_fisheye_image", - "right_fisheye_image", - "left_depth", - "right_depth", -] -"""List of image sources for side image periodic query""" -rear_image_sources = ["back_fisheye_image", "back_depth"] -"""List of image sources for rear image periodic query""" -VELODYNE_SERVICE_NAME = "velodyne-point-cloud" -"""Service name for getting pointcloud of VLP16 connected to Spot Core""" -point_cloud_sources = ["velodyne-point-cloud"] -"""List of point cloud sources""" -hand_image_sources = [ - "hand_image", - "hand_depth", - "hand_color_image", - "hand_depth_in_hand_color_frame", -] -"""List of image sources for hand image periodic query""" - - -# TODO: Missing Hand images -CAMERA_IMAGE_SOURCES = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "left_fisheye_image", - "right_fisheye_image", - "back_fisheye_image", -] -DEPTH_IMAGE_SOURCES = [ - "frontleft_depth", - "frontright_depth", - "left_depth", - "right_depth", - "back_depth", -] -DEPTH_REGISTERED_IMAGE_SOURCES = [ - "frontleft_depth_in_visual_frame", - "frontright_depth_in_visual_frame", - "right_depth_in_visual_frame", - "left_depth_in_visual_frame", - "back_depth_in_visual_frame", -] -ImageBundle = namedtuple( - "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] -) +from .spot_arm import SpotArm +from .spot_world_objects import SpotWorldObjects +from .spot_eap import SpotEAP +from .spot_docking import SpotDocking +from .spot_graph_nav import SpotGraphNav +from .spot_check import SpotCheck +from .spot_images import SpotImages +from .spot_config import * class AsyncRobotState(AsyncPeriodicQuery): @@ -216,36 +168,6 @@ def _start_query(self): return callback_future -class AsyncPointCloudService(AsyncPeriodicQuery): - """ - Class to get point cloud at regular intervals. get_point_cloud_from_sources_async query sent to the robot at - every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback, point_cloud_requests): - super(AsyncPointCloudService, self).__init__( - "robot_point_cloud_service", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - self._point_cloud_requests = point_cloud_requests - - def _start_query(self): - if self._callback and self._point_cloud_requests: - callback_future = self._client.get_point_cloud_async( - self._point_cloud_requests - ) - callback_future.add_done_callback(self._callback) - return callback_future - - class AsyncIdle(AsyncPeriodicQuery): """Class to check if the robot is moving, and if not, command a stand with the set mobility parameters @@ -418,31 +340,6 @@ def _start_query(self): pass -class AsyncWorldObjects(AsyncPeriodicQuery): - """Class to get world objects. list_world_objects_async query sent to the robot at every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback): - super(AsyncWorldObjects, self).__init__( - "world-objects", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - - def _start_query(self): - if self._callback: - callback_future = self._client.list_world_objects_async() - callback_future.add_done_callback(self._callback) - return callback_future - - def try_claim(func=None, *, power_on=False): """ Decorator which tries to acquire the lease before executing the wrapped function @@ -478,8 +375,6 @@ def wrapper_try_claim(self, *args, **kwargs): class SpotWrapper: """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically""" - SPOT_CLIENT_NAME = "ros_spot" - def __init__( self, username: str, @@ -489,8 +384,8 @@ def __init__( logger: logging.Logger, start_estop: bool = True, estop_timeout: float = 9.0, - rates: typing.Optional[typing.Dict] = None, - callbacks: typing.Optional[typing.Dict] = None, + rates: typing.Dict[str, float] = {}, + callbacks: typing.Dict[str, typing.Callable] = {}, use_take_lease: bool = False, get_lease_on_action: bool = False, continually_try_stand: bool = True, @@ -518,6 +413,8 @@ def __init__( self._password = password self._hostname = hostname self._robot_name = robot_name + self._rates = rates + self._callbacks = callbacks self._use_take_lease = use_take_lease self._get_lease_on_action = get_lease_on_action self._continually_try_stand = continually_try_stand @@ -525,14 +422,6 @@ def __init__( if robot_name is not None: self._frame_prefix = robot_name + "/" self._logger = logger - if rates is None: - self._rates = {} - else: - self._rates = rates - if callbacks is None: - self._callbacks = {} - else: - self._callbacks = callbacks self._estop_timeout = estop_timeout self._start_estop = start_estop self._keep_alive = True @@ -553,64 +442,18 @@ def __init__( self._last_trajectory_command_precise = None self._last_velocity_command_time = None self._last_docking_command = None - - self._front_image_requests = [] - for source in front_image_sources: - self._front_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._side_image_requests = [] - for source in side_image_sources: - self._side_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._rear_image_requests = [] - for source in rear_image_sources: - self._rear_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._point_cloud_requests = [] - for source in point_cloud_sources: - self._point_cloud_requests.append(build_pc_request(source)) - - self._hand_image_requests = [] - for source in hand_image_sources: - self._hand_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._camera_image_requests = [] - for camera_source in CAMERA_IMAGE_SOURCES: - self._camera_image_requests.append( - build_image_request( - camera_source, - image_format=image_pb2.Image.FORMAT_JPEG, - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ) - - self._depth_image_requests = [] - for camera_source in DEPTH_IMAGE_SOURCES: - self._depth_image_requests.append( - build_image_request( - camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 - ) - ) - - self._depth_registered_image_requests = [] - for camera_source in DEPTH_REGISTERED_IMAGE_SOURCES: - self._depth_registered_image_requests.append( - build_image_request( - camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 - ) - ) + self._robot_params = { + "is_standing": False, + "is_sitting": True, + "is_moving": False, + "robot_id": None, + "estop_timeout": self._estop_timeout, + "rates": self._rates, + "callbacks": self._callbacks, + } try: - self._sdk = create_standard_sdk(self.SPOT_CLIENT_NAME) + self._sdk = create_standard_sdk(SPOT_CLIENT_NAME) except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False @@ -648,255 +491,303 @@ def __init__( self._point_cloud_client = None self._logger.warning("No point cloud services are available.") - if self._robot: - # Clients - self._logger.info("Creating clients...") - initialised = False - while not initialised: - try: - self._robot_state_client = self._robot.ensure_client( - RobotStateClient.default_service_name - ) - self._world_objects_client = self._robot.ensure_client( - WorldObjectClient.default_service_name - ) - self._robot_command_client = self._robot.ensure_client( - RobotCommandClient.default_service_name - ) - self._graph_nav_client = self._robot.ensure_client( - GraphNavClient.default_service_name - ) - self._power_client = self._robot.ensure_client( - PowerClient.default_service_name - ) - self._lease_client = self._robot.ensure_client( - LeaseClient.default_service_name - ) - self._lease_wallet = self._lease_client.lease_wallet - self._image_client = self._robot.ensure_client( - ImageClient.default_service_name - ) - self._estop_client = self._robot.ensure_client( - EstopClient.default_service_name - ) - self._docking_client = self._robot.ensure_client( - DockingClient.default_service_name + if not self._robot: + self._logger.error("Failed to create robot object") + self._valid = False + return + + # Clients + self._logger.info("Creating clients...") + initialised = False + while not initialised: + try: + self._robot_state_client = self._robot.ensure_client( + RobotStateClient.default_service_name + ) + self._world_objects_client = self._robot.ensure_client( + WorldObjectClient.default_service_name + ) + self._robot_command_client = self._robot.ensure_client( + RobotCommandClient.default_service_name + ) + self._graph_nav_client = self._robot.ensure_client( + GraphNavClient.default_service_name + ) + self._map_processing_client = self._robot.ensure_client( + MapProcessingServiceClient.default_service_name + ) + self._power_client = self._robot.ensure_client( + PowerClient.default_service_name + ) + self._lease_client = self._robot.ensure_client( + LeaseClient.default_service_name + ) + self._lease_wallet = self._lease_client.lease_wallet + self._image_client = self._robot.ensure_client( + ImageClient.default_service_name + ) + self._estop_client = self._robot.ensure_client( + EstopClient.default_service_name + ) + self._docking_client = self._robot.ensure_client( + DockingClient.default_service_name + ) + self._spot_check_client = self._robot.ensure_client( + SpotCheckClient.default_service_name + ) + if self._robot.has_arm(): + self._manipulation_client = self._robot.ensure_client( + ManipulationApiClient.default_service_name ) - if self._robot.has_arm(): - self._manipulation_client = self._robot.ensure_client( - ManipulationApiClient.default_service_name - ) - initialised = True - except Exception as e: - sleep_secs = 15 - self._logger.warning( - "Unable to create client service: {}. This usually means the robot hasn't " - "finished booting yet. Will wait {} seconds and try again.".format( - e, sleep_secs - ) + + self._robot_clients = { + "robot_state_client": self._robot_state_client, + "robot_command_client": self._robot_command_client, + "graph_nav_client": self._graph_nav_client, + "map_processing_client": self._map_processing_client, + "power_client": self._power_client, + "lease_client": self._lease_client, + "image_client": self._image_client, + "estop_client": self._estop_client, + "docking_client": self._docking_client, + "spot_check_client": self._spot_check_client, + "robot_command_method": self._robot_command, + "world_objects_client": self._world_objects_client, + } + if self._point_cloud_client: + self._robot_clients["point_cloud_client"] = self._point_cloud_client + + initialised = True + except Exception as e: + sleep_secs = 15 + self._logger.warning( + "Unable to create client service: {}. This usually means the robot hasn't " + "finished booting yet. Will wait {} seconds and try again.".format( + e, sleep_secs ) - time.sleep(sleep_secs) - - # Store the most recent knowledge of the state of the robot based on rpc calls. - self._current_graph = None - self._current_edges = dict() # maps to_waypoint to list(from_waypoint) - self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot - self._current_edge_snapshots = dict() # maps id to edge snapshot - self._current_annotation_name_to_wp_id = dict() - - # Async Tasks - self._async_task_list = [] - self._robot_state_task = AsyncRobotState( - self._robot_state_client, - self._logger, - max(0.0, self._rates.get("robot_state", 0.0)), - self._callbacks.get("robot_state", None), - ) - self._robot_metrics_task = AsyncMetrics( - self._robot_state_client, - self._logger, - max(0.0, self._rates.get("metrics", 0.0)), - self._callbacks.get("metrics", None), - ) - self._lease_task = AsyncLease( - self._lease_client, - self._logger, - max(0.0, self._rates.get("lease", 0.0)), - self._callbacks.get("lease", None), - ) - self._front_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("front_image", 0.0)), - self._callbacks.get("front_image", None), - self._front_image_requests, - ) - self._side_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("side_image", 0.0)), - self._callbacks.get("side_image", None), - self._side_image_requests, - ) - self._rear_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("rear_image", 0.0)), - self._callbacks.get("rear_image", None), - self._rear_image_requests, - ) - self._hand_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("hand_image", 0.0)), - self._callbacks.get("hand_image", None), - self._hand_image_requests, - ) + ) + time.sleep(sleep_secs) - self._idle_task = AsyncIdle( - self._robot_command_client, self._logger, 10.0, self - ) - self._estop_monitor = AsyncEStopMonitor( - self._estop_client, self._logger, 20.0, self - ) - self._world_objects_task = AsyncWorldObjects( - self._world_objects_client, - self._logger, - 10.0, - self._callbacks.get("world_objects", None), - ) + # Core Async Tasks + self._async_task_list = [] + self._robot_state_task = AsyncRobotState( + self._robot_state_client, + self._logger, + max(0.0, self._rates.get("robot_state", 0.0)), + self._callbacks.get("robot_state", None), + ) + self._robot_metrics_task = AsyncMetrics( + self._robot_state_client, + self._logger, + max(0.0, self._rates.get("metrics", 0.0)), + self._callbacks.get("metrics", None), + ) + self._lease_task = AsyncLease( + self._lease_client, + self._logger, + max(0.0, self._rates.get("lease", 0.0)), + self._callbacks.get("lease", None), + ) + self._idle_task = AsyncIdle( + self._robot_command_client, self._logger, 10.0, self + ) + self._estop_monitor = AsyncEStopMonitor( + self._estop_client, self._logger, 20.0, self + ) - self._estop_endpoint = None - self._estop_keepalive = None + self._estop_endpoint = None + self._estop_keepalive = None + self._robot_id = None + self._lease = None + + robot_tasks = [ + self._robot_state_task, + self._robot_metrics_task, + self._lease_task, + self._idle_task, + self._estop_monitor, + ] - robot_tasks = [ - self._robot_state_task, - self._robot_metrics_task, - self._lease_task, - self._front_image_task, - self._idle_task, - self._estop_monitor, - self._world_objects_task, - ] - - if self._point_cloud_client: - self._point_cloud_task = AsyncPointCloudService( - self._point_cloud_client, - self._logger, - max(0.0, self._rates.get("point_cloud", 0.0)), - self._callbacks.get("lidar_points", None), - self._point_cloud_requests, - ) - robot_tasks.append(self._point_cloud_task) + # Create component objects for different functionality + self.spot_image = SpotImages( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + robot_tasks.append(self.spot_image.front_image_task) + robot_tasks.append(self.spot_image.side_image_task) + robot_tasks.append(self.spot_image.rear_image_task) + + if self._robot.has_arm(): + robot_tasks.append(self._hand_image_task) + self._spot_arm = SpotArm( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + else: + self._spot_arm = None + + self._spot_docking = SpotDocking( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_graph_nav = SpotGraphNav( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_check = SpotCheck( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_image = SpotImages( + self._robot, self._logger, self._robot_params, self._robot_clients + ) - self._async_tasks = AsyncTasks(robot_tasks) + if self._point_cloud_client: + self._spot_eap = SpotEAP( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._point_cloud_task = self._spot_eap.async_task + robot_tasks.append(self._point_cloud_task) + else: + self._spot_eap = None - self.camera_task_name_to_task_mapping = { - "hand_image": self._hand_image_task, - "side_image": self._side_image_task, - "rear_image": self._rear_image_task, - "front_image": self._front_image_task, - } + self._spot_world_objects = SpotWorldObjects( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._world_objects_task = self._spot_world_objects.async_task + robot_tasks.append(self._world_objects_task) - self._robot_id = None - self._lease = None + self._async_tasks = AsyncTasks(robot_tasks) @property - def robot_name(self): + def robot_name(self) -> str: return self._robot_name @property - def frame_prefix(self): + def frame_prefix(self) -> str: return self._frame_prefix @property - def logger(self): + def spot_images(self) -> SpotImages: + """Return SpotImages instance""" + return self._spot_image + + @property + def spot_arm(self) -> SpotArm: + """Return SpotArm instance""" + if not self._robot.has_arm(): + raise Exception("SpotArm is not available on this robot") + else: + return self._spot_arm + + @property + def spot_eap_lidar(self) -> SpotEAP: + """Return SpotEAP instance""" + return self._spot_eap + + @property + def spot_world_objects(self) -> SpotWorldObjects: + """Return SpotWorldObjects instance""" + return self._spot_world_objects + + @property + def spot_docking(self) -> SpotDocking: + """Return SpotDocking instance""" + return self._spot_docking + + @property + def spot_graph_nav(self) -> SpotGraphNav: + """Return SpotGraphNav instance""" + return self._spot_graph_nav + + @property + def spot_check(self) -> SpotCheck: + """Return SpotCheck instance""" + return self._spot_check + + @property + def logger(self) -> logging.Logger: """Return logger instance of the SpotWrapper""" return self._logger @property - def is_valid(self): + def is_valid(self) -> bool: """Return boolean indicating if the wrapper initialized successfully""" return self._valid @property - def id(self): + def id(self) -> str: """Return robot's ID""" return self._robot_id @property - def robot_state(self): + def robot_state(self) -> robot_state_pb2.RobotState: """Return latest proto from the _robot_state_task""" return self._robot_state_task.proto @property - def metrics(self): + def metrics(self) -> robot_state_pb2.RobotMetrics: """Return latest proto from the _robot_metrics_task""" return self._robot_metrics_task.proto @property - def lease(self): + def lease(self) -> typing.List[lease_pb2.LeaseResource]: """Return latest proto from the _lease_task""" return self._lease_task.proto @property - def world_objects(self): + def world_objects(self) -> world_object_pb2.ListWorldObjectResponse: """Return most recent proto from _world_objects_task""" - return self._world_objects_task.proto + return self.spot_world_objects.async_task.proto @property - def front_images(self): + def front_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _front_image_task""" - return self._front_image_task.proto + return self.spot_image.front_image_task.proto @property - def side_images(self): + def side_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _side_image_task""" - return self._side_image_task.proto + return self.spot_image.side_image_task.proto @property - def rear_images(self): + def rear_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _rear_image_task""" - return self._rear_image_task.proto + return self.spot_image.rear_image_task.proto @property - def hand_images(self): + def hand_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _hand_image_task""" return self._hand_image_task.proto @property - def point_clouds(self): + def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: """Return latest proto from the _point_cloud_task""" - return self._point_cloud_task.proto + return self.spot_eap_lidar.async_task.proto @property - def is_standing(self): + def is_standing(self) -> bool: """Return boolean of standing state""" return self._is_standing @property - def is_sitting(self): + def is_sitting(self) -> bool: """Return boolean of standing state""" return self._is_sitting @property - def is_moving(self): + def is_moving(self) -> bool: """Return boolean of walking state""" return self._is_moving @property - def near_goal(self): + def near_goal(self) -> bool: return self._near_goal @property - def at_goal(self): + def at_goal(self) -> bool: return self._at_goal - def is_estopped(self, timeout=None): + def is_estopped(self, timeout=None) -> bool: return self._robot.is_estopped(timeout=timeout) @property - def time_skew(self): + def time_skew(self) -> Timestamp: """Return the time skew between local and spot time""" return self._robot.time_sync.endpoint.clock_skew @@ -907,7 +798,7 @@ def resetMobilityParams(self): """ self._mobility_params = RobotCommandBuilder.mobility_params() - def robotToLocalTime(self, timestamp): + def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: """Takes a timestamp and an estimated skew and return seconds and nano seconds in local time Args: @@ -1034,7 +925,12 @@ def disconnect(self): self.releaseLease() self.releaseEStop() - def _robot_command(self, command_proto, end_time_secs=None, timesync_endpoint=None): + def _robot_command( + self, + command_proto: robot_command_pb2.RobotCommand, + end_time_secs: typing.Optional[float] = None, + timesync_endpoint: typing.Optional[TimeSyncEndpoint] = None, + ) -> typing.Tuple[bool, str, typing.Optional[str]]: """Generic blocking function for sending commands to robots. Args: @@ -1170,7 +1066,7 @@ def power_on(self): return True, "Was already powered on" - def set_mobility_params(self, mobility_params): + def set_mobility_params(self, mobility_params: spot_command_pb2.MobilityParams): """Set Params for mobility and movement Args: @@ -1178,17 +1074,18 @@ def set_mobility_params(self, mobility_params): """ self._mobility_params = mobility_params - def get_mobility_params(self): + def get_mobility_params(self) -> spot_command_pb2.MobilityParams: """Get mobility params""" return self._mobility_params - def list_world_objects(self, object_types, time_start_point): - return self._world_objects_client.list_world_objects( - object_types, time_start_point - ) - @try_claim - def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125): + def velocity_cmd( + self, + v_x: float, + v_y: float, + v_rot: float, + cmd_duration: float = 0.7, + ) -> typing.Tuple[bool, str]: """Send a velocity motion command to the robot. Args: @@ -1211,14 +1108,14 @@ def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125): @try_claim def trajectory_cmd( self, - goal_x, - goal_y, - goal_heading, - cmd_duration, - frame_name="odom", - precise_position=False, - mobility_params=None, - ): + goal_x: float, + goal_y: float, + goal_heading: float, + cmd_duration: float, + frame_name: str = "odom", + precise_position: bool = False, + mobility_params: spot_command_pb2.MobilityParams = None, + ) -> typing.Tuple[bool, str]: """Send a trajectory motion command to the robot. Args: @@ -1282,7 +1179,9 @@ def trajectory_cmd( self._last_trajectory_command = response[2] return response[0], response[1] - def robot_command(self, robot_command): + def robot_command( + self, robot_command: robot_command_pb2.RobotCommand + ) -> typing.Tuple[bool, str]: end_time = time.time() + MAX_COMMAND_DURATION return self._robot_command( robot_command, @@ -1290,1053 +1189,13 @@ def robot_command(self, robot_command): timesync_endpoint=self._robot.time_sync.endpoint, ) - def get_robot_command_feedback(self, cmd_id): + def get_robot_command_feedback( + self, cmd_id: int + ) -> robot_command_pb2.RobotCommandFeedbackResponse: return self._robot_command_client.robot_command_feedback(cmd_id) - def list_graph(self, upload_path): - """List waypoint ids of garph_nav - Args: - upload_path : Path to the root directory of the map. - """ - ids, eds = self._list_graph_waypoint_and_edge_ids() - # skip waypoint_ for v2.2.1, skip waypiont for < v2.2 - return [ - v - for k, v in sorted( - ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) - ) - ] - - @try_claim - def navigate_to( - self, - upload_path, - navigate_to, - initial_localization_fiducial=True, - initial_localization_waypoint=None, - ): - """navigate with graph nav. - - Args: - upload_path : Path to the root directory of the map. - navigate_to : Waypont id string for where to goal - initial_localization_fiducial : Tells the initializer whether to use fiducials - initial_localization_waypoint : Waypoint id string of current robot position (optional) - """ - # Filepath for uploading a saved graph's and snapshots too. - if upload_path[-1] == "/": - upload_filepath = upload_path[:-1] - else: - upload_filepath = upload_path - - # Boolean indicating the robot's power state. - power_state = self._robot_state_client.get_robot_state().power_state - self._started_powered_on = power_state.motor_power_state == power_state.STATE_ON - self._powered_on = self._started_powered_on - - # FIX ME somehow,,,, if the robot is stand, need to sit the robot before starting garph nav - if self.is_standing and not self.is_moving: - self.sit() - - # TODO verify estop / claim / power_on - self._clear_graph() - self._upload_graph_and_snapshots(upload_filepath) - if initial_localization_fiducial: - self._set_initial_localization_fiducial() - if initial_localization_waypoint: - self._set_initial_localization_waypoint([initial_localization_waypoint]) - self._list_graph_waypoint_and_edge_ids() - self._get_localization_state() - resp = self._navigate_to([navigate_to]) - - return resp - - # Arm ############################################ - @try_claim - def ensure_arm_power_and_stand(self): - if not self._robot.has_arm(): - return False, "Spot with an arm is required for this service" - - try: - if not self.check_is_powered_on(): - self._logger.info("Spot is powering on within the timeout of 20 secs") - self._robot.power_on(timeout_sec=20) - assert self._robot.is_powered_on(), "Spot failed to power on" - self._logger.info("Spot is powered on") - except Exception as e: - return ( - False, - "Exception occured while Spot or its arm were trying to power on", - ) - - if not self._is_standing: - robot_command.blocking_stand( - command_client=self._robot_command_client, timeout_sec=10.0 - ) - self._logger.info("Spot is standing") - else: - self._logger.info("Spot is already standing") - - return True, "Spot has an arm, is powered on, and standing" - - @try_claim - def arm_stow(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Stow Arm - stow = RobotCommandBuilder.arm_stow_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(stow) - self._logger.info("Command stow issued") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while trying to stow" - - return True, "Stow arm success" - - @try_claim - def arm_unstow(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Unstow Arm - unstow = RobotCommandBuilder.arm_ready_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(unstow) - self._logger.info("Command unstow issued") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while trying to unstow" - - return True, "Unstow arm success" - - @try_claim - def arm_carry(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Get Arm in carry mode - carry = RobotCommandBuilder.arm_carry_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(carry) - self._logger.info("Command carry issued") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while carry mode was issued" - - return True, "Carry mode success" - - def make_arm_trajectory_command(self, arm_joint_trajectory): - """Helper function to create a RobotCommand from an ArmJointTrajectory. - Copy from 'spot-sdk/python/examples/arm_joint_move/arm_joint_move.py'""" - - joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request( - trajectory=arm_joint_trajectory - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_joint_move_command=joint_move_command - ) - sync_arm = synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - arm_sync_robot_cmd = robot_command_pb2.RobotCommand( - synchronized_command=sync_arm - ) - return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd) - - @try_claim - def arm_joint_move(self, joint_targets): - # All perspectives are given when looking at the robot from behind after the unstow service is called - # Joint1: 0.0 arm points to the front. positive: turn left, negative: turn right) - # RANGE: -3.14 -> 3.14 - # Joint2: 0.0 arm points to the front. positive: move down, negative move up - # RANGE: 0.4 -> -3.13 ( - # Joint3: 0.0 arm straight. moves the arm down - # RANGE: 0.0 -> 3.1415 - # Joint4: 0.0 middle position. negative: moves ccw, positive moves cw - # RANGE: -2.79253 -> 2.79253 - # # Joint5: 0.0 gripper points to the front. positive moves the gripper down - # RANGE: -1.8326 -> 1.8326 - # Joint6: 0.0 Gripper is not rolled, positive is ccw - # RANGE: -2.87 -> 2.87 - # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0] - if abs(joint_targets[0]) > 3.14: - msg = "Joint 1 has to be between -3.14 and 3.14" - self._logger.warning(msg) - return False, msg - elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13: - msg = "Joint 2 has to be between -3.13 and 0.4" - self._logger.warning(msg) - return False, msg - elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0: - msg = "Joint 3 has to be between 0.0 and 3.14" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[3]) > 2.79253: - msg = "Joint 4 has to be between -2.79253 and 2.79253" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[4]) > 1.8326: - msg = "Joint 5 has to be between -1.8326 and 1.8326" - self._logger.warning(msg) - return False, msg - elif abs(joint_targets[5]) > 2.87: - msg = "Joint 6 has to be between -2.87 and 2.87" - self._logger.warning(msg) - return False, msg - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - trajectory_point = ( - RobotCommandBuilder.create_arm_joint_trajectory_point( - joint_targets[0], - joint_targets[1], - joint_targets[2], - joint_targets[3], - joint_targets[4], - joint_targets[5], - ) - ) - arm_joint_trajectory = arm_command_pb2.ArmJointTrajectory( - points=[trajectory_point] - ) - arm_command = self.make_arm_trajectory_command(arm_joint_trajectory) - - # Send the request - cmd_id = self._robot_command_client.robot_command(arm_command) - - # Query for feedback to determine how long it will take - feedback_resp = self._robot_command_client.robot_command_feedback( - cmd_id - ) - joint_move_feedback = ( - feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback - ) - time_to_goal: Duration = joint_move_feedback.time_to_goal - time_to_goal_in_seconds: float = time_to_goal.seconds + ( - float(time_to_goal.nanos) / float(10**9) - ) - time.sleep(time_to_goal_in_seconds) - return True, "Spot Arm moved successfully" - - except Exception as e: - return False, "Exception occured during arm movement: " + str(e) - - @try_claim - def force_trajectory(self, data): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - - def create_wrench_from_msg(forces, torques): - force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) - torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) - return geometry_pb2.Wrench(force=force, torque=torque) - - # Duration in seconds. - traj_duration = data.duration - - # first point on trajectory - wrench0 = create_wrench_from_msg(data.forces_pt0, data.torques_pt0) - t0 = seconds_to_duration(0) - traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( - wrench=wrench0, time_since_reference=t0 - ) - - # Second point on the trajectory - wrench1 = create_wrench_from_msg(data.forces_pt1, data.torques_pt1) - t1 = seconds_to_duration(traj_duration) - traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( - wrench=wrench1, time_since_reference=t1 - ) - - # Build the trajectory - trajectory = trajectory_pb2.WrenchTrajectory( - points=[traj_point0, traj_point1] - ) - - # Build the trajectory request, putting all axes into force mode - arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( - root_frame_name=data.frame, - wrench_trajectory_in_task=trajectory, - x_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - y_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - z_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - rx_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - ry_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - rz_axis=arm_command_pb2.ArmCartesianCommand.Request.AXIS_MODE_FORCE, - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_cartesian_command=arm_cartesian_command - ) - synchronized_command = ( - synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - ) - robot_command = robot_command_pb2.RobotCommand( - synchronized_command=synchronized_command - ) - - # Send the request - self._robot_command_client.robot_command(robot_command) - self._logger.info("Force trajectory command sent") - - time.sleep(float(traj_duration) + 1.0) - - except Exception as e: - return False, "Exception occured during arm movement" - - return True, "Moved arm successfully" - - @try_claim - def gripper_open(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Open gripper - command = RobotCommandBuilder.claw_gripper_open_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) - self._logger.info("Command gripper open sent") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while gripper was moving" - - return True, "Open gripper success" - - @try_claim - def gripper_close(self): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # Close gripper - command = RobotCommandBuilder.claw_gripper_close_command() - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) - self._logger.info("Command gripper close sent") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while gripper was moving" - - return True, "Closed gripper successfully" - - @try_claim - def gripper_angle_open(self, gripper_ang): - # takes an angle between 0 (closed) and 90 (fully opened) and opens the - # gripper at this angle - if gripper_ang > 90 or gripper_ang < 0: - return False, "Gripper angle must be between 0 and 90" - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - # The open angle command does not take degrees but the limits - # defined in the urdf, that is why we have to interpolate - closed = 0.349066 - opened = -1.396263 - angle = gripper_ang / 90.0 * (opened - closed) + closed - command = RobotCommandBuilder.claw_gripper_open_angle_command(angle) - - # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) - self._logger.info("Command gripper open angle sent") - time.sleep(2.0) - - except Exception as e: - return False, "Exception occured while gripper was moving" - - return True, "Opened gripper successfully" - - @try_claim - def hand_pose(self, data): - try: - success, msg = self.ensure_arm_power_and_stand() - if not success: - self._logger.info(msg) - return False, msg - else: - pose_point = data.pose_point - # Move the arm to a spot in front of the robot given a pose for the gripper. - # Build a position to move the arm to (in meters, relative to the body frame origin.) - position = geometry_pb2.Vec3( - x=pose_point.position.x, - y=pose_point.position.y, - z=pose_point.position.z, - ) - - # # Rotation as a quaternion. - rotation = geometry_pb2.Quaternion( - w=pose_point.orientation.w, - x=pose_point.orientation.x, - y=pose_point.orientation.y, - z=pose_point.orientation.z, - ) - - seconds = data.duration - duration = seconds_to_duration(seconds) - - # Build the SE(3) pose of the desired hand position in the moving body frame. - hand_pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) - hand_pose_traj_point = trajectory_pb2.SE3TrajectoryPoint( - pose=hand_pose, time_since_reference=duration - ) - hand_trajectory = trajectory_pb2.SE3Trajectory( - points=[hand_pose_traj_point] - ) - - arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( - root_frame_name=data.frame, - pose_trajectory_in_task=hand_trajectory, - force_remain_near_current_joint_configuration=True, - ) - arm_command = arm_command_pb2.ArmCommand.Request( - arm_cartesian_command=arm_cartesian_command - ) - synchronized_command = ( - synchronized_command_pb2.SynchronizedCommand.Request( - arm_command=arm_command - ) - ) - - robot_command = robot_command_pb2.RobotCommand( - synchronized_command=synchronized_command - ) - - command = RobotCommandBuilder.build_synchro_command(robot_command) - - # Send the request - self._robot_command_client.robot_command(robot_command) - self._logger.info("Moving arm to position.") - - time.sleep(2.0) - - except Exception as e: - return ( - False, - "An error occured while trying to move arm \n Exception:" + str(e), - ) - - return True, "Moved arm successfully" - - @try_claim - def grasp_3d(self, frame, object_rt_frame): - try: - frm = str(frame) - pos = geometry_pb2.Vec3( - x=object_rt_frame[0], y=object_rt_frame[1], z=object_rt_frame[2] - ) - - grasp = manipulation_api_pb2.PickObject(frame_name=frm, object_rt_frame=pos) - - # Ask the robot to pick up the object - grasp_request = manipulation_api_pb2.ManipulationApiRequest( - pick_object=grasp - ) - # Send the request - cmd_response = self._manipulation_client.manipulation_api_command( - manipulation_api_request=grasp_request - ) - - # Get feedback from the robot - while True: - feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( - manipulation_cmd_id=cmd_response.manipulation_cmd_id - ) - - # Send the request - response = self._manipulation_client.manipulation_api_feedback_command( - manipulation_api_feedback_request=feedback_request - ) - - print( - "Current state: ", - manipulation_api_pb2.ManipulationFeedbackState.Name( - response.current_state - ), - ) - - if ( - response.current_state - == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED - or response.current_state - == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED - ): - break - - time.sleep(0.25) - - self._robot.logger.info("Finished grasp.") - - except Exception as e: - return False, "An error occured while trying to grasp from pose" - - return True, "Grasped successfully" - - ################################################################### - - ## copy from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py - def _get_localization_state(self, *args): - """Get the current localization and state of the robot.""" - state = self._graph_nav_client.get_localization_state() - self._logger.info("Got localization: \n%s" % str(state.localization)) - odom_tform_body = get_odom_tform_body( - state.robot_kinematics.transforms_snapshot - ) - self._logger.info( - "Got robot state in kinematic odometry frame: \n%s" % str(odom_tform_body) - ) - - def _set_initial_localization_fiducial(self, *args): - """Trigger localization when near a fiducial.""" - robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() - # Create an empty instance for initial localization since we are asking it to localize - # based on the nearest fiducial. - localization = nav_pb2.Localization() - self._graph_nav_client.set_localization( - initial_guess_localization=localization, - ko_tform_body=current_odom_tform_body, - ) - - def _set_initial_localization_waypoint(self, *args): - """Trigger localization to a waypoint.""" - # Take the first argument as the localization waypoint. - if len(args) < 1: - # If no waypoint id is given as input, then return without initializing. - self._logger.error("No waypoint specified to initialize to.") - return - destination_waypoint = graph_nav_util.find_unique_waypoint_id( - args[0][0], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not destination_waypoint: - # Failed to find the unique waypoint id. - return - - robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() - # Create an initial localization to the specified waypoint as the identity. - localization = nav_pb2.Localization() - localization.waypoint_id = destination_waypoint - localization.waypoint_tform_body.rotation.w = 1.0 - self._graph_nav_client.set_localization( - initial_guess_localization=localization, - # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). - max_distance=0.2, - max_yaw=20.0 * math.pi / 180.0, - fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NO_FIDUCIAL, - ko_tform_body=current_odom_tform_body, - ) - - def _list_graph_waypoint_and_edge_ids(self, *args): - """List the waypoint ids and edge ids of the graph currently on the robot.""" - - # Download current graph - graph = self._graph_nav_client.download_graph() - if graph is None: - self._logger.error("Empty graph.") - return - self._current_graph = graph - - localization_id = ( - self._graph_nav_client.get_localization_state().localization.waypoint_id - ) - - # Update and print waypoints and edges - ( - self._current_annotation_name_to_wp_id, - self._current_edges, - ) = graph_nav_util.update_waypoints_and_edges( - graph, localization_id, self._logger - ) - return self._current_annotation_name_to_wp_id, self._current_edges - - def _upload_graph_and_snapshots(self, upload_filepath): - """Upload the graph and snapshots to the robot.""" - self._logger.info("Loading the graph from disk into local storage...") - with open(upload_filepath + "/graph", "rb") as graph_file: - # Load the graph from disk. - data = graph_file.read() - self._current_graph = map_pb2.Graph() - self._current_graph.ParseFromString(data) - self._logger.info( - "Loaded graph has {} waypoints and {} edges".format( - len(self._current_graph.waypoints), len(self._current_graph.edges) - ) - ) - for waypoint in self._current_graph.waypoints: - # Load the waypoint snapshots from disk. - with open( - upload_filepath + "/waypoint_snapshots/{}".format(waypoint.snapshot_id), - "rb", - ) as snapshot_file: - waypoint_snapshot = map_pb2.WaypointSnapshot() - waypoint_snapshot.ParseFromString(snapshot_file.read()) - self._current_waypoint_snapshots[ - waypoint_snapshot.id - ] = waypoint_snapshot - for edge in self._current_graph.edges: - # Load the edge snapshots from disk. - with open( - upload_filepath + "/edge_snapshots/{}".format(edge.snapshot_id), "rb" - ) as snapshot_file: - edge_snapshot = map_pb2.EdgeSnapshot() - edge_snapshot.ParseFromString(snapshot_file.read()) - self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot - # Upload the graph to the robot. - self._logger.info("Uploading the graph and snapshots to the robot...") - self._graph_nav_client.upload_graph( - lease=self._lease.lease_proto, graph=self._current_graph - ) - # Upload the snapshots to the robot. - for waypoint_snapshot in self._current_waypoint_snapshots.values(): - self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) - self._logger.info("Uploaded {}".format(waypoint_snapshot.id)) - for edge_snapshot in self._current_edge_snapshots.values(): - self._graph_nav_client.upload_edge_snapshot(edge_snapshot) - self._logger.info("Uploaded {}".format(edge_snapshot.id)) - - # The upload is complete! Check that the robot is localized to the graph, - # and it if is not, prompt the user to localize the robot before attempting - # any navigation commands. - localization_state = self._graph_nav_client.get_localization_state() - if not localization_state.localization.waypoint_id: - # The robot is not localized to the newly uploaded graph. - self._logger.info( - "Upload complete! The robot is currently not localized to the map; please localize", - "the robot using commands (2) or (3) before attempting a navigation command.", - ) - - @try_claim - def _navigate_to(self, *args): - """Navigate to a specific waypoint.""" - # Take the first argument as the destination waypoint. - if len(args) < 1: - # If no waypoint id is given as input, then return without requesting navigation. - self._logger.info("No waypoint provided as a destination for navigate to.") - return - - self._lease = self._lease_wallet.get_lease() - destination_waypoint = graph_nav_util.find_unique_waypoint_id( - args[0][0], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not destination_waypoint: - # Failed to find the appropriate unique waypoint id for the navigation command. - return - if not self.toggle_power(should_power_on=True): - self._logger.info( - "Failed to power on the robot, and cannot complete navigate to request." - ) - return - - # Stop the lease keepalive and create a new sublease for graph nav. - self._lease = self._lease_wallet.advance() - sublease = self._lease.create_sublease() - self._lease_keepalive.shutdown() - - # Navigate to the destination waypoint. - is_finished = False - nav_to_cmd_id = -1 - while not is_finished: - # Issue the navigation command about twice a second such that it is easy to terminate the - # navigation command (with estop or killing the program). - nav_to_cmd_id = self._graph_nav_client.navigate_to( - destination_waypoint, 1.0, leases=[sublease.lease_proto] - ) - time.sleep(0.5) # Sleep for half a second to allow for command execution. - # Poll the robot for feedback to determine if the navigation command is complete. Then sit - # the robot down once it is finished. - is_finished = self._check_success(nav_to_cmd_id) - - self._lease = self._lease_wallet.advance() - self._lease_keepalive = LeaseKeepAlive(self._lease_client) - - # Update the lease and power off the robot if appropriate. - if self._powered_on and not self._started_powered_on: - # Sit the robot down + power off after the navigation command is complete. - self.toggle_power(should_power_on=False) - - status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): - return True, "Successfully completed the navigation commands!" - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - return ( - False, - "Robot got lost when navigating the route, the robot will now sit down.", - ) - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - return ( - False, - "Robot got stuck when navigating the route, the robot will now sit down.", - ) - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): - return False, "Robot is impaired." - else: - return False, "Navigation command is not complete yet." - - @try_claim - def _navigate_route(self, *args): - """Navigate through a specific route of waypoints.""" - if len(args) < 1: - # If no waypoint ids are given as input, then return without requesting navigation. - self._logger.error("No waypoints provided for navigate route.") - return - waypoint_ids = args[0] - for i in range(len(waypoint_ids)): - waypoint_ids[i] = graph_nav_util.find_unique_waypoint_id( - waypoint_ids[i], - self._current_graph, - self._current_annotation_name_to_wp_id, - self._logger, - ) - if not waypoint_ids[i]: - # Failed to find the unique waypoint id. - return - - edge_ids_list = [] - all_edges_found = True - # Attempt to find edges in the current graph that match the ordered waypoint pairs. - # These are necessary to create a valid route. - for i in range(len(waypoint_ids) - 1): - start_wp = waypoint_ids[i] - end_wp = waypoint_ids[i + 1] - edge_id = self._match_edge(self._current_edges, start_wp, end_wp) - if edge_id is not None: - edge_ids_list.append(edge_id) - else: - all_edges_found = False - self._logger.error( - "Failed to find an edge between waypoints: ", - start_wp, - " and ", - end_wp, - ) - self._logger.error( - "List the graph's waypoints and edges to ensure pairs of waypoints has an edge." - ) - break - - self._lease = self._lease_wallet.get_lease() - if all_edges_found: - if not self.toggle_power(should_power_on=True): - self._logger.error( - "Failed to power on the robot, and cannot complete navigate route request." - ) - return - - # Stop the lease keepalive and create a new sublease for graph nav. - self._lease = self._lease_wallet.advance() - sublease = self._lease.create_sublease() - self._lease_keepalive.shutdown() - - # Navigate a specific route. - route = self._graph_nav_client.build_route(waypoint_ids, edge_ids_list) - is_finished = False - while not is_finished: - # Issue the route command about twice a second such that it is easy to terminate the - # navigation command (with estop or killing the program). - nav_route_command_id = self._graph_nav_client.navigate_route( - route, cmd_duration=1.0, leases=[sublease.lease_proto] - ) - time.sleep( - 0.5 - ) # Sleep for half a second to allow for command execution. - # Poll the robot for feedback to determine if the route is complete. Then sit - # the robot down once it is finished. - is_finished = self._check_success(nav_route_command_id) - - self._lease = self._lease_wallet.advance() - self._lease_keepalive = LeaseKeepAlive(self._lease_client) - - # Update the lease and power off the robot if appropriate. - if self._powered_on and not self._started_powered_on: - # Sit the robot down + power off after the navigation command is complete. - self.toggle_power(should_power_on=False) - - def _clear_graph(self, *args): - """Clear the state of the map on the robot, removing all waypoints and edges.""" - return self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) - - @try_claim - def toggle_power(self, should_power_on): - """Power the robot on/off dependent on the current power state.""" - is_powered_on = self.check_is_powered_on() - if not is_powered_on and should_power_on: - # Power on the robot up before navigating when it is in a powered-off state. - power_on(self._power_client) - motors_on = False - while not motors_on: - future = self._robot_state_client.get_robot_state_async() - state_response = future.result( - timeout=10 - ) # 10 second timeout for waiting for the state response. - if ( - state_response.power_state.motor_power_state - == robot_state_pb2.PowerState.STATE_ON - ): - motors_on = True - else: - # Motors are not yet fully powered on. - time.sleep(0.25) - elif is_powered_on and not should_power_on: - # Safe power off (robot will sit then power down) when it is in a - # powered-on state. - safe_power_off(self._robot_command_client, self._robot_state_client) - else: - # Return the current power state without change. - return is_powered_on - # Update the locally stored power state. - self.check_is_powered_on() - return self._powered_on - def check_is_powered_on(self): """Determine if the robot is powered on or off.""" power_state = self._robot_state_client.get_robot_state().power_state self._powered_on = power_state.motor_power_state == power_state.STATE_ON return self._powered_on - - def _check_success(self, command_id=-1): - """Use a navigation command id to get feedback from the robot and sit when command succeeds.""" - if command_id == -1: - # No command, so we have not status to check. - return False - status = self._graph_nav_client.navigation_feedback(command_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): - # Successfully completed the navigation commands! - return True - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - self._logger.error( - "Robot got lost when navigating the route, the robot will now sit down." - ) - return True - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - self._logger.error( - "Robot got stuck when navigating the route, the robot will now sit down." - ) - return True - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): - self._logger.error("Robot is impaired.") - return True - else: - # Navigation command is not complete yet. - return False - - def _match_edge(self, current_edges, waypoint1, waypoint2): - """Find an edge in the graph that is between two waypoint ids.""" - # Return the correct edge id as soon as it's found. - for edge_to_id in current_edges: - for edge_from_id in current_edges[edge_to_id]: - if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): - # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint2, to_waypoint=waypoint1 - ) - elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): - # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint1, to_waypoint=waypoint2 - ) - return None - - @try_claim - def dock(self, dock_id): - """Dock the robot to the docking station with fiducial ID [dock_id].""" - try: - # Make sure we're powered on and standing - self._robot.power_on() - self.stand() - # Dock the robot - self._last_docking_command = dock_id - blocking_dock_robot(self._robot, dock_id) - self._last_docking_command = None - # Necessary to reset this as docking often causes the last stand command to go into an unknown state - self._last_stand_command = None - return True, "Success" - except Exception as e: - return False, str(e) - - @try_claim - def undock(self, timeout=20): - """Power motors on and undock the robot from the station.""" - try: - # Maker sure we're powered on - self._robot.power_on() - # Undock the robot - blocking_undock(self._robot, timeout) - return True, "Success" - except Exception as e: - return False, str(e) - - def get_docking_state(self, **kwargs): - """Get docking state of robot.""" - state = self._docking_client.get_docking_state(**kwargs) - return state - - def update_image_tasks(self, image_name): - """Adds an async tasks to retrieve images from the specified image source""" - - task_to_add = self.camera_task_name_to_task_mapping[image_name] - - if task_to_add == self._hand_image_task and not self._robot.has_arm(): - self._logger.warning( - "Robot has no arm, therefore the arm image task can not be added" - ) - return - - if task_to_add in self._async_tasks._tasks: - self._logger.warning( - f"Task {image_name} already in async task list, will not be added again" - ) - return - - self._async_tasks.add_task(self.camera_task_name_to_task_mapping[image_name]) - - def get_frontleft_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "frontleft_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_frontright_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "frontright_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_left_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "left_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_right_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "right_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_back_rgb_image(self): - try: - return self._image_client.get_image( - [ - build_image_request( - "back_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, - ) - ] - )[0] - except UnsupportedPixelFormatRequestedError as e: - return None - - def get_images( - self, image_requests: typing.List[image_pb2.ImageRequest] - ) -> typing.Optional[ImageBundle]: - try: - image_responses = self._image_client.get_image(image_requests) - except UnsupportedPixelFormatRequestedError as e: - return None - return ImageBundle( - frontleft=image_responses[0], - frontright=image_responses[1], - left=image_responses[2], - right=image_responses[3], - back=image_responses[4], - ) - - def get_camera_images(self) -> ImageBundle: - return self.get_images(self._camera_image_requests) - - def get_depth_images(self) -> ImageBundle: - return self.get_images(self._depth_image_requests) - - def get_depth_registered_images(self) -> ImageBundle: - return self.get_images(self._depth_registered_image_requests) From 76f4899c5c8b58c8bcf2b2ba2ddaf47ec7d3164a Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 19:25:11 +0000 Subject: [PATCH 02/57] refactored image service --- spot_wrapper/spot_arm.py | 86 +++++++++++++++++++------ spot_wrapper/spot_config.py | 25 +------- spot_wrapper/spot_images.py | 125 ------------------------------------ spot_wrapper/wrapper.py | 27 ++------ 4 files changed, 75 insertions(+), 188 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index a06e3fab..0d75b73f 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -3,19 +3,12 @@ import logging from bosdyn.util import seconds_to_duration -from bosdyn.client.frame_helpers import ( - ODOM_FRAME_NAME, - GRAV_ALIGNED_BODY_FRAME_NAME, - get_a_tform_b, -) -from bosdyn.client import robot_command, math_helpers +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client import robot_command from bosdyn.client.robot import Robot from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.robot_command import ( - RobotCommandBuilder, - RobotCommandClient, - block_until_arm_arrives, -) +from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.image import ImageClient, build_image_request from bosdyn.client.manipulation_api_client import ManipulationApiClient from bosdyn.api import robot_command_pb2 from bosdyn.api import arm_command_pb2 @@ -23,15 +16,40 @@ from bosdyn.api import geometry_pb2 from bosdyn.api import trajectory_pb2 from bosdyn.api import manipulation_api_pb2 +from bosdyn.api import image_pb2 from google.protobuf.duration_pb2 import Duration -from geometry_msgs.msg import Pose -from spot_msgs.srv import HandPose, HandPoseResponse, HandPoseRequest -from spot_msgs.srv import ( - ArmForceTrajectory, - ArmForceTrajectoryResponse, - ArmForceTrajectoryRequest, -) + +from spot_msgs.srv import HandPoseRequest +from spot_msgs.srv import ArmForceTrajectoryRequest + +from .spot_config import * + + +class AsyncImageService(AsyncPeriodicQuery): + """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. + + Attributes: + client: The Client to a service on the robot + logger: Logger object + rate: Rate (Hz) to trigger the query + callback: Callback function to call when the results of the query are available + """ + + def __init__(self, client, logger, rate, callback, image_requests): + super(AsyncImageService, self).__init__( + "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) + ) + self._callback = None + if rate > 0.0: + self._callback = callback + self._image_requests = image_requests + + def _start_query(self): + if self._callback: + callback_future = self._client.get_image_async(self._image_requests) + callback_future.add_done_callback(self._callback) + return callback_future class SpotArm: @@ -62,7 +80,37 @@ def __init__( "manipulation_client" ] self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] + self._image_client: ImageClient = robot_clients["image_client"] self._robot_command: typing.Callable = robot_clients["robot_command_method"] + self._rates: typing.Dict[str, float] = robot_params["rates"] + self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + + self._hand_image_task: AsyncImageService = None + self.initialize_hand_image_service() + + def initialize_hand_image_service(self): + self._hand_image_requests = [] + + for source in HAND_IMAGE_SOURCES: + self._hand_image_requests.append( + build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) + ) + + self._hand_image_task = AsyncImageService( + self._image_client, + self._logger, + max(0.0, self._rates.get("hand_image", 0.0)), + self._callbacks.get("hand_image", None), + self._hand_image_requests, + ) + + self.camera_task_name_to_task_mapping: typing.Dict[str, AsyncImageService] = { + "hand_image": self._hand_image_task, + } + + @property + def hand_image_task(self): + return self._hand_image_task def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: if not self._robot.has_arm(): @@ -388,7 +436,7 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: return True, "Opened gripper successfully" - def hand_pose(self, data: HandPoseRequest): + def hand_pose(self, data: HandPoseRequest) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() if not success: diff --git a/spot_wrapper/spot_config.py b/spot_wrapper/spot_config.py index 0aa325b4..0cd7c921 100644 --- a/spot_wrapper/spot_config.py +++ b/spot_wrapper/spot_config.py @@ -6,40 +6,21 @@ SPOT_CLIENT_NAME = "ros_spot" MAX_COMMAND_DURATION = 1e5 -"""List of image sources for front image periodic query""" -front_image_sources = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "frontleft_depth", - "frontright_depth", -] - -"""List of image sources for side image periodic query""" -side_image_sources = [ - "left_fisheye_image", - "right_fisheye_image", - "left_depth", - "right_depth", -] - -"""List of image sources for rear image periodic query""" -rear_image_sources = ["back_fisheye_image", "back_depth"] - """Service name for getting pointcloud of VLP16 connected to Spot Core""" VELODYNE_SERVICE_NAME = "velodyne-point-cloud" """List of point cloud sources""" point_cloud_sources = ["velodyne-point-cloud"] -"""List of image sources for hand image periodic query""" -hand_image_sources = [ +"""List of hand image sources for asynchronous periodic query""" +HAND_IMAGE_SOURCES = [ "hand_image", "hand_depth", "hand_color_image", "hand_depth_in_hand_color_frame", ] -# TODO: Missing Hand images +"""List of body image sources for periodic query""" CAMERA_IMAGE_SOURCES = [ "frontleft_fisheye_image", "frontright_fisheye_image", diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index d8444a6e..ffdcb940 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,9 +1,7 @@ import typing import logging -from bosdyn.client.async_tasks import AsyncPeriodicQuery from bosdyn.client.robot import Robot -from bosdyn.client import robot_command from bosdyn.client.image import ( ImageClient, build_image_request, @@ -14,32 +12,6 @@ from .spot_config import * -class AsyncImageService(AsyncPeriodicQuery): - """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback, image_requests): - super(AsyncImageService, self).__init__( - "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - self._image_requests = image_requests - - def _start_query(self): - if self._callback: - callback_future = self._client.get_image_async(self._image_requests) - callback_future.add_done_callback(self._callback) - return callback_future - - class SpotImages: def __init__( self, @@ -51,72 +23,9 @@ def __init__( self._robot = robot self._logger = logger self._robot_params = robot_params - self._rates: typing.Dict[str, float] = robot_params["rates"] - self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] self._image_client: ImageClient = robot_clients["image_client"] - self._front_image_requests = [] - for source in front_image_sources: - self._front_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._side_image_requests = [] - for source in side_image_sources: - self._side_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._rear_image_requests = [] - for source in rear_image_sources: - self._rear_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._hand_image_requests = [] - for source in hand_image_sources: - self._hand_image_requests.append( - build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) - ) - - self._front_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("front_image", 0.0)), - self._callbacks.get("front_image", None), - self._front_image_requests, - ) - self._side_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("side_image", 0.0)), - self._callbacks.get("side_image", None), - self._side_image_requests, - ) - self._rear_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("rear_image", 0.0)), - self._callbacks.get("rear_image", None), - self._rear_image_requests, - ) - self._hand_image_task = AsyncImageService( - self._image_client, - self._logger, - max(0.0, self._rates.get("hand_image", 0.0)), - self._callbacks.get("hand_image", None), - self._hand_image_requests, - ) - - self.camera_task_name_to_task_mapping: typing.Dict[str, AsyncImageService] = { - "hand_image": self._hand_image_task, - "side_image": self._side_image_task, - "rear_image": self._rear_image_task, - "front_image": self._front_image_task, - } - ############################################ - # TODO: Sort out double publishing of images self._camera_image_requests = [] for camera_source in CAMERA_IMAGE_SOURCES: self._camera_image_requests.append( @@ -144,40 +53,6 @@ def __init__( ) ) - @property - def front_image_task(self) -> AsyncImageService: - return self._front_image_task - - @property - def side_image_task(self) -> AsyncImageService: - return self._side_image_task - - @property - def rear_image_task(self) -> AsyncImageService: - return self._rear_image_task - - @property - def hand_image_task(self) -> AsyncImageService: - return self._hand_image_task - - def update_image_tasks(self, image_name: str): - """Adds an async tasks to retrieve images from the specified image source""" - task_to_add = self.camera_task_name_to_task_mapping[image_name] - - if task_to_add == self._hand_image_task and not self._robot.has_arm(): - self._logger.warning( - "Robot has no arm, therefore the arm image task can not be added" - ) - return - - if task_to_add in self._async_tasks._tasks: - self._logger.warning( - f"Task {image_name} already in async task list, will not be added again" - ) - return - - self._async_tasks.add_task(self.camera_task_name_to_task_mapping[image_name]) - def get_frontleft_rgb_image(self) -> image_pb2.Image: try: return self._image_client.get_image( diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index ba85d8ca..6273c3da 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -612,15 +612,13 @@ def __init__( self.spot_image = SpotImages( self._robot, self._logger, self._robot_params, self._robot_clients ) - robot_tasks.append(self.spot_image.front_image_task) - robot_tasks.append(self.spot_image.side_image_task) - robot_tasks.append(self.spot_image.rear_image_task) if self._robot.has_arm(): - robot_tasks.append(self._hand_image_task) self._spot_arm = SpotArm( self._robot, self._logger, self._robot_params, self._robot_clients ) + self._hand_image_task = self._spot_arm.hand_image_task + robot_tasks.append(self._hand_image_task) else: self._spot_arm = None @@ -633,7 +631,7 @@ def __init__( self._spot_check = SpotCheck( self._robot, self._logger, self._robot_params, self._robot_clients ) - self._spot_image = SpotImages( + self._spot_images = SpotImages( self._robot, self._logger, self._robot_params, self._robot_clients ) @@ -665,7 +663,7 @@ def frame_prefix(self) -> str: @property def spot_images(self) -> SpotImages: """Return SpotImages instance""" - return self._spot_image + return self._spot_images @property def spot_arm(self) -> SpotArm: @@ -735,25 +733,10 @@ def world_objects(self) -> world_object_pb2.ListWorldObjectResponse: """Return most recent proto from _world_objects_task""" return self.spot_world_objects.async_task.proto - @property - def front_images(self) -> typing.List[image_pb2.ImageResponse]: - """Return latest proto from the _front_image_task""" - return self.spot_image.front_image_task.proto - - @property - def side_images(self) -> typing.List[image_pb2.ImageResponse]: - """Return latest proto from the _side_image_task""" - return self.spot_image.side_image_task.proto - - @property - def rear_images(self) -> typing.List[image_pb2.ImageResponse]: - """Return latest proto from the _rear_image_task""" - return self.spot_image.rear_image_task.proto - @property def hand_images(self) -> typing.List[image_pb2.ImageResponse]: """Return latest proto from the _hand_image_task""" - return self._hand_image_task.proto + return self.spot_arm.hand_image_task.proto @property def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: From f026a3e729154d3e42d656817813e2fb12d36ff7 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 19:27:53 +0000 Subject: [PATCH 03/57] cleanup imports --- spot_wrapper/spot_arm.py | 10 ++-------- spot_wrapper/spot_eap.py | 1 - spot_wrapper/spot_world_objects.py | 1 - spot_wrapper/wrapper.py | 23 +++-------------------- 4 files changed, 5 insertions(+), 30 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 0d75b73f..fce2ae29 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -19,10 +19,6 @@ from bosdyn.api import image_pb2 from google.protobuf.duration_pb2 import Duration - -from spot_msgs.srv import HandPoseRequest -from spot_msgs.srv import ArmForceTrajectoryRequest - from .spot_config import * @@ -297,9 +293,7 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: except Exception as e: return False, "Exception occured during arm movement: " + str(e) - def force_trajectory( - self, data: ArmForceTrajectoryRequest - ) -> typing.Tuple[bool, str]: + def force_trajectory(self, data) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -436,7 +430,7 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: return True, "Opened gripper successfully" - def hand_pose(self, data: HandPoseRequest) -> typing.Tuple[bool, str]: + def hand_pose(self, data) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() if not success: diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py index b3347a75..1d05718d 100644 --- a/spot_wrapper/spot_eap.py +++ b/spot_wrapper/spot_eap.py @@ -6,7 +6,6 @@ from bosdyn.client.robot import Robot from bosdyn.client import robot_command - from .spot_config import * diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py index 067801d2..90bbaa06 100644 --- a/spot_wrapper/spot_world_objects.py +++ b/spot_wrapper/spot_world_objects.py @@ -3,7 +3,6 @@ from bosdyn.client.async_tasks import AsyncPeriodicQuery from bosdyn.client.robot import Robot -from bosdyn.client import robot_command from bosdyn.client.world_object import WorldObjectClient from .spot_config import * diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 6273c3da..df91bfca 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1,58 +1,41 @@ import functools import logging -import math import time import traceback import typing import bosdyn.client.auth -from bosdyn.api import arm_command_pb2 -from bosdyn.api import geometry_pb2 from bosdyn.api import image_pb2 -from bosdyn.api import manipulation_api_pb2 from bosdyn.api import robot_command_pb2 from bosdyn.api import robot_state_pb2 -from bosdyn.api import synchronized_command_pb2 -from bosdyn.api import trajectory_pb2 from bosdyn.api import world_object_pb2 from bosdyn.api import lease_pb2 from bosdyn.api import point_cloud_pb2 from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 -from bosdyn.api.graph_nav import graph_nav_pb2 -from bosdyn.api.graph_nav import map_pb2 -from bosdyn.api.graph_nav import nav_pb2 from bosdyn.client import create_standard_sdk, ResponseError, RpcError from bosdyn.client import frame_helpers from bosdyn.client import math_helpers from bosdyn.client import robot_command from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks -from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock +from bosdyn.client.docking import DockingClient from bosdyn.client.estop import ( EstopClient, EstopEndpoint, EstopKeepAlive, MotorsOnError, ) -from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.client.graph_nav import GraphNavClient from bosdyn.client.map_processing import MapProcessingServiceClient -from bosdyn.client.image import ( - ImageClient, - build_image_request, - UnsupportedPixelFormatRequestedError, -) +from bosdyn.client.image import ImageClient from bosdyn.client.lease import LeaseClient, LeaseKeepAlive from bosdyn.client.manipulation_api_client import ManipulationApiClient -from bosdyn.client.point_cloud import build_pc_request -from bosdyn.client.power import safe_power_off, PowerClient, power_on +from bosdyn.client.power import PowerClient from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder from bosdyn.client.robot_state import RobotStateClient from bosdyn.client.world_object import WorldObjectClient from bosdyn.client.spot_check import SpotCheckClient from bosdyn.geometry import EulerZXY -from bosdyn.util import seconds_to_duration -from google.protobuf.duration_pb2 import Duration from bosdyn.api import basic_command_pb2 from google.protobuf.timestamp_pb2 import Timestamp From 88ae203fd8dec90c3e7bc373ab079c8af9af109b Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 19:36:39 +0000 Subject: [PATCH 04/57] add unit tests for some graph_nav_util functions --- .github/workflows/graph_nav_util_test.yml | 27 +++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 .github/workflows/graph_nav_util_test.yml diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml new file mode 100644 index 00000000..cd6bd556 --- /dev/null +++ b/.github/workflows/graph_nav_util_test.yml @@ -0,0 +1,27 @@ +name: graph_nav_util unit tests + +# Run on every push +on: push + +jobs: + test: + runs-on: ubuntu-latest + defaults: + run: + shell: bash + + steps: + - uses: actions/checkout@v3 + with: + path: $GITHUB_WORKSPACE + + - name: Set up Python + uses: actions/setup-python@v2 + with: + python-version: "3.8" + + - name: Install dependencies + run: pip install -r requirements.txt + + - name: Run tests + run: python3 -m unittest discover $GITHUB_WORKSPACE/spot_wrapper/spot_wrapper/tests From be3507833be1358f7d70ba237c5a25b2ff43f0ab Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:20:35 +0000 Subject: [PATCH 05/57] pip install requirements --- .github/workflows/graph_nav_util_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index cd6bd556..25172e7d 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -21,7 +21,7 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r requirements.txt + run: pip install -r $GITHUB_WORKSPACE/spot_wrapper/requirements.txt - name: Run tests run: python3 -m unittest discover $GITHUB_WORKSPACE/spot_wrapper/spot_wrapper/tests From 86a604fe4584d0902148ad67ecf574035291ca18 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:20:45 +0000 Subject: [PATCH 06/57] typing changes --- spot_wrapper/spot_images.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index ffdcb940..992347e8 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -53,7 +53,7 @@ def __init__( ) ) - def get_frontleft_rgb_image(self) -> image_pb2.Image: + def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ @@ -67,7 +67,7 @@ def get_frontleft_rgb_image(self) -> image_pb2.Image: except UnsupportedPixelFormatRequestedError as e: return None - def get_frontright_rgb_image(self) -> image_pb2.Image: + def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ @@ -81,7 +81,7 @@ def get_frontright_rgb_image(self) -> image_pb2.Image: except UnsupportedPixelFormatRequestedError as e: return None - def get_left_rgb_image(self) -> image_pb2.Image: + def get_left_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ @@ -95,7 +95,7 @@ def get_left_rgb_image(self) -> image_pb2.Image: except UnsupportedPixelFormatRequestedError as e: return None - def get_right_rgb_image(self) -> image_pb2.Image: + def get_right_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ @@ -109,7 +109,7 @@ def get_right_rgb_image(self) -> image_pb2.Image: except UnsupportedPixelFormatRequestedError as e: return None - def get_back_rgb_image(self) -> image_pb2.Image: + def get_back_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( [ From f4e9e43767926c3829a73b57d7438a78d7aa28db Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:22:49 +0000 Subject: [PATCH 07/57] absolute paths --- .github/workflows/graph_nav_util_test.yml | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 25172e7d..5d8f7496 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -11,9 +11,13 @@ jobs: shell: bash steps: - - uses: actions/checkout@v3 + - name: Make directory + run: mkdir -p /ros/spot_wrapper + + - name: Checkout repository + uses: actions/checkout@v3 with: - path: $GITHUB_WORKSPACE + path: /ros/spot_wrapper - name: Set up Python uses: actions/setup-python@v2 @@ -21,7 +25,7 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r $GITHUB_WORKSPACE/spot_wrapper/requirements.txt + run: pip install -r /ros/spot_wrapper/requirements.txt - name: Run tests - run: python3 -m unittest discover $GITHUB_WORKSPACE/spot_wrapper/spot_wrapper/tests + run: python3 -m unittest discover /ros/spot_wrapper/spot_wrapper/tests From 03c59970c9df7efc798f691c37f9d3aec9d22dde Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:23:53 +0000 Subject: [PATCH 08/57] clone to specific directory --- .github/workflows/graph_nav_util_test.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 5d8f7496..281e69a3 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -12,12 +12,12 @@ jobs: steps: - name: Make directory - run: mkdir -p /ros/spot_wrapper + run: mkdir -p ~/spot_wrapper - name: Checkout repository uses: actions/checkout@v3 with: - path: /ros/spot_wrapper + path: ~/spot_wrapper - name: Set up Python uses: actions/setup-python@v2 @@ -25,7 +25,7 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r /ros/spot_wrapper/requirements.txt + run: pip install -r ~/spot_wrapper/requirements.txt - name: Run tests - run: python3 -m unittest discover /ros/spot_wrapper/spot_wrapper/tests + run: python3 -m unittest discover ~/spot_wrapper/spot_wrapper/tests From 89213d8f9f3b4c069cbfcdbc251a872178d80530 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:25:45 +0000 Subject: [PATCH 09/57] check file structure for CI --- .github/workflows/graph_nav_util_test.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 281e69a3..9dedaaf5 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -19,6 +19,9 @@ jobs: with: path: ~/spot_wrapper + - name: Check files + run: ls -l ~/spot_wrapper + - name: Set up Python uses: actions/setup-python@v2 with: From 1ffab673e4c3463d7011baee48aee68db4e95d13 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:27:04 +0000 Subject: [PATCH 10/57] check files --- .github/workflows/graph_nav_util_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 9dedaaf5..c94b937b 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -20,7 +20,7 @@ jobs: path: ~/spot_wrapper - name: Check files - run: ls -l ~/spot_wrapper + run: ls ~/spot_wrapper - name: Set up Python uses: actions/setup-python@v2 From 860c796d038caaa41179f246338b5448206e5d2b Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:29:40 +0000 Subject: [PATCH 11/57] check files --- .github/workflows/graph_nav_util_test.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index c94b937b..7090a1c0 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -17,10 +17,10 @@ jobs: - name: Checkout repository uses: actions/checkout@v3 with: - path: ~/spot_wrapper + path: spot_wrapper - name: Check files - run: ls ~/spot_wrapper + run: ls spot_wrapper - name: Set up Python uses: actions/setup-python@v2 @@ -28,7 +28,7 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r ~/spot_wrapper/requirements.txt + run: pip install -r spot_wrapper/requirements.txt - name: Run tests - run: python3 -m unittest discover ~/spot_wrapper/spot_wrapper/tests + run: python3 -m unittest discover spot_wrapper/spot_wrapper/tests From 0097875fad09372ac62e1ff96a6f1397a0ea5c72 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:31:50 +0000 Subject: [PATCH 12/57] add test script --- setup.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/setup.py b/setup.py index 65b80317..6d9c5646 100644 --- a/setup.py +++ b/setup.py @@ -5,5 +5,8 @@ version="1.0.0", description="Wrapper for Boston Dynamics Spot SDK", packages=["spot_wrapper"], + scripts=[ + "tests/test_graph_nav_util.py", + ], install_requires=["bosdyn-client", "bosdyn-api", "bosdyn-mission", "bosdyn-core"], ) From 5f043537e0093bdd70e508bb8387604767d170a2 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:32:49 +0000 Subject: [PATCH 13/57] install spot wrapper --- .github/workflows/graph_nav_util_test.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 7090a1c0..678288bd 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -28,7 +28,9 @@ jobs: python-version: "3.8" - name: Install dependencies - run: pip install -r spot_wrapper/requirements.txt + run: | + pip install -r spot_wrapper/requirements.txt + pip install -e spot_wrapper - name: Run tests run: python3 -m unittest discover spot_wrapper/spot_wrapper/tests From 7a9811b3b90fde6746f4832451861544a267bf3b Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:33:56 +0000 Subject: [PATCH 14/57] install script --- setup.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/setup.py b/setup.py index 6d9c5646..65b80317 100644 --- a/setup.py +++ b/setup.py @@ -5,8 +5,5 @@ version="1.0.0", description="Wrapper for Boston Dynamics Spot SDK", packages=["spot_wrapper"], - scripts=[ - "tests/test_graph_nav_util.py", - ], install_requires=["bosdyn-client", "bosdyn-api", "bosdyn-mission", "bosdyn-core"], ) From 15addcdddcc6d2b949a7a4d38e46818d5b58d3c6 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:36:40 +0000 Subject: [PATCH 15/57] remove asyncimageservice --- spot_wrapper/wrapper.py | 26 -------------------------- 1 file changed, 26 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index df91bfca..9e7a9c67 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -125,32 +125,6 @@ def _start_query(self): return callback_future -class AsyncImageService(AsyncPeriodicQuery): - """Class to get images at regular intervals. get_image_from_sources_async query sent to the robot at every tick. Callback registered to defined callback function. - - Attributes: - client: The Client to a service on the robot - logger: Logger object - rate: Rate (Hz) to trigger the query - callback: Callback function to call when the results of the query are available - """ - - def __init__(self, client, logger, rate, callback, image_requests): - super(AsyncImageService, self).__init__( - "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) - ) - self._callback = None - if rate > 0.0: - self._callback = callback - self._image_requests = image_requests - - def _start_query(self): - if self._callback: - callback_future = self._client.get_image_async(self._image_requests) - callback_future.add_done_callback(self._callback) - return callback_future - - class AsyncIdle(AsyncPeriodicQuery): """Class to check if the robot is moving, and if not, command a stand with the set mobility parameters From 3fa6430ea9dffedb7c386540efd2096777b4e14a Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Wed, 19 Apr 2023 09:40:40 +0000 Subject: [PATCH 16/57] use robot_params to share state variables --- spot_wrapper/wrapper.py | 64 +++++++++++++++++++++-------------------- 1 file changed, 33 insertions(+), 31 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9e7a9c67..39e79db0 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -149,24 +149,24 @@ def _start_query(self): status = ( response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status ) - self._spot_wrapper._is_sitting = False + self._spot_wrapper._robot_params["is_sitting"] = False if status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING: - self._spot_wrapper._is_standing = True + self._spot_wrapper._robot_params["is_standing"] = True self._spot_wrapper._last_stand_command = None elif ( status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS ): - self._spot_wrapper._is_standing = False + self._spot_wrapper._robot_params["is_standing"] = False else: self._logger.warning("Stand command in unknown state") - self._spot_wrapper._is_standing = False + self._spot_wrapper._robot_params["is_standing"] = False except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_stand_command = None if self._spot_wrapper._last_sit_command != None: try: - self._spot_wrapper._is_standing = False + self._spot_wrapper._robot_params["is_standing"] = False response = self._client.robot_command_feedback( self._spot_wrapper._last_sit_command ) @@ -174,10 +174,10 @@ def _start_query(self): response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status == basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING ): - self._spot_wrapper._is_sitting = True + self._spot_wrapper._robot_params["is_sitting"] = True self._spot_wrapper._last_sit_command = None else: - self._spot_wrapper._is_sitting = False + self._spot_wrapper._robot_params["is_sitting"] = False except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_sit_command = None @@ -209,7 +209,7 @@ def _start_query(self): and not self._spot_wrapper._last_trajectory_command_precise ) ): - self._spot_wrapper._at_goal = True + self._spot_wrapper._robot_params["at_goal"] = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None elif ( @@ -222,7 +222,7 @@ def _start_query(self): == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL ): is_moving = True - self._spot_wrapper._near_goal = True + self._spot_wrapper._robot_params["near_goal"] = True elif ( status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN @@ -240,14 +240,14 @@ def _start_query(self): self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper._last_trajectory_command = None - self._spot_wrapper._is_moving = is_moving + self._spot_wrapper._robot_params["is_moving"] = is_moving # We must check if any command currently has a non-None value for its id. If we don't do this, this stand # command can cause other commands to be interrupted before they get to start if ( - self._spot_wrapper.is_standing + self._spot_wrapper._robot_params["is_standing"] and self._spot_wrapper._continually_try_stand - and not self._spot_wrapper.is_moving + and not self._spot_wrapper._robot_params["is_moving"] and self._spot_wrapper._last_trajectory_command is not None and self._spot_wrapper._last_stand_command is not None and self._spot_wrapper._last_velocity_command_time is not None @@ -399,15 +399,6 @@ def __init__( self._last_trajectory_command_precise = None self._last_velocity_command_time = None self._last_docking_command = None - self._robot_params = { - "is_standing": False, - "is_sitting": True, - "is_moving": False, - "robot_id": None, - "estop_timeout": self._estop_timeout, - "rates": self._rates, - "callbacks": self._callbacks, - } try: self._sdk = create_standard_sdk(SPOT_CLIENT_NAME) @@ -566,6 +557,17 @@ def __init__( ] # Create component objects for different functionality + self._robot_params = { + "is_standing": self._is_standing, + "is_sitting": self._is_sitting, + "is_moving": self._is_moving, + "at_goal": self._at_goal, + "near_goal": self._near_goal, + "robot_id": self._robot_id, + "estop_timeout": self._estop_timeout, + "rates": self._rates, + "callbacks": self._callbacks, + } self.spot_image = SpotImages( self._robot, self._logger, self._robot_params, self._robot_clients ) @@ -668,7 +670,7 @@ def is_valid(self) -> bool: @property def id(self) -> str: """Return robot's ID""" - return self._robot_id + return self._robot_params["robot_id"] @property def robot_state(self) -> robot_state_pb2.RobotState: @@ -703,25 +705,25 @@ def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: @property def is_standing(self) -> bool: """Return boolean of standing state""" - return self._is_standing + return self._robot_params["is_standing"] @property def is_sitting(self) -> bool: """Return boolean of standing state""" - return self._is_sitting + return self._robot_params["is_sitting"] @property def is_moving(self) -> bool: """Return boolean of walking state""" - return self._is_moving + return self._robot_params["is_moving"] @property def near_goal(self) -> bool: - return self._near_goal + return self._robot_params["near_goal"] @property def at_goal(self) -> bool: - return self._at_goal + return self._robot_params["at_goal"] def is_estopped(self, timeout=None) -> bool: return self._robot.is_estopped(timeout=timeout) @@ -772,7 +774,7 @@ def claim(self): return True, "We already claimed the lease" try: - self._robot_id = self._robot.get_id() + self._robot_params["robot_id"] = self._robot.get_id() self.getLease() if self._start_estop and not self.check_is_powered_on(): # If we are requested to start the estop, and the robot is not already powered on, then we reset the @@ -965,7 +967,7 @@ def battery_change_pose(self, dir_hint: int = 1): Args: dir_hint: 1 rolls to the right side of the robot, 2 to the left """ - if self._is_sitting: + if self._robot_params["is_sitting"]: response = self._robot_command( RobotCommandBuilder.battery_change_pose_command(dir_hint) ) @@ -1072,8 +1074,8 @@ def trajectory_cmd( """ if mobility_params is None: mobility_params = self._mobility_params - self._at_goal = False - self._near_goal = False + self._robot_params["at_goal"] = False + self._robot_params["near_goal"] = False self._last_trajectory_command_precise = precise_position self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration From 55c68a1b575599a56ee39f5134819c86f81288c1 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Wed, 19 Apr 2023 23:41:38 +0200 Subject: [PATCH 17/57] Pytest replacement (#1) * use pytest instead * add pytest dependency * fail test on purpose * remove failing test --- .github/workflows/graph_nav_util_test.yml | 19 ++- requirements.txt | 1 + spot_wrapper/tests/test_graph_nav_util.py | 164 +++++++++------------- 3 files changed, 82 insertions(+), 102 deletions(-) diff --git a/.github/workflows/graph_nav_util_test.yml b/.github/workflows/graph_nav_util_test.yml index 678288bd..08636ac9 100644 --- a/.github/workflows/graph_nav_util_test.yml +++ b/.github/workflows/graph_nav_util_test.yml @@ -33,4 +33,21 @@ jobs: pip install -e spot_wrapper - name: Run tests - run: python3 -m unittest discover spot_wrapper/spot_wrapper/tests + run: pytest --junit-xml=test-results.xml + + - name: Surface failing tests + if: always() + uses: pmeier/pytest-summary-gha@main + with: + # A list of JUnit XML files, directories containing the former, and wildcard patterns to process. + path: test-results.xml + + # Add a summary of the results at the top of the report + summary: true + + # Select which results should be included in the report. + # Follows the same syntax as `pytest -r` + display-options: fEX + + # Fail the workflow if no JUnit XML was found. + fail-on-empty: true diff --git a/requirements.txt b/requirements.txt index 200f1390..76c87a8b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,4 @@ bosdyn_core==3.2.2.post1 protobuf==4.22.1 setuptools==45.2.0 +pytest==7.3.1 \ No newline at end of file diff --git a/spot_wrapper/tests/test_graph_nav_util.py b/spot_wrapper/tests/test_graph_nav_util.py index 1dc78556..18ba04d2 100755 --- a/spot_wrapper/tests/test_graph_nav_util.py +++ b/spot_wrapper/tests/test_graph_nav_util.py @@ -1,9 +1,5 @@ #!/usr/bin/env python3 -PKG = "graph_nav_util" -NAME = "test_graph_nav_util" -SUITE = "test_graph_nav_util.TestSuiteGraphNavUtil" - -import unittest +import pytest import logging from bosdyn.api.graph_nav import map_pb2 @@ -20,95 +16,93 @@ def __init__(self) -> None: graph_nav_util = MockSpotGraphNav() -class TestGraphNavUtilShortCode(unittest.TestCase): +class TestGraphNavUtilShortCode: def test_id_to_short_code(self): - self.assertEqual( - graph_nav_util._id_to_short_code("ebony-pug-mUzxLNq.TkGlVIxga+UKAQ=="), "ep" + assert ( + graph_nav_util._id_to_short_code("ebony-pug-mUzxLNq.TkGlVIxga+UKAQ==") + == "ep" ) - self.assertEqual( - graph_nav_util._id_to_short_code("erose-simian-sug9xpxhCxgft7Mtbhr98A=="), - "es", + assert ( + graph_nav_util._id_to_short_code("erose-simian-sug9xpxhCxgft7Mtbhr98A==") + == "es" ) -class TestGraphNavUtilFindUniqueWaypointId(unittest.TestCase): - def setUp(self): +class TestGraphNavUtilFindUniqueWaypointId: + def test_short_code(self): + # Set up self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) self.graph = map_pb2.Graph() self.name_to_id = {"ABCDE": "Node1"} - - def test_short_code(self): # Test normal short code - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "AC", self.graph, self.name_to_id, self.logger - ), - "AC", - "AC!=AC, normal short code", + ) + == "AC" ) # Test annotation name that is known - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "ABCDE", self.graph, self.name_to_id, self.logger - ), - "Node1", - "ABCDE!=Node1, known annotation name", + ) + == "Node1" ) # Test annotation name that is unknown - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "ABCDEF", self.graph, self.name_to_id, self.logger - ), - "ABCDEF", - "ABCDEF!=ABCDEF, unknown annotation name", + ) + == "ABCDEF" ) def test_short_code_with_graph(self): + # Set up + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + self.graph = map_pb2.Graph() + self.name_to_id = {"ABCDE": "Node1"} + # Test short code that is in graph self.graph.waypoints.add(id="AB-CD-EF") - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "AC", self.graph, self.name_to_id, self.logger - ), - "AB-CD-EF", - "AC!=AB-CD-EF, short code in graph", + ) + == "AB-CD-EF" ) # Test short code that is not in graph - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "AD", self.graph, self.name_to_id, self.logger - ), - "AD", - "AD!=AD, short code not in graph", + ) + == "AD" ) # Test multiple waypoints with the same short code self.graph.waypoints.add(id="AB-CD-EF-1") - self.assertEqual( + assert ( graph_nav_util._find_unique_waypoint_id( "AC", self.graph, self.name_to_id, self.logger - ), - "AC", - "AC!=AC, multiple waypoints with same short code", + ) + == "AC" ) -class TestGraphNavUtilUpdateWaypointsEdges(unittest.TestCase): - def setUp(self): +class TestGraphNavUtilUpdateWaypointsEdges: + def test_empty_graph(self): self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) - def test_empty_graph(self): # Test empty graph self.graph = map_pb2.Graph() self.localization_id = "" graph_nav_util._update_waypoints_and_edges( self.graph, self.localization_id, self.logger ) - self.assertEqual( - len(self.graph.waypoints), 0, "Empty graph should have 0 waypoints" - ) - self.assertEqual(len(self.graph.edges), 0, "Empty graph should have 0 edges") + assert len(self.graph.waypoints) == 0 + assert len(self.graph.edges) == 0 def test_one_waypoint(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + # Test graph with 1 waypoint self.localization_id = "" self.graph = map_pb2.Graph() @@ -121,19 +115,15 @@ def test_one_waypoint(self): self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( self.graph, self.localization_id, self.logger ) - self.assertEqual( - len(self.graph.waypoints), 1, "Graph with 1 waypoint should have 1 waypoint" - ) - self.assertEqual( - len(self.graph.edges), 0, "Graph with 1 waypoint should have 0 edges" - ) - self.assertEqual(len(self.edges), 0, "Edges should have 0 entries") - self.assertEqual(len(self.name_to_id), 1, "Name to id should have 1 entry") - self.assertEqual( - self.name_to_id["Node1"], "ABCDE", "Name to id should have 1 entry" - ) + assert len(self.graph.waypoints) == 1 + assert len(self.graph.edges) == 0 + assert len(self.edges) == 0 + assert len(self.name_to_id) == 1 + assert self.name_to_id["Node1"] == "ABCDE" def test_two_waypoints_with_edge(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + # Test graph with 2 waypoints and an edge between them self.localization_id = "" self.graph = map_pb2.Graph() @@ -154,23 +144,17 @@ def test_two_waypoints_with_edge(self): self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( self.graph, self.localization_id, self.logger ) - self.assertEqual( - len(self.graph.waypoints), - 2, - "Graph with 2 waypoints should have 2 waypoints", - ) - self.assertEqual( - len(self.graph.edges), 1, "Graph with 2 waypoints should have 1 edge" - ) - self.assertEqual(len(self.edges), 1, "Edges should have 1 entry") - self.assertEqual( - self.edges["DE"][0], "ABCDE", "Edges should point to the correct waypoint" - ) - self.assertEqual(len(self.name_to_id), 2, "Name to id should have 2 entries") - self.assertEqual(self.name_to_id["Node1"], "ABCDE", "Name to id entry, ABCDE") - self.assertEqual(self.name_to_id["Node2"], "DE", "Name to id entry, DE") + assert len(self.graph.waypoints) == 2 + assert len(self.graph.edges) == 1 + assert len(self.edges) == 1 + assert self.edges["DE"][0] == "ABCDE" + assert len(self.name_to_id) == 2 + assert self.name_to_id["Node1"] == "ABCDE" + assert self.name_to_id["Node2"] == "DE" def test_two_waypoints_with_edge_and_localization(self): + self.logger = logging.Logger("test_graph_nav_util", level=logging.INFO) + # Test graph with 2 waypoints and an edge between them. Mainly affects the pretty print. self.localization_id = "ABCDE" self.graph = map_pb2.Graph() @@ -191,32 +175,10 @@ def test_two_waypoints_with_edge_and_localization(self): self.name_to_id, self.edges = graph_nav_util._update_waypoints_and_edges( self.graph, self.localization_id, self.logger ) - self.assertEqual( - len(self.graph.waypoints), - 2, - "Graph with 2 waypoints should have 2 waypoints", - ) - self.assertEqual( - len(self.graph.edges), 1, "Graph with 2 waypoints should have 1 edge" - ) - self.assertEqual(len(self.edges), 1, "Edges should have 1 entry") - self.assertEqual( - self.edges["DE"][0], "ABCDE", "Edges should point to the correct waypoint" - ) - self.assertEqual(len(self.name_to_id), 2, "Name to id should have 2 entries") - self.assertEqual(self.name_to_id["Node1"], "ABCDE", "Name to id entry, ABCDE") - self.assertEqual(self.name_to_id["Node2"], "DE", "Name to id entry, DE") - - -class TestSuiteGraphNavUtil(unittest.TestSuite): - def __init__(self) -> None: - super(TestSuiteGraphNavUtil, self).__init__() - - self.loader = unittest.TestLoader() - self.addTest(self.loader.loadTestsFromTestCase(TestGraphNavUtilShortCode)) - self.addTest( - self.loader.loadTestsFromTestCase(TestGraphNavUtilFindUniqueWaypointId) - ) - self.addTest( - self.loader.loadTestsFromTestCase(TestGraphNavUtilUpdateWaypointsEdges) - ) + assert len(self.graph.waypoints) == 2 + assert len(self.graph.edges) == 1 + assert len(self.edges) == 1 + assert self.edges["DE"][0] == "ABCDE" + assert len(self.name_to_id) == 2 + assert self.name_to_id["Node1"] == "ABCDE" + assert self.name_to_id["Node2"] == "DE" From cf262e412632aeb0b8a89a21e676b1d12377b71c Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Thu, 20 Apr 2023 07:17:37 +0000 Subject: [PATCH 18/57] replace print() with self._logger.error() --- spot_wrapper/wrapper.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 39e79db0..abec1eb2 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -787,7 +787,7 @@ def claim(self): self._logger.error("Failed to initialize robot communication: %s", err) return False, str(err) except Exception as err: - print(traceback.format_exc(), flush=True) + self._logger.error(traceback.format_exc(), flush=True) return False, str(err) def updateTasks(self): @@ -795,7 +795,7 @@ def updateTasks(self): try: self._async_tasks.update() except Exception as e: - print(f"Update tasks failed with error: {str(e)}") + self._logger.error(f"Update tasks failed with error: {str(e)}") def resetEStop(self): """Get keepalive for eStop""" From 4928997bd682d374ebf435d595ee7891c6379dea Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Fri, 21 Apr 2023 13:18:21 +0000 Subject: [PATCH 19/57] image publishing works well --- spot_wrapper/spot_images.py | 29 ++++++++++++++--------------- spot_wrapper/wrapper.py | 4 +++- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 992347e8..b2291601 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -31,9 +31,7 @@ def __init__( self._camera_image_requests.append( build_image_request( camera_source, - image_format=image_pb2.Image.FORMAT_JPEG, - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW, ) ) @@ -41,7 +39,7 @@ def __init__( for camera_source in DEPTH_IMAGE_SOURCES: self._depth_image_requests.append( build_image_request( - camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + camera_source, image_format=image_pb2.Image.FORMAT_RAW ) ) @@ -49,7 +47,7 @@ def __init__( for camera_source in DEPTH_REGISTERED_IMAGE_SOURCES: self._depth_registered_image_requests.append( build_image_request( - camera_source, pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + camera_source, image_format=image_pb2.Image.FORMAT_RAW ) ) @@ -59,12 +57,12 @@ def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "frontleft_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: @@ -73,12 +71,12 @@ def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "frontright_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_left_rgb_image(self) -> image_pb2.ImageResponse: @@ -87,12 +85,12 @@ def get_left_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "left_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_right_rgb_image(self) -> image_pb2.ImageResponse: @@ -101,12 +99,12 @@ def get_right_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "right_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_back_rgb_image(self) -> image_pb2.ImageResponse: @@ -115,12 +113,12 @@ def get_back_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "back_fisheye_image", - pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, - quality_percent=50, + image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None def get_images( @@ -129,6 +127,7 @@ def get_images( try: image_responses = self._image_client.get_image(image_requests) except UnsupportedPixelFormatRequestedError as e: + self._logger.error(e) return None return ImageBundle( frontleft=image_responses[0], diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 39e79db0..8363f40c 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -501,6 +501,7 @@ def __init__( "spot_check_client": self._spot_check_client, "robot_command_method": self._robot_command, "world_objects_client": self._world_objects_client, + "manipulation_client": self._manipulation_client, } if self._point_cloud_client: self._robot_clients["point_cloud_client"] = self._point_cloud_client @@ -787,6 +788,7 @@ def claim(self): self._logger.error("Failed to initialize robot communication: %s", err) return False, str(err) except Exception as err: + self._logger.error("Failed to claim lease: %s", err) print(traceback.format_exc(), flush=True) return False, str(err) @@ -795,7 +797,7 @@ def updateTasks(self): try: self._async_tasks.update() except Exception as e: - print(f"Update tasks failed with error: {str(e)}") + self._logger.error(f"Update tasks failed with error: {str(e)}") def resetEStop(self): """Get keepalive for eStop""" From 0c2bb45922dfc7c79505a83ef97d26461fe320bb Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Fri, 21 Apr 2023 13:22:56 +0000 Subject: [PATCH 20/57] moved SPOT_CLIENT_NAME --- spot_wrapper/wrapper.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 8363f40c..12d72676 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -765,12 +765,12 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: return rtime - def claim(self): + def claim(self) -> typing.List[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" for resource in self.lease: if ( resource.resource == "all-leases" - and self.SPOT_CLIENT_NAME in resource.lease_owner.client_name + and SPOT_CLIENT_NAME in resource.lease_owner.client_name ): return True, "We already claimed the lease" @@ -802,7 +802,7 @@ def updateTasks(self): def resetEStop(self): """Get keepalive for eStop""" self._estop_endpoint = EstopEndpoint( - self._estop_client, self.SPOT_CLIENT_NAME, self._estop_timeout + self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout ) self._estop_endpoint.force_simple_setup() # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) From 6779aa5584221b12dfb5924403410f38f218a5c8 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Fri, 21 Apr 2023 13:27:01 +0000 Subject: [PATCH 21/57] static typing for claim() --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 12d72676..561c1459 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -765,7 +765,7 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: return rtime - def claim(self) -> typing.List[bool, str]: + def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" for resource in self.lease: if ( From 4881d38d63935da2f360713ba6a67f120226f2ad Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Fri, 21 Apr 2023 14:36:31 +0000 Subject: [PATCH 22/57] black formatting --- spot_wrapper/spot_images.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index b2291601..6012e543 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -57,7 +57,7 @@ def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "frontleft_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + image_format=image_pb2.Image.FORMAT_RAW, ) ] )[0] @@ -71,7 +71,7 @@ def get_frontright_rgb_image(self) -> image_pb2.ImageResponse: [ build_image_request( "frontright_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + image_format=image_pb2.Image.FORMAT_RAW, ) ] )[0] @@ -84,8 +84,7 @@ def get_left_rgb_image(self) -> image_pb2.ImageResponse: return self._image_client.get_image( [ build_image_request( - "left_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + "left_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] @@ -98,8 +97,7 @@ def get_right_rgb_image(self) -> image_pb2.ImageResponse: return self._image_client.get_image( [ build_image_request( - "right_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + "right_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] @@ -112,8 +110,7 @@ def get_back_rgb_image(self) -> image_pb2.ImageResponse: return self._image_client.get_image( [ build_image_request( - "back_fisheye_image", - image_format=image_pb2.Image.FORMAT_RAW + "back_fisheye_image", image_format=image_pb2.Image.FORMAT_RAW ) ] )[0] From 392da898d027fc716a17f1cd7ff487183cc192cb Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Sat, 22 Apr 2023 18:43:43 +0000 Subject: [PATCH 23/57] comments and passing error feedback --- spot_wrapper/spot_arm.py | 15 +++---- spot_wrapper/spot_check.py | 73 +++++++++------------------------- spot_wrapper/spot_graph_nav.py | 69 ++++++++------------------------ spot_wrapper/wrapper.py | 8 ++-- 4 files changed, 47 insertions(+), 118 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index fce2ae29..843a13fb 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -58,13 +58,14 @@ def __init__( ): """ Constructor for SpotArm class. - :param robot: Robot object - :param logger: Logger object - :param robot_params: Dictionary of robot parameters - - robot_params['is_standing']: True if robot is standing, False otherwise - :param robot_clients: Dictionary of robot clients - - robot_clients['robot_command_client']: RobotCommandClient object - - robot_clients['robot_command_method']: RobotCommand method + Args: + robot: Robot object + logger: Logger object + robot_params: Dictionary of robot parameters + - robot_params['is_standing']: True if robot is standing, False otherwise + robot_clients: Dictionary of robot clients + - robot_clients['robot_command_client']: RobotCommandClient object + - robot_clients['robot_command_method']: RobotCommand method """ self._robot = robot self._logger = logger diff --git a/spot_wrapper/spot_check.py b/spot_wrapper/spot_check.py index 992d3aa3..ddeab131 100644 --- a/spot_wrapper/spot_check.py +++ b/spot_wrapper/spot_check.py @@ -46,63 +46,28 @@ def _feedback_error_check( # Save results from Spot Check self._spot_check_resp = resp + errorcode_mapping = { + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_UNEXPECTED_POWER_CHANGE: "Unexpected power change", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_IMU_CHECK: "Robot body is not flat on the ground", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_NOT_SITTING: "Robot body is not close to a sitting pose", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_LOADCELL_TIMEOUT: "Timeout during loadcell calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_ON_FAILURE: "Error enabling motor power", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ENDSTOP_TIMEOUT: "Timeout during endstop calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FAILED_STAND: "Robot failed to stand", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_CAMERA_TIMEOUT: "Timeout during camera check", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GROUND_CHECK: "Flat ground check failed", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_OFF_FAILURE: "Robot failed to power off", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_REVERT_FAILURE: "Robot failed to revert calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FGKC_FAILURE: "Robot failed to do flat ground kinematic calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GRIPPER_CAL_TIMEOUT: "Timeout during gripper calibration", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ARM_CHECK_COLLISION: "Arm motion would cause collisions", + spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ARM_CHECK_TIMEOUT: "Timeout during arm joint check", + } # Check for common errors if resp.header.error.code in (2, 3): return False, str(resp.header.error.message) - - # Check for other errors - if ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_UNEXPECTED_POWER_CHANGE - ): - return False, "Unexpected power change" - elif ( - resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_IMU_CHECK - ): - return False, "Robot body is not flat on the ground" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_INIT_NOT_SITTING - ): - return False, "Robot body is not close to a sitting pose" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_LOADCELL_TIMEOUT - ): - return False, "Timeout during loadcell calibration" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_ON_FAILURE - ): - return False, "Error enabling motor power" - elif ( - resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_ENDSTOP_TIMEOUT - ): - return False, "Timeout during endstop calibration" - elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FAILED_STAND: - return False, "Robot failed to stand" - elif ( - resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_CAMERA_TIMEOUT - ): - return False, "Timeout during camera check" - elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GROUND_CHECK: - return False, "Flat ground check failed" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_POWER_OFF_FAILURE - ): - return False, "Robot failed to power off" - elif ( - resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_REVERT_FAILURE - ): - return False, "Robot failed to revert calibration" - elif resp.error == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_FGKC_FAILURE: - return False, "Robot failed to do flat ground kinematic calibration" - elif ( - resp.error - == spot_check_pb2.SpotCheckFeedbackResponse.ERROR_GRIPPER_CAL_TIMEOUT - ): - return False, "Timeout during gripper calibration" + if resp.error > 1: + return False, errorcode_mapping[resp.error] return True, "Successfully ran Spot Check" diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 305dfa33..daad4404 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -139,7 +139,7 @@ def navigation_close_loops( def optmize_anchoring(self) -> typing.Tuple[bool, str]: return self._optimize_anchoring() - ## Copied from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py + ## Copied from https://github.com/boston-dynamics/spot-sdk/blob/master/python/examples/graph_nav_command_line/recording_command_line.py and https://github.com/boston-dynamics/spot-sdk/blob/master/python/examples/graph_nav_command_line/graph_nav_command_line.py with minor modifications # Copyright (c) 2020 Boston Dynamics, Inc. All rights reserved. # # Downloading, reproducing, distributing or otherwise using the SDK Software @@ -610,58 +610,21 @@ def _optimize_anchoring(self, *args): return True, "Successfully optimized anchoring." else: self._logger.error("Error optimizing {}".format(response)) - if ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS - ): - return False, "Missing waypoint snapshots." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAPH - ): - return False, "Invalid graph." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_OPTIMIZATION_FAILURE - ): - return False, "Optimization failure." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_PARAMS - ): - return False, "Invalid parameters." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_CONSTRAINT_VIOLATION - ): - return False, "Constraint violation." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_ITERATIONS - ): - return False, "Max iterations, timeout." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_TIME - ): - return False, "Max time reached, timeout." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_HINTS - ): - return False, "Invalid hints." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING - ): - return False, "Map modified during processing." - elif ( - response.status - == map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT - ): - return False, "Invalid gravity alignment." - else: - return False, "Unknown error during anchoring optimization." + status_mapping = { + map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: "Missing waypoint snapshots.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAPH: "Invalid graph.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_OPTIMIZATION_FAILURE: "Optimization failure.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_PARAMS: "Invalid parameters.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_CONSTRAINT_VIOLATION: "Constraint violation.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_ITERATIONS: "Max iterations, timeout.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_TIME: "Max time reached, timeout.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_HINTS: "Invalid hints.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: "Map modified during processing.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT: "Invalid gravity alignment.", + } + if response.status in status_mapping: + return False, status_mapping[response.status] + return False, "Unknown error during anchoring optimization." def _id_to_short_code(self, id: str): """Convert a unique id to a 2 letter short code.""" diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 5722f641..dc83d99b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -341,8 +341,8 @@ def __init__( logger: logging.Logger, start_estop: bool = True, estop_timeout: float = 9.0, - rates: typing.Dict[str, float] = {}, - callbacks: typing.Dict[str, typing.Callable] = {}, + rates: typing.Optional[typing.Dict[str, float]] = None, + callbacks: typing.Optional[typing.Dict[str, typing.Callable]] = None, use_take_lease: bool = False, get_lease_on_action: bool = False, continually_try_stand: bool = True, @@ -370,8 +370,8 @@ def __init__( self._password = password self._hostname = hostname self._robot_name = robot_name - self._rates = rates - self._callbacks = callbacks + self._rates = rates or {} + self._callbacks = callbacks or {} self._use_take_lease = use_take_lease self._get_lease_on_action = get_lease_on_action self._continually_try_stand = continually_try_stand From 22d108496e0906e798e1324fabe113378cd8e806 Mon Sep 17 00:00:00 2001 From: jeremysee2 <32976023+jeremysee2@users.noreply.github.com> Date: Sat, 22 Apr 2023 18:49:23 +0000 Subject: [PATCH 24/57] remove spot_config --- spot_wrapper/spot_arm.py | 8 ++++- spot_wrapper/spot_config.py | 47 ------------------------------ spot_wrapper/spot_eap.py | 3 +- spot_wrapper/spot_images.py | 27 ++++++++++++++++- spot_wrapper/spot_world_objects.py | 2 -- spot_wrapper/wrapper.py | 7 ++++- 6 files changed, 41 insertions(+), 53 deletions(-) delete mode 100644 spot_wrapper/spot_config.py diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 843a13fb..15e03a4b 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -19,7 +19,13 @@ from bosdyn.api import image_pb2 from google.protobuf.duration_pb2 import Duration -from .spot_config import * +"""List of hand image sources for asynchronous periodic query""" +HAND_IMAGE_SOURCES = [ + "hand_image", + "hand_depth", + "hand_color_image", + "hand_depth_in_hand_color_frame", +] class AsyncImageService(AsyncPeriodicQuery): diff --git a/spot_wrapper/spot_config.py b/spot_wrapper/spot_config.py deleted file mode 100644 index 0cd7c921..00000000 --- a/spot_wrapper/spot_config.py +++ /dev/null @@ -1,47 +0,0 @@ -""" -Global variables used for configuration. These will not change at runtime. -""" -from collections import namedtuple - -SPOT_CLIENT_NAME = "ros_spot" -MAX_COMMAND_DURATION = 1e5 - -"""Service name for getting pointcloud of VLP16 connected to Spot Core""" -VELODYNE_SERVICE_NAME = "velodyne-point-cloud" - -"""List of point cloud sources""" -point_cloud_sources = ["velodyne-point-cloud"] - -"""List of hand image sources for asynchronous periodic query""" -HAND_IMAGE_SOURCES = [ - "hand_image", - "hand_depth", - "hand_color_image", - "hand_depth_in_hand_color_frame", -] - -"""List of body image sources for periodic query""" -CAMERA_IMAGE_SOURCES = [ - "frontleft_fisheye_image", - "frontright_fisheye_image", - "left_fisheye_image", - "right_fisheye_image", - "back_fisheye_image", -] -DEPTH_IMAGE_SOURCES = [ - "frontleft_depth", - "frontright_depth", - "left_depth", - "right_depth", - "back_depth", -] -DEPTH_REGISTERED_IMAGE_SOURCES = [ - "frontleft_depth_in_visual_frame", - "frontright_depth_in_visual_frame", - "right_depth_in_visual_frame", - "left_depth_in_visual_frame", - "back_depth_in_visual_frame", -] -ImageBundle = namedtuple( - "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] -) diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py index 1d05718d..05557655 100644 --- a/spot_wrapper/spot_eap.py +++ b/spot_wrapper/spot_eap.py @@ -6,7 +6,8 @@ from bosdyn.client.robot import Robot from bosdyn.client import robot_command -from .spot_config import * +"""List of point cloud sources""" +point_cloud_sources = ["velodyne-point-cloud"] class AsyncPointCloudService(AsyncPeriodicQuery): diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 6012e543..9d151f29 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,5 +1,6 @@ import typing import logging +from collections import namedtuple from bosdyn.client.robot import Robot from bosdyn.client.image import ( @@ -9,7 +10,31 @@ ) from bosdyn.api import image_pb2 -from .spot_config import * +"""List of body image sources for periodic query""" +CAMERA_IMAGE_SOURCES = [ + "frontleft_fisheye_image", + "frontright_fisheye_image", + "left_fisheye_image", + "right_fisheye_image", + "back_fisheye_image", +] +DEPTH_IMAGE_SOURCES = [ + "frontleft_depth", + "frontright_depth", + "left_depth", + "right_depth", + "back_depth", +] +DEPTH_REGISTERED_IMAGE_SOURCES = [ + "frontleft_depth_in_visual_frame", + "frontright_depth_in_visual_frame", + "right_depth_in_visual_frame", + "left_depth_in_visual_frame", + "back_depth_in_visual_frame", +] +ImageBundle = namedtuple( + "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] +) class SpotImages: diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py index 90bbaa06..da7061a3 100644 --- a/spot_wrapper/spot_world_objects.py +++ b/spot_wrapper/spot_world_objects.py @@ -5,8 +5,6 @@ from bosdyn.client.robot import Robot from bosdyn.client.world_object import WorldObjectClient -from .spot_config import * - class AsyncWorldObjects(AsyncPeriodicQuery): """Class to get world objects. list_world_objects_async query sent to the robot at every tick. Callback registered to defined callback function. diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index dc83d99b..10d20eec 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -47,7 +47,12 @@ from .spot_graph_nav import SpotGraphNav from .spot_check import SpotCheck from .spot_images import SpotImages -from .spot_config import * + +SPOT_CLIENT_NAME = "ros_spot" +MAX_COMMAND_DURATION = 1e5 + +"""Service name for getting pointcloud of VLP16 connected to Spot Core""" +VELODYNE_SERVICE_NAME = "velodyne-point-cloud" class AsyncRobotState(AsyncPeriodicQuery): From 73d88bea581c140836885f423fe733c8e154bfb6 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 30 May 2023 13:55:51 +0100 Subject: [PATCH 25/57] use fstring for short code, move wrench from msg function to class body --- spot_wrapper/spot_arm.py | 29 ++++++++++++++--------------- spot_wrapper/spot_graph_nav.py | 2 +- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 15e03a4b..2a687833 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -38,7 +38,7 @@ class AsyncImageService(AsyncPeriodicQuery): callback: Callback function to call when the results of the query are available """ - def __init__(self, client, logger, rate, callback, image_requests): + def __init__(self, client, logger , rate, callback, image_requests): super(AsyncImageService, self).__init__( "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) ) @@ -237,27 +237,27 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: # Values after unstow are: [0.0, -0.9, 1.8, 0.0, -0.9, 0.0] if abs(joint_targets[0]) > 3.14: msg = "Joint 1 has to be between -3.14 and 3.14" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif joint_targets[1] > 0.4 or joint_targets[1] < -3.13: msg = "Joint 2 has to be between -3.13 and 0.4" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif joint_targets[2] > 3.14 or joint_targets[2] < 0.0: msg = "Joint 3 has to be between 0.0 and 3.14" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif abs(joint_targets[3]) > 2.79253: msg = "Joint 4 has to be between -2.79253 and 2.79253" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif abs(joint_targets[4]) > 1.8326: msg = "Joint 5 has to be between -1.8326 and 1.8326" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg elif abs(joint_targets[5]) > 2.87: msg = "Joint 6 has to be between -2.87 and 2.87" - self._logger.warn(msg) + self._logger.warning(msg) return False, msg try: success, msg = self.ensure_arm_power_and_stand() @@ -300,6 +300,11 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: except Exception as e: return False, "Exception occured during arm movement: " + str(e) + def create_wrench_from_msg(self, forces, torques): + force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) + torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) + return geometry_pb2.Wrench(force=force, torque=torque) + def force_trajectory(self, data) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() @@ -307,24 +312,18 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: self._logger.info(msg) return False, msg else: - - def create_wrench_from_msg(forces, torques): - force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) - torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) - return geometry_pb2.Wrench(force=force, torque=torque) - # Duration in seconds. traj_duration = data.duration # first point on trajectory - wrench0 = create_wrench_from_msg(data.forces_pt0, data.torques_pt0) + wrench0 = self.create_wrench_from_msg(data.forces_pt0, data.torques_pt0) t0 = seconds_to_duration(0) traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench0, time_since_reference=t0 ) # Second point on the trajectory - wrench1 = create_wrench_from_msg(data.forces_pt1, data.torques_pt1) + wrench1 = self.create_wrench_from_msg(data.forces_pt1, data.torques_pt1) t1 = seconds_to_duration(traj_duration) traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench1, time_since_reference=t1 diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index daad4404..1c39d528 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -630,7 +630,7 @@ def _id_to_short_code(self, id: str): """Convert a unique id to a 2 letter short code.""" tokens = id.split("-") if len(tokens) > 2: - return "%c%c" % (tokens[0][0], tokens[1][0]) + return f"{tokens[0][0]:c}, {tokens[1][0]:c}" return None def _pretty_print_waypoints( From c473eefef7da5b1c75522a6bf441875918829a9c Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 30 May 2023 16:33:54 +0100 Subject: [PATCH 26/57] Add changes from [SW-62] Elements for publishing the hand camera in spot_ros2 (#7) Co-authored-by: Andrew Messing <129519955+amessing-bdai@users.noreply.github.com> --- spot_wrapper/spot_images.py | 81 +++++++++++++++++++++++++++++++------ 1 file changed, 69 insertions(+), 12 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 9d151f29..30a45165 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -35,6 +35,9 @@ ImageBundle = namedtuple( "ImageBundle", ["frontleft", "frontright", "left", "right", "back"] ) +ImageWithHandBundle = namedtuple( + "ImageBundle", ["frontleft", "frontright", "left", "right", "back", "hand"] +) class SpotImages: @@ -76,6 +79,28 @@ def __init__( ) ) + if self._robot.has_arm(): + self._camera_image_requests.append( + build_image_request( + "hand_color_image", + image_format=image_pb2.Image.FORMAT_JPEG, + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ) + self._depth_image_requests.append( + build_image_request( + "hand_depth", + pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, + ) + ) + self._depth_registered_image_requests.append( + build_image_request( + "hand_depth_in_hand_color_frame", + pixel_format=image_pb2.Image.PIXEL_FORMAT_DEPTH_U16, + ) + ) + def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( @@ -143,27 +168,59 @@ def get_back_rgb_image(self) -> image_pb2.ImageResponse: self._logger.error(e) return None + def get_hand_rgb_image(self): + if not self._robot.has_arm(): + return None + try: + return self._image_client.get_image( + [ + build_image_request( + "hand_color_image", + pixel_format=image_pb2.Image.PIXEL_FORMAT_RGB_U8, + quality_percent=50, + ) + ] + )[0] + except UnsupportedPixelFormatRequestedError as e: + return None + def get_images( self, image_requests: typing.List[image_pb2.ImageRequest] - ) -> typing.Optional[ImageBundle]: + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: try: image_responses = self._image_client.get_image(image_requests) except UnsupportedPixelFormatRequestedError as e: self._logger.error(e) return None - return ImageBundle( - frontleft=image_responses[0], - frontright=image_responses[1], - left=image_responses[2], - right=image_responses[3], - back=image_responses[4], - ) - - def get_camera_images(self) -> ImageBundle: + if self._robot.has_arm(): + return ImageWithHandBundle( + frontleft=image_responses[0], + frontright=image_responses[1], + left=image_responses[2], + right=image_responses[3], + back=image_responses[4], + hand=image_responses[5], + ) + else: + return ImageBundle( + frontleft=image_responses[0], + frontright=image_responses[1], + left=image_responses[2], + right=image_responses[3], + back=image_responses[4], + ) + + def get_camera_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: return self.get_images(self._camera_image_requests) - def get_depth_images(self) -> ImageBundle: + def get_depth_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: return self.get_images(self._depth_image_requests) - def get_depth_registered_images(self) -> ImageBundle: + def get_depth_registered_images( + self, + ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: return self.get_images(self._depth_registered_image_requests) From 8963fb7a842205af60babb8bf681ed3d51dfd03b Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 28 Apr 2023 14:15:12 +0100 Subject: [PATCH 27/57] Wrapper for spot cam interaction (#4) * simple led brightness control, only able to set all leds to same value * Add power control, but unclear if it is actually possible to set power for aux and external mic * most basic functional image stream publisher with webrtc * add compositor to handle IR and webrtc stream selection with services Add timestamp for the webrtc images Add compressed version of the webrtc image stream * Add health wrapper, move body of robotToLocalTime out of spot wrapper object robotToLocalTime now takes the timestamp and a robot object, which allows it to be used by the spot cam wrapper as well. * add handler and wrapper for audio commands * update webrtc_client to 3.2 version * add stream quality wrapper and ros handler * initial implementation of ptz wrapper and handler, can list ptzs * ptz handler publishes position and velocity of ptzs, can set position and velocity * add egg info to gitignore --- .gitignore | 1 + spot_wrapper/cam_webrtc_client.py | 137 ++++++ spot_wrapper/cam_wrapper.py | 678 ++++++++++++++++++++++++++++++ spot_wrapper/wrapper.py | 133 ++++-- 4 files changed, 906 insertions(+), 43 deletions(-) create mode 100644 spot_wrapper/cam_webrtc_client.py create mode 100644 spot_wrapper/cam_wrapper.py diff --git a/.gitignore b/.gitignore index c3c89839..27dd47d1 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ *.pyo *.orig build +spot_wrapper.egg-info diff --git a/spot_wrapper/cam_webrtc_client.py b/spot_wrapper/cam_webrtc_client.py new file mode 100644 index 00000000..d79aeb1b --- /dev/null +++ b/spot_wrapper/cam_webrtc_client.py @@ -0,0 +1,137 @@ +# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +import asyncio +import base64 + +import requests +from aiortc import MediaStreamTrack, RTCPeerConnection, RTCSessionDescription +from aiortc.contrib.media import MediaBlackhole + + +class SpotCAMMediaStreamTrack(MediaStreamTrack): + def __init__(self, track, queue): + super().__init__() + self.track = track + self.queue = queue + + async def recv(self): + frame = await self.track.recv() + await self.queue.put(frame) + + return frame + + +class WebRTCClient: + def __init__( + self, + hostname, + sdp_port, + sdp_filename, + cam_ssl_cert, + token, + rtc_config, + media_recorder=None, + recorder_type=None, + ): + self.pc = RTCPeerConnection(configuration=rtc_config) + + self.video_frame_queue = asyncio.Queue() + self.audio_frame_queue = asyncio.Queue() + + self.hostname = hostname + self.token = token + self.sdp_port = sdp_port + self.media_recorder = media_recorder + self.media_black_hole = None + self.recorder_type = recorder_type + self.sdp_filename = sdp_filename + self.cam_ssl_cert = cam_ssl_cert + + def get_bearer_token(self, mock=False): + if mock: + return "token" + return self.token + + def get_sdp_offer_from_spot_cam(self, token): + # then made the sdp request with the token + headers = {"Authorization": f"Bearer {token}"} + server_url = f"https://{self.hostname}:{self.sdp_port}/{self.sdp_filename}" + response = requests.get(server_url, verify=self.cam_ssl_cert, headers=headers) + result = response.json() + return result["id"], base64.b64decode(result["sdp"]).decode() + + def send_sdp_answer_to_spot_cam(self, token, offer_id, sdp_answer): + headers = {"Authorization": f"Bearer {token}"} + server_url = f"https://{self.hostname}:{self.sdp_port}/{self.sdp_filename}" + + payload = {"id": offer_id, "sdp": base64.b64encode(sdp_answer).decode("utf8")} + r = requests.post( + server_url, verify=self.cam_ssl_cert, json=payload, headers=headers + ) + if r.status_code != 200: + raise ValueError(r) + + async def start(self): + # first get a token + try: + token = self.get_bearer_token() + except: + token = self.get_bearer_token(mock=True) + + offer_id, sdp_offer = self.get_sdp_offer_from_spot_cam(token) + + @self.pc.on("icegatheringstatechange") + def _on_ice_gathering_state_change(): + print(f"ICE gathering state changed to {self.pc.iceGatheringState}") + + @self.pc.on("signalingstatechange") + def _on_signaling_state_change(): + print(f"Signaling state changed to: {self.pc.signalingState}") + + @self.pc.on("icecandidate") + def _on_ice_candidate(event): + print(f"Received candidate: {event.candidate}") + + @self.pc.on("iceconnectionstatechange") + async def _on_ice_connection_state_change(): + print(f"ICE connection state changed to: {self.pc.iceConnectionState}") + + if self.pc.iceConnectionState == "checking": + self.send_sdp_answer_to_spot_cam( + token, offer_id, self.pc.localDescription.sdp.encode() + ) + + @self.pc.on("track") + def _on_track(track): + print(f"Received track: {track.kind}") + + if self.media_recorder: + if track.kind == self.recorder_type: + self.media_recorder.addTrack(track) + else: + # We only care about the track we are recording. + self.media_black_hole = MediaBlackhole() + self.media_black_hole.addTrack(track) + loop = asyncio.get_event_loop() + loop.create_task(self.media_black_hole.start()) + else: + if track.kind == "video": + video_track = SpotCAMMediaStreamTrack(track, self.video_frame_queue) + video_track.kind = "video" + self.pc.addTrack(video_track) + + if track.kind == "audio": + self.media_recorder = MediaBlackhole() + self.media_recorder.addTrack(track) + loop = asyncio.get_event_loop() + loop.create_task(self.media_recorder.start()) + + desc = RTCSessionDescription(sdp_offer, "offer") + await self.pc.setRemoteDescription(desc) + + sdp_answer = await self.pc.createAnswer() + await self.pc.setLocalDescription(sdp_answer) diff --git a/spot_wrapper/cam_wrapper.py b/spot_wrapper/cam_wrapper.py new file mode 100644 index 00000000..00460422 --- /dev/null +++ b/spot_wrapper/cam_wrapper.py @@ -0,0 +1,678 @@ +import asyncio +import datetime +import enum +import os.path +import threading +import typing +import wave + +import bosdyn.client +import cv2 +import numpy as np +from aiortc import RTCConfiguration +from bosdyn.client import Robot +from bosdyn.client import spot_cam +from bosdyn.client.spot_cam.compositor import CompositorClient +from bosdyn.client.spot_cam.lighting import LightingClient +from bosdyn.client.spot_cam.power import PowerClient +from bosdyn.client.spot_cam.health import HealthClient +from bosdyn.client.spot_cam.audio import AudioClient +from bosdyn.client.spot_cam.streamquality import StreamQualityClient +from bosdyn.client.spot_cam.ptz import PtzClient +from bosdyn.client.spot_cam.media_log import MediaLogClient +from bosdyn.client.payload import PayloadClient +from bosdyn.api.spot_cam.ptz_pb2 import PtzDescription, PtzVelocity, PtzPosition +from bosdyn.api.spot_cam import audio_pb2 + +from spot_wrapper.cam_webrtc_client import WebRTCClient +from spot_wrapper.wrapper import SpotWrapper + + +class LightingWrapper: + """ + Wrapper for LED brightness interaction + """ + + class LEDPosition(enum.Enum): + """ + Values indicate the position of the specified LED in the brightness list + """ + + REAR_LEFT = 0 + FRONT_LEFT = 1 + FRONT_RIGHT = 2 + REAR_RIGHT = 3 + + def __init__(self, robot: Robot, logger): + self.logger = logger + self.client: LightingClient = robot.ensure_client( + LightingClient.default_service_name + ) + + def set_led_brightness(self, brightness): + """ + Set the brightness of the LEDs to the specified brightness + + Args: + brightness: LEDs will all be set to this brightness, which should be in the range [0, 1]. The value will + be clipped if outside this range. + """ + # Clamp brightness to [0,1] range + brightness = min(max(brightness, 0), 1) + self.client.set_led_brightness([brightness] * 4) + + def get_led_brightness(self) -> typing.List[float]: + """ + Get the brightness of the LEDS + + Returns: + List of floats indicating current brightness of each LED, in the order they are specified in the + LEDPosition enum + """ + return self.client.get_led_brightness() + + +class PowerWrapper: + """ + Wrapper for power interaction + """ + + def __init__(self, robot: Robot, logger): + self.logger = logger + self.client: PowerClient = robot.ensure_client(PowerClient.default_service_name) + + def get_power_status(self): + """ + Get power status for the devices + """ + return self.client.get_power_status() + + def set_power_status( + self, + ptz: typing.Optional[bool] = None, + aux1: typing.Optional[bool] = None, + aux2: typing.Optional[bool] = None, + external_mic: typing.Optional[bool] = None, + ): + """ + Set power status for each of the devices + + Args: + ptz: + aux1: ?? + aux2: ?? + external_mic: + """ + self.client.set_power_status(ptz, aux1, aux2, external_mic) + + def cycle_power( + self, + ptz: typing.Optional[bool] = None, + aux1: typing.Optional[bool] = None, + aux2: typing.Optional[bool] = None, + external_mic: typing.Optional[bool] = None, + ): + """ + Cycle power of the specified devices + + Args: + ptz: + aux1: + aux2: + external_mic: + """ + self.client.cycle_power(ptz, aux1, aux2, external_mic) + + +class CompositorWrapper: + """ + Wrapper for compositor interaction + """ + + def __init__(self, robot: Robot, logger): + self.logger = logger + self.client: CompositorClient = robot.ensure_client( + CompositorClient.default_service_name + ) + + def list_screens(self) -> typing.List[str]: + """ + List the available screens - this includes individual cameras and also panoramic and other stitched images + provided by the camera + + Returns: + List of strings indicating available screens + """ + return [screen.name for screen in self.client.list_screens()] + + def get_visible_cameras(self): + """ + Get the camera data for the camera currently visible on the stream + + Returns: + List of visible camera streams + """ + return self.client.get_visible_cameras() + + def set_screen(self, screen): + """ + Set the screen to be streamed over the network + + Args: + screen: Screen to show + """ + self.client.set_screen(screen) + + def set_ir_colormap(self, colormap, min_temp, max_temp, auto_scale=True): + """ + Set the colormap used for the IR camera + + Args: + colormap: Colormap to use, options are https://dev.bostondynamics.com/protos/bosdyn/api/proto_reference#ircolormap-colormap + min_temp: Minimum temperature on the scale + max_temp: Maximum temperature on the scale + auto_scale: Auto-scales the colormap according to the image. If this is set min_temp and max_temp are + ignored + """ + self.client.set_ir_colormap(colormap, min_temp, max_temp, auto_scale) + + def set_ir_meter_overlay(self, x, y, enable=True): + """ + Set the reticle position on the Spot CAM IR. + + Args: + x: Horizontal coordinate between 0 and 1 + y: Vertical coordinate between 0 and 1 + enable: If true, enable the reticle on the display + """ + self.client.set_ir_meter_overlay(x, y, enable) + + +class HealthWrapper: + """ + Wrapper for health details + """ + + def __init__(self, robot, logger): + self.client: HealthClient = robot.ensure_client( + HealthClient.default_service_name + ) + self.logger = logger + + def get_bit_status( + self, + ) -> typing.Tuple[typing.List[str], typing.List[typing.Tuple[int, str]]]: + """ + Get fault events and degradations + + Returns: + Dictionary + + """ + bit_status = self.client.get_bit_status() + events = [] + for event in bit_status[0]: + events.append(event) + + degradations = [] + for degradation in bit_status[1]: + degradations.append((degradation.type, degradation.description)) + return events, degradations + + def get_temperature(self) -> typing.Tuple[str, float]: + """ + Get temperatures of various components of the camera + + Returns: + Tuple of string and float indicating the component and its temperature in celsius + """ + return [ + (composite.channel_name, composite.temperature / 1000.0) + for composite in self.client.get_temperature() + ] + + # def get_system_log(self): + # """ + # This seems to always time out + # """ + # return self.client.get_system_log() + + +class AudioWrapper: + """ + Wrapper for audio commands on the camera + """ + + def __init__(self, robot, logger): + self.client: AudioClient = robot.ensure_client(AudioClient.default_service_name) + self.logger = logger + + def list_sounds(self) -> typing.List[str]: + """ + List sounds available on the device + + Returns: + List of names of available sounds + """ + return self.client.list_sounds() + + def set_volume(self, percentage): + """ + Set the volume at which sounds should be played + + Args: + percentage: How loud sounds should be from 0 to 100% + """ + self.client.set_volume(percentage) + + def get_volume(self): + """ + Get the current volume at which sounds are played + + Returns: + Current volume as a percentage + """ + return self.client.get_volume() + + def play_sound(self, sound_name, gain=1.0): + """ + Play a sound which is on the device + + Args: + sound_name: Name of the sound to play + gain: Volume gain multiplier + """ + sound = audio_pb2.Sound(name=sound_name) + self.client.play_sound(sound, gain) + + def load_sound(self, sound_file, name): + """ + Load a sound from a wav file and save it with the given name onto the device + Args: + sound_file: Wav file to read from + name: Name to assign to the sound + + Raises: + IOError: If the given file is not a file + wave.Error: If the given file is not a wav file + """ + full_path = os.path.abspath(os.path.expanduser(sound_file)) + print(full_path) + if not os.path.isfile(full_path): + raise IOError(f"Tried to load sound from {full_path} but it is not a file.") + + sound = audio_pb2.Sound(name=name) + + with wave.open(full_path, "rb") as fh: + # Use this to make sure that the file is actually a wav file + pass + + with open(full_path, "rb") as fh: + data = fh.read() + + self.client.load_sound(sound, data) + + def delete_sound(self, name): + """ + Delete a sound from the device + + Args: + name: Name of the sound to delete + """ + self.client.delete_sound(audio_pb2.Sound(name=name)) + + +class StreamQualityWrapper: + """ + Wrapper for stream quality commands + """ + + def __init__(self, robot, logger): + self.client: StreamQualityClient = robot.ensure_client( + StreamQualityClient.default_service_name + ) + self.logger = logger + + def set_stream_params(self, target_bitrate, refresh_interval, idr_interval, awb): + """ + Set image compression and postprocessing parameters + + Note: It is currently not possible to turn off the auto white balance. You will get a crash + + Args: + target_bitrate: Compression level target in bits per second + refresh_interval: How often the whole feed should be refreshed (in frames) + idr_interval: How often an IDR message should be sent (in frames) + awb: Mode for automatic white balance + """ + self.client.set_stream_params(target_bitrate, refresh_interval, idr_interval, 0) + + def get_stream_params(self) -> typing.Dict[str, int]: + """ + Get the current stream quality parameters + + Returns: + Dictionary containing the parameters + """ + params = self.client.get_stream_params() + param_dict = { + "target_bitrate": params.targetbitrate.value, + "refresh_interval": params.refreshinterval.value, + "idr_interval": params.idrinterval.value, + "awb": params.awb.awb, + } + + return param_dict + + def enable_congestion_control(self, enable): + """ + Enable congestion control on the receiver... not sure what this does + + Args: + enable: If true, enable congestion control + """ + self.client.enable_congestion_control(enable) + + +class MediaLogWrapper: + """ + Wrapper for interacting with the media log. And importantly, information about the cameras themselves + """ + + def __init__(self, robot, logger): + self.client: MediaLogClient = robot.ensure_client( + MediaLogClient.default_service_name + ) + self.logger = logger + + def list_cameras(self) -> typing.List: + """ + List the cameras on the spot cam + """ + return self.client.list_cameras() + + +class PTZWrapper: + """ + Wrapper for controlling the PTZ unit + """ + + def __init__(self, robot, logger): + self.client: PtzClient = robot.ensure_client(PtzClient.default_service_name) + self.logger = logger + self.ptzs = {} + descriptions = self.client.list_ptz() + for description in descriptions: + self.ptzs[description.name] = description + + def list_ptz(self) -> typing.Dict[str, typing.Dict]: + """ + List the available ptz units on the device + + Returns: + Dict of descriptions of ptz units + """ + ptzs = [] + + descriptions = self.client.list_ptz() + for ptz_desc in descriptions: + ptzs.append(ptz_desc) + # Also update the internal list of raw ptz definitions + self.ptzs[ptz_desc.name] = ptz_desc + + return ptzs + + def _get_ptz_description(self, name): + """ + Get the bosdyn version of the ptz description + + Args: + name: Get description for this ptz + + Returns: + PtzDescription + """ + if name not in self.ptzs: + self.logger.warn( + f"Tried to retrieve description for ptz {name} but it does not exist." + ) + return None + + return self.ptzs[name] + + def _clamp_value_to_limits(self, value, limits: PtzDescription.Limits): + """ + Clamp the given value to the specified limits. If the limits are unspecified (i.e. both 0), the value is not + clamped + + Args: + value: Value to clamp + limits: PTZ description limit proto + + Returns: + Value clamped to limits + """ + if limits.max.value == 0 and limits.min.value == 0: + # If both max and min are zero, this means the limit is unset. The documentation states that if a limit + # is unset, then all positions are valid. + # https://dev.bostondynamics.com/protos/bosdyn/api/proto_reference#ptzdescription + return value + + return max(min(value, limits.max.value), limits.min.value) + + def _clamp_request_to_limits( + self, ptz_name, pan, tilt, zoom + ) -> typing.Tuple[float, float, float]: + """ + + Args: + ptz_name: Name of the ptz for which the pan, tilt, and zoom should be clamped + + Returns: + Tuple of pan, tilt, zoom, clamped to the limits of the requested ptz + """ + ptz_desc = self._get_ptz_description(ptz_name) + + return ( + self._clamp_value_to_limits(pan, ptz_desc.pan_limit), + self._clamp_value_to_limits(tilt, ptz_desc.tilt_limit), + self._clamp_value_to_limits(zoom, ptz_desc.zoom_limit), + ) + + def get_ptz_position(self, ptz_name) -> PtzPosition: + """ + Get the position of the ptz with the given name + + Args: + ptz_name: Name of the ptz + + Returns: + ptz position proto + """ + return self.client.get_ptz_position(PtzDescription(name=ptz_name)) + + def set_ptz_position(self, ptz_name, pan, tilt, zoom): + """ + Set the position of the specified ptz + + Args: + ptz_name: Name of the ptz + pan: Set the pan to this value in degrees + tilt: Set the tilt to this value in degrees + zoom: Set the zoom to this zoom level + """ + pan, tilt, zoom = self._clamp_request_to_limits(ptz_name, pan, tilt, zoom) + self.client.set_ptz_position( + self._get_ptz_description(ptz_name), pan, tilt, zoom + ) + + def get_ptz_velocity(self, ptz_name) -> PtzVelocity: + """ + Get the velocity of the ptz with the given name + + Args: + ptz_name: Name of the ptz + + Returns: + ptz velocity proto + """ + return self.client.get_ptz_velocity(PtzDescription(name=ptz_name)) + + def set_ptz_velocity(self, ptz_name, pan, tilt, zoom): + """ + Set the velocity of the various axes of the specified ptz + + Args: + ptz_name: Name of the ptz + pan: Set the pan to this value in degrees per second + tilt: Set the tilt to this value in degrees per second + zoom: Set the zoom to this value in zoom level per second + """ + # We do not clamp the velocity to the limits, as it is a rate + self.client.set_ptz_velocity( + self._get_ptz_description(ptz_name), pan, tilt, zoom + ) + + def initialise_lens(self): + """ + Initialises or resets ptz autofocus + """ + self.client.initialize_lens() + + +class ImageStreamWrapper: + """ + A wrapper for the image stream from WebRTC + + Can view the same stream at https://192.168.50.3:31102/h264.sdp.html (depending on the IP of the robot) + + Contains functions adapted from + https://github.com/boston-dynamics/spot-sdk/blob/master/python/examples/spot_cam/webrtc.py + """ + + def __init__( + self, + hostname: str, + robot, + logger, + sdp_port=31102, + sdp_filename="h264.sdp", + cam_ssl_cert_path=None, + ): + """ + Initialise the wrapper + + Args: + hostname: Hostname/IP of the robot + robot: Handle for the robot the camera is on + logger: Logger to use + sdp_port: SDP port of Spot's WebRTC server + sdp_filename: File being streamed from the WebRTC server + cam_ssl_cert_path: Path to the Spot CAM's client cert to check with Spot CAM server + """ + self.shutdown_flag = threading.Event() + self.logger = logger + self.last_image_time = None + self.image_lock = threading.Lock() + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + + config = RTCConfiguration(iceServers=[]) + self.client = WebRTCClient( + hostname, + sdp_port, + sdp_filename, + cam_ssl_cert_path if cam_ssl_cert_path else False, + robot.user_token, + config, + ) + + asyncio.gather( + self.client.start(), + self._process_func(), + self._monitor_shutdown(), + ) + # Put the async loop into a separate thread so we can continue initialisation + self.async_thread = threading.Thread(target=loop.run_forever) + self.async_thread.start() + + async def _monitor_shutdown(self): + while not self.shutdown_flag.is_set(): + await asyncio.sleep(1.0) + + self.logger.info("Image stream wrapper received shutdown flag") + await self.client.pc.close() + asyncio.get_event_loop().stop() + + async def _process_func(self): + while asyncio.get_event_loop().is_running(): + try: + frame = await self.client.video_frame_queue.get() + + pil_image = frame.to_image() + cv_image = np.array(pil_image) + # OpenCV needs BGR + cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) + with self.image_lock: + self.last_image_time = datetime.datetime.now() + self.last_image = cv_image + except Exception as e: + self.logger.error(f"Image stream wrapper exception {e}") + try: + # discard audio frames + while not self.client.audio_frame_queue.empty(): + await self.client.audio_frame_queue.get() + except Exception as e: + self.logger.error( + f"Image stream wrapper exception while discarding audio frames {e}" + ) + + self.shutdown_flag.set() + + +class SpotCamWrapper: + def __init__(self, hostname, username, password, logger): + self._hostname = hostname + self._username = username + self._password = password + self._logger = logger + + # Create robot object and authenticate. + self.sdk = bosdyn.client.create_standard_sdk("Spot CAM Client") + spot_cam.register_all_service_clients(self.sdk) + + self.robot = self.sdk.create_robot(self._hostname) + SpotWrapper.authenticate( + self.robot, self._username, self._password, self._logger + ) + + self.payload_client: PayloadClient = self.robot.ensure_client( + PayloadClient.default_service_name + ) + self.payload_details = None + for payload in self.payload_client.list_payloads(): + if payload.is_enabled and "Spot CAM" in payload.name: + self.payload_details = payload + + if not self.payload_details: + raise SystemError( + "Expected an enabled payload with Spot CAM in the name. This does not appear to exist. " + "Please verify that the spot cam is correctly configured in the payload list on the " + "admin interface" + ) + + self.lighting = LightingWrapper(self.robot, self._logger) + self.power = PowerWrapper(self.robot, self._logger) + self.compositor = CompositorWrapper(self.robot, self._logger) + self.image = ImageStreamWrapper(self._hostname, self.robot, self._logger) + self.health = HealthWrapper(self.robot, self._logger) + self.audio = AudioWrapper(self.robot, self._logger) + self.stream_quality = StreamQualityWrapper(self.robot, self._logger) + self.media_log = MediaLogWrapper(self.robot, self._logger) + self.ptz = PTZWrapper(self.robot, self._logger) + + self._logger.info("Finished setting up spot cam wrapper components") + + def shutdown(self): + self._logger.info("Shutting down Spot CAM wrapper") + self.image.shutdown_flag.set() diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 10d20eec..9851db4b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -55,6 +55,32 @@ VELODYNE_SERVICE_NAME = "velodyne-point-cloud" +def robotToLocalTime(timestamp, robot): + """Takes a timestamp and an estimated skew and return seconds and nano seconds in local time + + Args: + timestamp: google.protobuf.Timestamp + robot: Robot handle to use to get the time skew + Returns: + google.protobuf.Timestamp + """ + + rtime = Timestamp() + + rtime.seconds = timestamp.seconds - robot.time_sync.endpoint.clock_skew.seconds + rtime.nanos = timestamp.nanos - robot.time_sync.endpoint.clock_skew.nanos + if rtime.nanos < 0: + rtime.nanos = rtime.nanos + 1000000000 + rtime.seconds = rtime.seconds - 1 + + # Workaround for timestamps being incomplete + if rtime.seconds < 0: + rtime.seconds = 0 + rtime.nanos = 0 + + return rtime + + class AsyncRobotState(AsyncPeriodicQuery): """Class to get robot state at regular intervals. get_robot_state_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -415,34 +441,12 @@ def __init__( self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) - authenticated = False - while not authenticated: - try: - self._logger.info("Trying to authenticate with robot...") - self._robot.authenticate(self._username, self._password) - self._robot.time_sync.wait_for_sync(10) - self._logger.info("Successfully authenticated.") - authenticated = True - except RpcError as err: - sleep_secs = 15 - self._logger.warning( - "Failed to communicate with robot: {}\nEnsure the robot is powered on and you can " - "ping {}. Robot may still be booting. Will retry in {} seconds".format( - err, self._hostname, sleep_secs - ) - ) - time.sleep(sleep_secs) - except bosdyn.client.auth.InvalidLoginError as err: - self._logger.error("Failed to log in to robot: {}".format(err)) - self._valid = False - return - try: - self._point_cloud_client = self._robot.ensure_client( - VELODYNE_SERVICE_NAME - ) - except Exception as e: - self._point_cloud_client = None - self._logger.warning("No point cloud services are available.") + authenticated = self.authenticate( + self._robot, self._username, self._password, self._logger + ) + if not authenticated: + self._valid = False + return if not self._robot: self._logger.error("Failed to create robot object") @@ -488,6 +492,15 @@ def __init__( self._spot_check_client = self._robot.ensure_client( SpotCheckClient.default_service_name ) + try: + self._point_cloud_client = self._robot.ensure_client( + VELODYNE_SERVICE_NAME + ) + except Exception as e: + self._point_cloud_client = None + self._logger.info("No point cloud services are available.") + + if self._robot.has_arm(): self._manipulation_client = self._robot.ensure_client( ManipulationApiClient.default_service_name @@ -617,6 +630,54 @@ def __init__( self._async_tasks = AsyncTasks(robot_tasks) + self.camera_task_name_to_task_mapping = { + "hand_image": self._hand_image_task, + "side_image": self._side_image_task, + "rear_image": self._rear_image_task, + "front_image": self._front_image_task, + } + + self._robot_id = None + self._lease = None + + @staticmethod + def authenticate(robot, username, password, logger): + """ + Authenticate with a robot through the bosdyn API. A blocking function which will wait until authenticated (if + the robot is still booting) or login fails + + Args: + robot: Robot object which we are authenticating with + username: Username to authenticate with + password: Password for the given username + logger: Logger with which to print messages + + Returns: + + """ + authenticated = False + while not authenticated: + try: + logger.info("Trying to authenticate with robot...") + robot.authenticate(username, password) + robot.time_sync.wait_for_sync(10) + logger.info("Successfully authenticated.") + authenticated = True + except RpcError as err: + sleep_secs = 15 + logger.warn( + "Failed to communicate with robot: {}\nEnsure the robot is powered on and you can " + "ping {}. Robot may still be booting. Will retry in {} seconds".format( + err, robot.address, sleep_secs + ) + ) + time.sleep(sleep_secs) + except bosdyn.client.auth.InvalidLoginError as err: + logger.error("Failed to log in to robot: {}".format(err)) + raise err + + return authenticated + @property def robot_name(self) -> str: return self._robot_name @@ -754,21 +815,7 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: Returns: google.protobuf.Timestamp """ - - rtime = Timestamp() - - rtime.seconds = timestamp.seconds - self.time_skew.seconds - rtime.nanos = timestamp.nanos - self.time_skew.nanos - if rtime.nanos < 0: - rtime.nanos = rtime.nanos + 1000000000 - rtime.seconds = rtime.seconds - 1 - - # Workaround for timestamps being incomplete - if rtime.seconds < 0: - rtime.seconds = 0 - rtime.nanos = 0 - - return rtime + return robotToLocalTime(timestamp, self._robot) def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" From 9b20fe1e997fb2fa4d3036abfb77f13250149602 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 11:31:52 +0100 Subject: [PATCH 28/57] fix bad indent after cherry-pick --- spot_wrapper/wrapper.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9851db4b..b7a7a066 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -630,15 +630,15 @@ def __init__( self._async_tasks = AsyncTasks(robot_tasks) - self.camera_task_name_to_task_mapping = { - "hand_image": self._hand_image_task, - "side_image": self._side_image_task, - "rear_image": self._rear_image_task, - "front_image": self._front_image_task, - } - - self._robot_id = None - self._lease = None + self.camera_task_name_to_task_mapping = { + "hand_image": self._hand_image_task, + "side_image": self._side_image_task, + "rear_image": self._rear_image_task, + "front_image": self._front_image_task, + } + + self._robot_id = None + self._lease = None @staticmethod def authenticate(robot, username, password, logger): From 2ea754a245bf09990663353c01ba7495681ebada Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 11:32:58 +0100 Subject: [PATCH 29/57] fix formatting --- spot_wrapper/spot_arm.py | 2 +- spot_wrapper/wrapper.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 2a687833..eb61736e 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -38,7 +38,7 @@ class AsyncImageService(AsyncPeriodicQuery): callback: Callback function to call when the results of the query are available """ - def __init__(self, client, logger , rate, callback, image_requests): + def __init__(self, client, logger, rate, callback, image_requests): super(AsyncImageService, self).__init__( "robot_image_service", client, logger, period_sec=1.0 / max(rate, 1.0) ) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index b7a7a066..e63b6e13 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -500,7 +500,6 @@ def __init__( self._point_cloud_client = None self._logger.info("No point cloud services are available.") - if self._robot.has_arm(): self._manipulation_client = self._robot.ensure_client( ManipulationApiClient.default_service_name From e913cba3d47062e355deb8e44b65a90c7bf6e7fc Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 11:42:09 +0100 Subject: [PATCH 30/57] fix short code conversion --- spot_wrapper/spot_graph_nav.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 1c39d528..2317ad57 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -630,7 +630,7 @@ def _id_to_short_code(self, id: str): """Convert a unique id to a 2 letter short code.""" tokens = id.split("-") if len(tokens) > 2: - return f"{tokens[0][0]:c}, {tokens[1][0]:c}" + return f"{tokens[0][0]}{tokens[1][0]}" return None def _pretty_print_waypoints( From b5aa38265aa41deb047cdf11f92019c3e4f91755 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 28 Apr 2023 15:00:07 +0100 Subject: [PATCH 31/57] Always include exception message in response strings (#8) --- spot_wrapper/spot_arm.py | 22 +++++++++++----------- spot_wrapper/spot_docking.py | 4 ++-- spot_wrapper/wrapper.py | 18 ++++++++---------- 3 files changed, 21 insertions(+), 23 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index eb61736e..5e50d6a6 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -127,7 +127,7 @@ def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: except Exception as e: return ( False, - "Exception occured while Spot or its arm were trying to power on", + f"Exception occured while Spot or its arm were trying to power on: {e}", ) if not self._robot_params["is_standing"]: @@ -156,7 +156,7 @@ def arm_stow(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while trying to stow" + return False, f"Exception occured while trying to stow: {e}" return True, "Stow arm success" @@ -176,7 +176,7 @@ def arm_unstow(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while trying to unstow" + return False, f"Exception occured while trying to unstow: {e}" return True, "Unstow arm success" @@ -196,7 +196,7 @@ def arm_carry(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while carry mode was issued" + return False, f"Exception occured while carry mode was issued: {e}" return True, "Carry mode success" @@ -298,7 +298,7 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: return True, "Spot Arm moved successfully" except Exception as e: - return False, "Exception occured during arm movement: " + str(e) + return False, f"Exception occured during arm movement: {e}" def create_wrench_from_msg(self, forces, torques): force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) @@ -364,7 +364,7 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: time.sleep(float(traj_duration) + 1.0) except Exception as e: - return False, "Exception occured during arm movement" + return False, f"Exception occured during arm movement: {e}" return True, "Moved arm successfully" @@ -384,7 +384,7 @@ def gripper_open(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while gripper was moving" + return False, f"Exception occured while gripper was moving: {e}" return True, "Open gripper success" @@ -404,7 +404,7 @@ def gripper_close(self) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while gripper was moving" + return False, f"Exception occured while gripper was moving: {e}" return True, "Closed gripper successfully" @@ -432,7 +432,7 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: time.sleep(2.0) except Exception as e: - return False, "Exception occured while gripper was moving" + return False, f"Exception occured while gripper was moving: {e}" return True, "Opened gripper successfully" @@ -501,7 +501,7 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: except Exception as e: return ( False, - "An error occured while trying to move arm \n Exception:" + str(e), + f"An error occured while trying to move arm \n Exception: {e}" ) return True, "Moved arm successfully" @@ -555,6 +555,6 @@ def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): self._robot.logger.info("Finished grasp.") except Exception as e: - return False, "An error occured while trying to grasp from pose" + return False, f"An error occured while trying to grasp from pose {e}" return True, "Grasped successfully" diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py index d1cd14d3..eb6eec7e 100644 --- a/spot_wrapper/spot_docking.py +++ b/spot_wrapper/spot_docking.py @@ -41,7 +41,7 @@ def dock(self, dock_id: int) -> typing.Tuple[bool, str]: self.last_docking_command = None return True, "Success" except Exception as e: - return False, str(e) + return False, f"Exception while trying to dock: {e}" def undock(self, timeout: int = 20) -> typing.Tuple[bool, str]: """Power motors on and undock the robot from the station.""" @@ -52,7 +52,7 @@ def undock(self, timeout: int = 20) -> typing.Tuple[bool, str]: blocking_undock(self._robot, timeout) return True, "Success" except Exception as e: - return False, str(e) + return False, f"Exception while trying to undock: {e}" def get_docking_state(self, **kwargs) -> docking_pb2.DockState: """Get docking state of robot.""" diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index e63b6e13..434140e4 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -870,16 +870,16 @@ def assertEStop(self, severe=True): self._estop_keepalive.settle_then_cut() return True, "Success" - except: - return False, "Error" + except Exception as e: + return False, f"Exception while attempting to estop: {e}" def disengageEStop(self): """Disengages the E-Stop""" try: self._estop_keepalive.allow() return True, "Success" - except: - return False, "Error" + except Exception as e: + return False, f"Exception while attempting to disengage estop {e}" def releaseEStop(self): """Stop eStop keepalive""" @@ -910,7 +910,7 @@ def release(self): self.releaseEStop() return True, "Success" except Exception as e: - return False, str(e) + return False, f"Exception while attempting to release the lease: {e}" def disconnect(self): """Release control of robot as gracefully as posssible.""" @@ -941,9 +941,7 @@ def _robot_command( ) return True, "Success", command_id except Exception as e: - self._logger.error( - "Unable to execute robot command, exception was" + str(e) - ) + self._logger.error(f"Unable to execute robot command: {e}") return False, str(e), None @try_claim @@ -1040,7 +1038,7 @@ def clear_behavior_fault(self, id): ) return True, "Success", rid except Exception as e: - return False, str(e), None + return False, f"Exception while clearing behavior fault: {e}", None @try_claim def power_on(self): @@ -1054,7 +1052,7 @@ def power_on(self): self._logger.info("Powering on") self._robot.power_on() except Exception as e: - return False, str(e) + return False, f"Exception while powering on: {e}" return True, "Success" From 1da2f3a60956e816ab7aa38e7cb90f074f2efb19 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 5 May 2023 13:59:43 +0100 Subject: [PATCH 32/57] fix trajectory status unknown not being reset in trajectory command and async idle (#10) --- spot_wrapper/wrapper.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 434140e4..55098e79 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -243,6 +243,7 @@ def _start_query(self): self._spot_wrapper._robot_params["at_goal"] = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None + self._spot_wrapper._trajectory_status_unknown = False elif ( status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL @@ -1124,6 +1125,7 @@ def trajectory_cmd( """ if mobility_params is None: mobility_params = self._mobility_params + self._trajectory_status_unknown = False self._robot_params["at_goal"] = False self._robot_params["near_goal"] = False self._last_trajectory_command_precise = precise_position From f481dfe2bd6167130bc11d74d5adff54c7e43597 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:12:16 +0100 Subject: [PATCH 33/57] Add changes from [SW-127] Add function to get images by cameras (#11) Co-authored-by: Kaiyu Zheng <125413689+kaiyu-zheng@users.noreply.github.com> --- spot_wrapper/spot_images.py | 143 ++++++++++++++++++++++++++++++++++++ 1 file changed, 143 insertions(+) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 30a45165..c8a7ddca 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,6 +1,8 @@ import typing import logging +from enum import enum from collections import namedtuple +from dataclasses import dataclass, field from bosdyn.client.robot import Robot from bosdyn.client.image import ( @@ -39,6 +41,54 @@ "ImageBundle", ["frontleft", "frontright", "left", "right", "back", "hand"] ) +IMAGE_SOURCES_BY_CAMERA = { + "frontleft": { + "visual": "frontleft_fisheye_image", + "depth": "frontleft_depth", + "depth_registered": "frontleft_depth_in_visual_frame", + }, + "frontright": { + "visual": "frontright_fisheye_image", + "depth": "frontright_depth", + "depth_registered": "frontright_depth_in_visual_frame", + }, + "left": { + "visual": "left_fisheye_image", + "depth": "left_depth", + "depth_registered": "left_depth_in_visual_frame", + }, + "right": { + "visual": "right_fisheye_image", + "depth": "right_depth", + "depth_registered": "right_depth_in_visual_frame", + }, + "back": { + "visual": "back_fisheye_image", + "depth": "back_depth", + "depth_registered": "back_depth_in_visual_frame", + }, + "hand": { + "visual": "hand_color_image", + "depth": "hand_depth", + "depth_registered": "hand_depth_in_color_frame", + }, +} + +IMAGE_TYPES = {"visual", "depth", "depth_registered"} + + +@dataclass(frozen=True, eq=True) +class CameraSource: + camera_name: str + image_types: list[str] + + +@dataclass(frozen=True) +class ImageEntry: + camera_name: str + image_type: str + image_response: image_pb2.ImageResponse + class SpotImages: def __init__( @@ -101,6 +151,31 @@ def __init__( ) ) + # Build image requests by camera + self._image_requests_by_camera = {} + for camera in IMAGE_SOURCES_BY_CAMERA: + if camera == "hand" and not self._robot.has_arm(): + continue + self._image_requests_by_camera[camera] = {} + image_types = IMAGE_SOURCES_BY_CAMERA[camera] + for image_type in image_types: + if image_type.startswith("depth"): + image_format = image_pb2.Image.FORMAT_RAW + pixel_format = image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 + else: + image_format = image_pb2.Image.FORMAT_JPEG + pixel_format = image_pb2.Image.PIXEL_FORMAT_RGB_U8 + + source = IMAGE_SOURCES_BY_CAMERA[camera][image_type] + self._image_requests_by_camera[camera][ + image_type + ] = build_image_request( + source, + image_format=image_format, + pixel_format=pixel_format, + quality_percent=75, + ) + def get_frontleft_rgb_image(self) -> image_pb2.ImageResponse: try: return self._image_client.get_image( @@ -224,3 +299,71 @@ def get_depth_registered_images( self, ) -> typing.Optional[typing.Union[ImageBundle, ImageWithHandBundle]]: return self.get_images(self._depth_registered_image_requests) + + def get_images_by_cameras( + self, camera_sources: typing.List[CameraSource] + ) -> typing.Optional[typing.List[ImageEntry]]: + """Calls the GetImage RPC using the image client with requests + corresponding to the given cameras. + Args: + camera_sources: a list of CameraSource objects. There are two + possibilities for each item in this list. Either it is + CameraSource(camera='front') or + CameraSource(camera='front', image_types=['visual', 'depth_registered') + - If the former is provided, the image requests will include all + image types for each specified camera. + - If the latter is provided, the image requests will be + limited to the specified image types for each corresponding + camera. + Note that duplicates of camera names are not allowed. + Returns: + a list, where each entry is (camera_name, image_type, image_response) + e.g. ('frontleft', 'visual', image_response), or none if there was an error + """ + # Build image requests + image_requests = [] + source_types = [] + cameras_specified = set() + for item in camera_sources: + if item.camera_name in cameras_specified: + self._logger.error( + f"Duplicated camera source for camera {item.camera_name}" + ) + return None + image_types = item.image_types + if image_types is None: + image_types = IMAGE_TYPES + for image_type in image_types: + try: + image_requests.append( + self._image_requests_by_camera[item.camera_name][image_type] + ) + except KeyError: + self._logger.error( + f"Unexpected camera name '{item.camera_name}' or image type '{image_type}'" + ) + return None + source_types.append((item.camera_name, image_type)) + cameras_specified.add(item.camera_name) + + # Send image requests + try: + image_responses = self._image_client.get_image(image_requests) + except UnsupportedPixelFormatRequestedError: + self._logger.error( + "UnsupportedPixelFormatRequestedError. " + "Likely pixel_format is set wrong for some image request" + ) + return None + + # Return + result = [] + for i, (camera_name, image_type) in enumerate(source_types): + result.append( + ImageEntry( + camera_name=camera_name, + image_type=image_type, + image_response=image_responses[i], + ) + ) + return result From 5d46cda31cf8e69274f7fcefd70b7ea58a4afe80 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 19 May 2023 13:03:31 +0100 Subject: [PATCH 34/57] fix dataclass typing issue for older python versions (20.04), check lease object is initialised in claim function (#14) --- spot_wrapper/spot_images.py | 2 +- spot_wrapper/wrapper.py | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index c8a7ddca..6891cdcc 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -80,7 +80,7 @@ @dataclass(frozen=True, eq=True) class CameraSource: camera_name: str - image_types: list[str] + image_types: typing.List[str] @dataclass(frozen=True) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 55098e79..a9329a60 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -819,12 +819,13 @@ def robotToLocalTime(self, timestamp: Timestamp) -> Timestamp: def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" - for resource in self.lease: - if ( - resource.resource == "all-leases" - and SPOT_CLIENT_NAME in resource.lease_owner.client_name - ): - return True, "We already claimed the lease" + if self.lease is not None: + for resource in self.lease: + if ( + resource.resource == "all-leases" + and SPOT_CLIENT_NAME in resource.lease_owner.client_name + ): + return True, "We already claimed the lease" try: self._robot_params["robot_id"] = self._robot.get_id() From b4010061f463a190a877690f6a38d3d610e81554 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:33:53 +0100 Subject: [PATCH 35/57] remove old camera task mapping introduced in merge --- spot_wrapper/wrapper.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index a9329a60..9c4f5c2b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -630,16 +630,6 @@ def __init__( self._async_tasks = AsyncTasks(robot_tasks) - self.camera_task_name_to_task_mapping = { - "hand_image": self._hand_image_task, - "side_image": self._side_image_task, - "rear_image": self._rear_image_task, - "front_image": self._front_image_task, - } - - self._robot_id = None - self._lease = None - @staticmethod def authenticate(robot, username, password, logger): """ From c9eb3515d6a97cfa75b3e975386fa1617a71edb4 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:34:19 +0100 Subject: [PATCH 36/57] formatting --- spot_wrapper/spot_arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 5e50d6a6..aeac4d51 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -501,7 +501,7 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: except Exception as e: return ( False, - f"An error occured while trying to move arm \n Exception: {e}" + f"An error occured while trying to move arm \n Exception: {e}", ) return True, "Moved arm successfully" From 056fc25103b26c62594edbac883a250660ef3b40 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:42:37 +0100 Subject: [PATCH 37/57] Add changes from [WUD-126] Add manipulation client (#13) Co-authored-by: myeatman-bdai <129521731+myeatman-bdai@users.noreply.github.com> --- spot_wrapper/spot_arm.py | 52 ++++++++++++++++++++++++++++++++++++---- spot_wrapper/wrapper.py | 10 +++++--- 2 files changed, 54 insertions(+), 8 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index aeac4d51..d3019cbd 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -61,6 +61,7 @@ def __init__( logger: logging.Logger, robot_params: typing.Dict[str, typing.Any], robot_clients: typing.Dict[str, typing.Any], + max_command_duration: float ): """ Constructor for SpotArm class. @@ -72,15 +73,17 @@ def __init__( robot_clients: Dictionary of robot clients - robot_clients['robot_command_client']: RobotCommandClient object - robot_clients['robot_command_method']: RobotCommand method + max_command_duration: Maximum duration for commands when using the manipulation command method """ self._robot = robot self._logger = logger self._robot_params = robot_params + self._max_command_duration = max_command_duration self._robot_command_client: RobotCommandClient = robot_clients[ "robot_command_client" ] - self._manipulation_client: ManipulationApiClient = robot_clients[ - "manipulation_client" + self._manipulation_api_client: ManipulationApiClient = robot_clients[ + "manipulation_api_client" ] self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] self._image_client: ImageClient = robot_clients["image_client"] @@ -115,6 +118,43 @@ def initialize_hand_image_service(self): def hand_image_task(self): return self._hand_image_task + def _manipulation_request( + self, + request_proto: manipulation_api_pb2, + end_time_secs=None, + timesync_endpoint=None, + ): + """Generic function for sending requests to the manipulation api of a robot. + Args: + request_proto: manipulation_api_pb2 object to send to the robot. + """ + try: + command_id = self._manipulation_api_client.manipulation_api_command( + manipulation_api_request=request_proto + ).manipulation_cmd_id + + return True, "Success", command_id + except Exception as e: + self._logger.error(f"Unable to execute manipulation command: {e}") + return False, str(e), None + + def manipulation_command(self, request: manipulation_api_pb2): + end_time = time.time() + self._max_command_duration + return self._manipulation_request( + request, + end_time_secs=end_time, + timesync_endpoint=self._robot.time_sync.endpoint, + ) + + def get_manipulation_command_feedback(self, cmd_id): + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_id + ) + + return self._manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) + def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: if not self._robot.has_arm(): return False, "Spot with an arm is required for this service" @@ -520,7 +560,7 @@ def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): pick_object=grasp ) # Send the request - cmd_response = self._manipulation_client.manipulation_api_command( + cmd_response = self._manipulation_api_client.manipulation_api_command( manipulation_api_request=grasp_request ) @@ -531,8 +571,10 @@ def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): ) # Send the request - response = self._manipulation_client.manipulation_api_feedback_command( - manipulation_api_feedback_request=feedback_request + response = ( + self._manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) ) print( diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9c4f5c2b..cdacf1eb 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -502,7 +502,7 @@ def __init__( self._logger.info("No point cloud services are available.") if self._robot.has_arm(): - self._manipulation_client = self._robot.ensure_client( + self._manipulation_api_client = self._robot.ensure_client( ManipulationApiClient.default_service_name ) @@ -519,7 +519,7 @@ def __init__( "spot_check_client": self._spot_check_client, "robot_command_method": self._robot_command, "world_objects_client": self._world_objects_client, - "manipulation_client": self._manipulation_client, + "manipulation_api_client": self._manipulation_api_client, } if self._point_cloud_client: self._robot_clients["point_cloud_client"] = self._point_cloud_client @@ -593,7 +593,11 @@ def __init__( if self._robot.has_arm(): self._spot_arm = SpotArm( - self._robot, self._logger, self._robot_params, self._robot_clients + self._robot, + self._logger, + self._robot_params, + self._robot_clients, + MAX_COMMAND_DURATION, ) self._hand_image_task = self._spot_arm.hand_image_task robot_tasks.append(self._hand_image_task) From 65347c46fa2a7f65fb42333198365466204d5aa2 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 12:53:01 +0100 Subject: [PATCH 38/57] Add changes for added support for the rgb_cameras parameter in spot_ros2 (#15) Co-authored-by: Philip Keller Co-authored-by: Shubham <52372631+skpawar1305@users.noreply.github.com> --- spot_wrapper/spot_images.py | 10 +++++++++- spot_wrapper/wrapper.py | 5 ++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 6891cdcc..536eb7ad 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -97,9 +97,11 @@ def __init__( logger: logging.Logger, robot_params: typing.Dict[str, typing.Any], robot_clients: typing.Dict[str, typing.Any], + rgb_cameras: bool = True, ): self._robot = robot self._logger = logger + self._rgb_cameras = rgb_cameras self._robot_params = robot_params self._image_client: ImageClient = robot_clients["image_client"] @@ -164,7 +166,13 @@ def __init__( pixel_format = image_pb2.Image.PIXEL_FORMAT_DEPTH_U16 else: image_format = image_pb2.Image.FORMAT_JPEG - pixel_format = image_pb2.Image.PIXEL_FORMAT_RGB_U8 + if camera == "hand" or self._rgb_cameras: + pixel_format = image_pb2.Image.PIXEL_FORMAT_RGB_U8 + elif camera != "hand": + self._logger.info( + f"Switching {camera}:{image_type} to greyscale image format." + ) + pixel_format = image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8 source = IMAGE_SOURCES_BY_CAMERA[camera][image_type] self._image_requests_by_camera[camera][ diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index cdacf1eb..4c2aa47b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -378,6 +378,7 @@ def __init__( use_take_lease: bool = False, get_lease_on_action: bool = False, continually_try_stand: bool = True, + rgb_cameras: bool = True, ): """ Args: @@ -397,6 +398,7 @@ def __init__( power on the robot for commands which require it - stand, rollover, self-right. continually_try_stand: If the robot expects to be standing and is not, command a stand. This can result in strange behavior if you use the wrapper and tablet together. + rgb_cameras: If the robot has only body-cameras with greyscale images, this must be set to false. """ self._username = username self._password = password @@ -407,6 +409,7 @@ def __init__( self._use_take_lease = use_take_lease self._get_lease_on_action = get_lease_on_action self._continually_try_stand = continually_try_stand + self._rgb_cameras = rgb_cameras self._frame_prefix = "" if robot_name is not None: self._frame_prefix = robot_name + "/" @@ -614,7 +617,7 @@ def __init__( self._robot, self._logger, self._robot_params, self._robot_clients ) self._spot_images = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients + self._robot, self._logger, self._robot_params, self._robot_clients, self._rgb_cameras ) if self._point_cloud_client: From 3e855a47c1a5a99a24b8ead43650784d4871d991 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 13:07:38 +0100 Subject: [PATCH 39/57] Add changes from [SW-141] Checking edge cases in upload_graph (#12) Co-authored-by: kzheng <125413689+kaiyu-zheng@users.noreply.github.com> --- spot_wrapper/spot_graph_nav.py | 89 +++++++++++++++++++++++++--------- 1 file changed, 67 insertions(+), 22 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 2317ad57..045a5690 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -1,20 +1,23 @@ -import typing import logging import math -import time import os +import time +import typing -from bosdyn.client.robot import Robot -from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.lease import LeaseClient, LeaseWallet, LeaseKeepAlive -from bosdyn.client.graph_nav import GraphNavClient -from bosdyn.client.map_processing import MapProcessingServiceClient -from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.api.graph_nav import graph_nav_pb2 from bosdyn.api.graph_nav import map_pb2 -from bosdyn.api.graph_nav import nav_pb2 from bosdyn.api.graph_nav import map_processing_pb2 - +from bosdyn.api.graph_nav import nav_pb2 +from bosdyn.client.frame_helpers import get_odom_tform_body +from bosdyn.client.graph_nav import GraphNavClient +from bosdyn.client.lease import ( + LeaseClient, + LeaseWallet, + LeaseKeepAlive, +) +from bosdyn.client.map_processing import MapProcessingServiceClient +from bosdyn.client.robot import Robot +from bosdyn.client.robot_state import RobotStateClient from google.protobuf import wrappers_pb2 @@ -37,12 +40,19 @@ def __init__( self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet self._robot_params = robot_params + self._init_current_graph_nav_state() + + def _init_current_graph_nav_state(self): # Store the most recent knowledge of the state of the robot based on rpc calls. self._current_graph = None self._current_edges = dict() # maps to_waypoint to list(from_waypoint) self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() + self._current_anchored_world_objects = ( + dict() + ) # maps object id to a (wo, waypoint, fiducial) + self._current_anchors = dict() # maps anchor id to anchor def list_graph(self) -> typing.List[str]: """List waypoint ids of graph_nav @@ -318,7 +328,7 @@ def _list_graph_waypoint_and_edge_ids(self, *args): def _upload_graph_and_snapshots(self, upload_filepath: str): """Upload the graph and snapshots to the robot.""" self._logger.info("Loading the graph from disk into local storage...") - with open(upload_filepath + "/graph", "rb") as graph_file: + with open(os.path.join(upload_filepath, "graph"), "rb") as graph_file: # Load the graph from disk. data = graph_file.read() self._current_graph = map_pb2.Graph() @@ -330,28 +340,60 @@ def _upload_graph_and_snapshots(self, upload_filepath: str): ) for waypoint in self._current_graph.waypoints: # Load the waypoint snapshots from disk. - with open( - upload_filepath + "/waypoint_snapshots/{}".format(waypoint.snapshot_id), - "rb", - ) as snapshot_file: + if len(waypoint.snapshot_id) == 0: + continue + waypoint_filepath = os.path.join( + upload_filepath, "waypoint_snapshots", waypoint.snapshot_id + ) + if not os.path.exists(waypoint_filepath): + continue + with open(waypoint_filepath, "rb") as snapshot_file: waypoint_snapshot = map_pb2.WaypointSnapshot() waypoint_snapshot.ParseFromString(snapshot_file.read()) self._current_waypoint_snapshots[ waypoint_snapshot.id ] = waypoint_snapshot + + for fiducial in waypoint_snapshot.objects: + if not fiducial.HasField("apriltag_properties"): + continue + + str_id = str(fiducial.apriltag_properties.tag_id) + if ( + str_id in self._current_anchored_world_objects + and len(self._current_anchored_world_objects[str_id]) == 1 + ): + # Replace the placeholder tuple with a tuple of (wo, waypoint, fiducial). + anchored_wo = self._current_anchored_world_objects[str_id][0] + self._current_anchored_world_objects[str_id] = ( + anchored_wo, + waypoint, + fiducial, + ) for edge in self._current_graph.edges: # Load the edge snapshots from disk. - self._logger.info(f"Trying edge: {edge.snapshot_id}") - if not edge.snapshot_id: + if len(edge.snapshot_id) == 0: + continue + edge_filepath = os.path.join( + upload_filepath, "edge_snapshots", edge.snapshot_id + ) + if not os.path.exists(edge_filepath): continue - with open( - upload_filepath + "/edge_snapshots/{}".format(edge.snapshot_id), "rb" - ) as snapshot_file: + with open(edge_filepath, "rb") as snapshot_file: edge_snapshot = map_pb2.EdgeSnapshot() edge_snapshot.ParseFromString(snapshot_file.read()) self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot + for anchor in self._current_graph.anchoring.anchors: + self._current_anchors[anchor.id] = anchor # Upload the graph to the robot. self._logger.info("Uploading the graph and snapshots to the robot...") + if self._lease is None: + self._logger.error( + "Graph nav module did not have a lease to the robot. Claim it before attempting to upload the graph " + "and snapshots." + ) + return + self._graph_nav_client.upload_graph( lease=self._lease.lease_proto, graph=self._current_graph ) @@ -370,7 +412,8 @@ def _upload_graph_and_snapshots(self, upload_filepath: str): if not localization_state.localization.waypoint_id: # The robot is not localized to the newly uploaded graph. self._logger.info( - "Upload complete! The robot is currently not localized to the map; please localize the robot using a fiducial before attempting a navigation command." + "Upload complete! The robot is currently not localized to the map; please localize the robot using a " + "fiducial before attempting a navigation command." ) def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: @@ -498,7 +541,9 @@ def _navigate_route( def _clear_graph(self, *args) -> bool: """Clear the state of the map on the robot, removing all waypoints and edges.""" - return self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) + result = self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) + self._init_current_graph_nav_state() + return result def _check_success(self, command_id: int = -1) -> bool: """Use a navigation command id to get feedback from the robot and sit when command succeeds.""" From 03c987578f99edb59f09a024acb1662e69ce954a Mon Sep 17 00:00:00 2001 From: David Watkins <129521611+davidwatkins-bdai@users.noreply.github.com> Date: Thu, 1 Jun 2023 13:54:27 -0400 Subject: [PATCH 40/57] Updated bosdyn to 3.2.3 (#16) --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 76c87a8b..2c4b4fde 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,4 +1,4 @@ -bosdyn_core==3.2.2.post1 +bosdyn_core==3.2.3 protobuf==4.22.1 setuptools==45.2.0 pytest==7.3.1 \ No newline at end of file From 3ed105d72b729cf250d86a01bbed7922fdcce1c0 Mon Sep 17 00:00:00 2001 From: Vedant Gupta <134331854+Guptabot@users.noreply.github.com> Date: Fri, 2 Jun 2023 19:01:23 -0400 Subject: [PATCH 41/57] [OC-4] Build a Spot Dance Interface (#17) * [OC-5] add method to upload hardcoded file * [OC-4] Code cleanup for PR * [OC-4] Create separate SpotDacing class * [OC-4] remove old execute_dance code * [OC-4] Fix authentication method for compatibility with WUD test * [OC-5] format files with black * [OC-4] Modify code based on PR comments * delete execute_dance from wrapper.py * [OC-4] Get lease from decorator * [OC-4] Remove print statements * [OC-4] Remove extra imports * [OC-4] Remove extra argument --- spot_wrapper/spot_dance.py | 79 ++++++++++++++++++++++++++++++++++++++ spot_wrapper/wrapper.py | 26 ++++++++++++- 2 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 spot_wrapper/spot_dance.py diff --git a/spot_wrapper/spot_dance.py b/spot_wrapper/spot_dance.py new file mode 100644 index 00000000..e4882de3 --- /dev/null +++ b/spot_wrapper/spot_dance.py @@ -0,0 +1,79 @@ +import time + +from bosdyn.choreography.client.choreography import ( + load_choreography_sequence_from_txt_file, +) +from bosdyn.client import ResponseError +from bosdyn.client.exceptions import UnauthenticatedError +from bosdyn.client.robot import Robot +from bosdyn.choreography.client.choreography import ChoreographyClient + + +class SpotDance: + def __init__( + self, + robot: Robot, + choreography_client: ChoreographyClient, + is_licensed_for_choreography: bool, + ): + self._robot = robot + self._choreography_client = choreography_client + self._is_licensed_for_choreography = is_licensed_for_choreography + + def execute_dance(self, filepath: str) -> tuple[bool, str]: + """uploads and executes the dance at filepath to Spot""" + + if not self._is_licensed_for_choreography: + return False, "Robot is not licensed for choreography." + if self._robot.is_estopped(): + error_msg = "Robot is estopped. Please use an external E-Stop client" + "such as the estop SDK example, to configure E-Stop." + return False, error_msg + try: + choreography = load_choreography_sequence_from_txt_file(filepath) + except Exception as execp: + error_msg = "Failed to load choreography. Raised exception: " + str(execp) + return False, error_msg + try: + upload_response = self._choreography_client.upload_choreography( + choreography, non_strict_parsing=True + ) + except UnauthenticatedError as err: + error_msg = "The robot license must contain 'choreography' permissions to upload and execute dances." + "Please contact Boston Dynamics Support to get the appropriate license file. " + return False, error_msg + except ResponseError as err: + error_msg = "Choreography sequence upload failed. The following warnings were produced: " + for warn in err.response.warnings: + error_msg += warn + return False, error_msg + + # Routine is valid! Power on robot and execute routine. + try: + self._robot.power_on() + routine_name = choreography.name + client_start_time = time.time() + 5.0 + start_slice = 0 # start the choreography at the beginning + + # Issue the command to the robot's choreography service. + + self._choreography_client.execute_choreography( + choreography_name=routine_name, + client_start_time=client_start_time, + choreography_starting_slice=start_slice, + ) + + # Estimate how long the choreographed sequence will take, and sleep that long. + + total_choreography_slices = 0 + for move in choreography.moves: + total_choreography_slices += move.requested_slices + estimated_time_seconds = ( + total_choreography_slices / choreography.slices_per_minute * 60.0 + ) + time.sleep(estimated_time_seconds) + + self._robot.power_off() + return True, "sucess" + except Exception as e: + return False, f"Error executing dance: {e}" diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 4c2aa47b..de146782 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -35,6 +35,11 @@ from bosdyn.client.robot_state import RobotStateClient from bosdyn.client.world_object import WorldObjectClient from bosdyn.client.spot_check import SpotCheckClient +from bosdyn.client.license import LicenseClient +from bosdyn.choreography.client.choreography import ( + ChoreographyClient, + load_choreography_sequence_from_txt_file, +) from bosdyn.geometry import EulerZXY from bosdyn.api import basic_command_pb2 @@ -47,6 +52,7 @@ from .spot_graph_nav import SpotGraphNav from .spot_check import SpotCheck from .spot_images import SpotImages +from .spot_dance import SpotDance SPOT_CLIENT_NAME = "ros_spot" MAX_COMMAND_DURATION = 1e5 @@ -428,6 +434,7 @@ def __init__( self._near_goal = False self._trajectory_status_unknown = False self._last_robot_command_feedback = False + self._is_licensed_for_choreography = True self._last_stand_command = None self._last_sit_command = None self._last_trajectory_command = None @@ -441,6 +448,7 @@ def __init__( self._logger.error("Error creating SDK object: %s", e) self._valid = False return + self._sdk.register_service_client(ChoreographyClient) self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) @@ -503,7 +511,17 @@ def __init__( except Exception as e: self._point_cloud_client = None self._logger.info("No point cloud services are available.") - + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) + if not self._license_client.get_feature_enabled( + [ChoreographyClient.license_name] + )[ChoreographyClient.license_name]: + self._logger.error(f"Robot is not licensed for choreography: {e}") + self._is_licensed_for_choreography = False if self._robot.has_arm(): self._manipulation_api_client = self._robot.ensure_client( ManipulationApiClient.default_service_name @@ -635,6 +653,12 @@ def __init__( self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) + self._spot_dance = SpotDance( + self._robot, + self._choreography_client, + self._is_licensed_for_choreography, + ) + self._async_tasks = AsyncTasks(robot_tasks) @staticmethod From 439965af6f8e9e8e9d36414dc7301e107329c99d Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 14:23:36 +0100 Subject: [PATCH 42/57] formatting --- spot_wrapper/spot_arm.py | 2 +- spot_wrapper/wrapper.py | 14 +++++++++----- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index d3019cbd..bfb34095 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -61,7 +61,7 @@ def __init__( logger: logging.Logger, robot_params: typing.Dict[str, typing.Any], robot_clients: typing.Dict[str, typing.Any], - max_command_duration: float + max_command_duration: float, ): """ Constructor for SpotArm class. diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 96045438..524e5123 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -635,7 +635,11 @@ def __init__( self._robot, self._logger, self._robot_params, self._robot_clients ) self._spot_images = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients, self._rgb_cameras + self._robot, + self._logger, + self._robot_params, + self._robot_clients, + self._rgb_cameras, ) if self._point_cloud_client: @@ -654,10 +658,10 @@ def __init__( robot_tasks.append(self._world_objects_task) self._spot_dance = SpotDance( - self._robot, - self._choreography_client, - self._is_licensed_for_choreography, - ) + self._robot, + self._choreography_client, + self._is_licensed_for_choreography, + ) self._async_tasks = AsyncTasks(robot_tasks) From 90d60ac1ec9fe21d592553102bded72bbe085241 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 15:19:36 +0100 Subject: [PATCH 43/57] fix startup issues when choreography or arm is not present --- spot_wrapper/spot_images.py | 3 +- spot_wrapper/wrapper.py | 69 +++++++++++++++++++++++-------------- 2 files changed, 44 insertions(+), 28 deletions(-) diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 536eb7ad..563a5291 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,8 +1,7 @@ import typing import logging -from enum import enum from collections import namedtuple -from dataclasses import dataclass, field +from dataclasses import dataclass from bosdyn.client.robot import Robot from bosdyn.client.image import ( diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 524e5123..c2cba15b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -16,6 +16,7 @@ from bosdyn.client import frame_helpers from bosdyn.client import math_helpers from bosdyn.client import robot_command +from bosdyn.client.robot import UnregisteredServiceError from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks from bosdyn.client.docking import DockingClient @@ -36,10 +37,17 @@ from bosdyn.client.world_object import WorldObjectClient from bosdyn.client.spot_check import SpotCheckClient from bosdyn.client.license import LicenseClient -from bosdyn.choreography.client.choreography import ( - ChoreographyClient, - load_choreography_sequence_from_txt_file, -) + +HAVE_CHOREOGRAPHY_MODULE = True +try: + from bosdyn.choreography.client.choreography import ( + ChoreographyClient, + load_choreography_sequence_from_txt_file, + ) + from .spot_dance import SpotDance +except ModuleNotFoundError: + HAVE_CHOREOGRAPHY_MODULE = False + from bosdyn.geometry import EulerZXY from bosdyn.api import basic_command_pb2 @@ -52,7 +60,6 @@ from .spot_graph_nav import SpotGraphNav from .spot_check import SpotCheck from .spot_images import SpotImages -from .spot_dance import SpotDance SPOT_CLIENT_NAME = "ros_spot" MAX_COMMAND_DURATION = 1e5 @@ -445,10 +452,10 @@ def __init__( try: self._sdk = create_standard_sdk(SPOT_CLIENT_NAME) except Exception as e: - self._logger.error("Error creating SDK object: %s", e) + self._logger.error(f"Error creating SDK object: {e}") self._valid = False return - self._sdk.register_service_client(ChoreographyClient) + self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) @@ -508,24 +515,33 @@ def __init__( self._point_cloud_client = self._robot.ensure_client( VELODYNE_SERVICE_NAME ) - except Exception as e: + except UnregisteredServiceError as e: self._point_cloud_client = None self._logger.info("No point cloud services are available.") - self._choreography_client = self._robot.ensure_client( - ChoreographyClient.default_service_name - ) - self._license_client = self._robot.ensure_client( - LicenseClient.default_service_name - ) - if not self._license_client.get_feature_enabled( - [ChoreographyClient.license_name] - )[ChoreographyClient.license_name]: - self._logger.error(f"Robot is not licensed for choreography: {e}") - self._is_licensed_for_choreography = False + + if HAVE_CHOREOGRAPHY_MODULE: + self._sdk.register_service_client(ChoreographyClient) + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) + if not self._license_client.get_feature_enabled( + [ChoreographyClient.license_name] + )[ChoreographyClient.license_name]: + self._logger.error(f"Robot is not licensed for choreography: {e}") + self._is_licensed_for_choreography = False + else: + self._choreography_client = None + if self._robot.has_arm(): self._manipulation_api_client = self._robot.ensure_client( ManipulationApiClient.default_service_name ) + else: + self._manipulation_api_client = None + self._robot_clients = { "robot_state_client": self._robot_state_client, @@ -541,9 +557,9 @@ def __init__( "robot_command_method": self._robot_command, "world_objects_client": self._world_objects_client, "manipulation_api_client": self._manipulation_api_client, + "choreography_client": self._choreography_client, + "point_cloud_client": self._point_cloud_client, } - if self._point_cloud_client: - self._robot_clients["point_cloud_client"] = self._point_cloud_client initialised = True except Exception as e: @@ -657,11 +673,12 @@ def __init__( self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) - self._spot_dance = SpotDance( - self._robot, - self._choreography_client, - self._is_licensed_for_choreography, - ) + if HAVE_CHOREOGRAPHY_MODULE: + self._spot_dance = SpotDance( + self._robot, + self._choreography_client, + self._is_licensed_for_choreography, + ) self._async_tasks = AsyncTasks(robot_tasks) From 7201b3a69f21cafb56b78940258928680ec2a4e4 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 15:37:32 +0100 Subject: [PATCH 44/57] small changes to choreo check and output when services are not available --- spot_wrapper/wrapper.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index c2cba15b..908a9936 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -38,13 +38,14 @@ from bosdyn.client.spot_check import SpotCheckClient from bosdyn.client.license import LicenseClient -HAVE_CHOREOGRAPHY_MODULE = True try: from bosdyn.choreography.client.choreography import ( ChoreographyClient, load_choreography_sequence_from_txt_file, ) from .spot_dance import SpotDance + + HAVE_CHOREOGRAPHY_MODULE = True except ModuleNotFoundError: HAVE_CHOREOGRAPHY_MODULE = False @@ -441,7 +442,6 @@ def __init__( self._near_goal = False self._trajectory_status_unknown = False self._last_robot_command_feedback = False - self._is_licensed_for_choreography = True self._last_stand_command = None self._last_sit_command = None self._last_trajectory_command = None @@ -456,7 +456,6 @@ def __init__( self._valid = False return - self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) @@ -517,7 +516,7 @@ def __init__( ) except UnregisteredServiceError as e: self._point_cloud_client = None - self._logger.info("No point cloud services are available.") + self._logger.info("Velodyne point cloud service is not available.") if HAVE_CHOREOGRAPHY_MODULE: self._sdk.register_service_client(ChoreographyClient) @@ -530,8 +529,12 @@ def __init__( if not self._license_client.get_feature_enabled( [ChoreographyClient.license_name] )[ChoreographyClient.license_name]: - self._logger.error(f"Robot is not licensed for choreography: {e}") + self._logger.info( + f"Robot is not licensed for choreography: {e}" + ) self._is_licensed_for_choreography = False + else: + self._is_licensed_for_choreography = True else: self._choreography_client = None @@ -541,7 +544,7 @@ def __init__( ) else: self._manipulation_api_client = None - + self._logger.info("Manipulation API is not available.") self._robot_clients = { "robot_state_client": self._robot_state_client, @@ -673,7 +676,7 @@ def __init__( self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) - if HAVE_CHOREOGRAPHY_MODULE: + if self._is_licensed_for_choreography: self._spot_dance = SpotDance( self._robot, self._choreography_client, From 9b7ecf55f4ed8ea69677972a110cf073a49f72de Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 15:46:15 +0100 Subject: [PATCH 45/57] better choreo ordering --- spot_wrapper/wrapper.py | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 908a9936..4bd2b45f 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -518,24 +518,27 @@ def __init__( self._point_cloud_client = None self._logger.info("Velodyne point cloud service is not available.") + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) + if HAVE_CHOREOGRAPHY_MODULE: - self._sdk.register_service_client(ChoreographyClient) - self._choreography_client = self._robot.ensure_client( - ChoreographyClient.default_service_name - ) - self._license_client = self._robot.ensure_client( - LicenseClient.default_service_name - ) - if not self._license_client.get_feature_enabled( + if self._license_client.get_feature_enabled( [ChoreographyClient.license_name] )[ChoreographyClient.license_name]: + self._sdk.register_service_client(ChoreographyClient) + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) + self._is_licensed_for_choreography = True + else: self._logger.info( f"Robot is not licensed for choreography: {e}" ) self._is_licensed_for_choreography = False - else: - self._is_licensed_for_choreography = True + self._choreography_client = None else: + self._is_licensed_for_choreography = False self._choreography_client = None if self._robot.has_arm(): From 0a67be94c18c2ea263736951fc5a0400edccb625 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 3 Jun 2023 15:48:22 +0100 Subject: [PATCH 46/57] message when choreo module is missing --- spot_wrapper/wrapper.py | 1 + 1 file changed, 1 insertion(+) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 4bd2b45f..b6fda72b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -538,6 +538,7 @@ def __init__( self._is_licensed_for_choreography = False self._choreography_client = None else: + self._logger.info(f"Choreography is not available.") self._is_licensed_for_choreography = False self._choreography_client = None From 5d53c58ea5b7a190dbcd71a63b46c2548f283897 Mon Sep 17 00:00:00 2001 From: Ming Jie See Date: Fri, 9 Jun 2023 14:46:05 +0100 Subject: [PATCH 47/57] cleanup unused spot_image and renamed graphnav private methods --- spot_wrapper/spot_graph_nav.py | 8 ++++---- spot_wrapper/wrapper.py | 3 --- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 045a5690..f3d917a9 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -97,8 +97,8 @@ def navigate_initial_localization( self._lease = self._lease_wallet.get_lease() self._lease_keepalive = LeaseKeepAlive(self._lease_client) if upload_filepath: - self._clear_graph() - self._upload_graph_and_snapshots(upload_filepath) + self.clear_graph() + self.upload_graph_and_snapshots(upload_filepath) else: self._download_current_graph() self._logger.info( @@ -325,7 +325,7 @@ def _list_graph_waypoint_and_edge_ids(self, *args): ) = self._update_waypoints_and_edges(graph, localization_id, self._logger) return self._current_annotation_name_to_wp_id, self._current_edges - def _upload_graph_and_snapshots(self, upload_filepath: str): + def upload_graph_and_snapshots(self, upload_filepath: str): """Upload the graph and snapshots to the robot.""" self._logger.info("Loading the graph from disk into local storage...") with open(os.path.join(upload_filepath, "graph"), "rb") as graph_file: @@ -539,7 +539,7 @@ def _navigate_route( return True, "Finished navigating route!" - def _clear_graph(self, *args) -> bool: + def clear_graph(self, *args) -> bool: """Clear the state of the map on the robot, removing all waypoints and edges.""" result = self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) self._init_current_graph_nav_state() diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index b6fda72b..f4b4733e 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -631,9 +631,6 @@ def __init__( "rates": self._rates, "callbacks": self._callbacks, } - self.spot_image = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients - ) if self._robot.has_arm(): self._spot_arm = SpotArm( From 17dd90c7ba8c5f2c9850161a64ba21984e839bf6 Mon Sep 17 00:00:00 2001 From: Ming Jie See Date: Fri, 9 Jun 2023 15:06:57 +0100 Subject: [PATCH 48/57] update graph_nav private methods --- spot_wrapper/spot_graph_nav.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index f3d917a9..fac56cfc 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -105,9 +105,9 @@ def navigate_initial_localization( "Re-using existing graph on robot. Check that the correct graph is loaded!" ) if initial_localization_fiducial: - self._set_initial_localization_fiducial() + self.set_initial_localization_fiducial() if initial_localization_waypoint: - self._set_initial_localization_waypoint([initial_localization_waypoint]) + self.set_initial_localization_waypoint([initial_localization_waypoint]) self._list_graph_waypoint_and_edge_ids() self._get_localization_state() @@ -166,7 +166,7 @@ def _get_localization_state(self, *args): f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}" ) - def _set_initial_localization_fiducial(self, *args): + def set_initial_localization_fiducial(self, *args): """Trigger localization when near a fiducial.""" robot_state = self._robot_state_client.get_robot_state() current_odom_tform_body = get_odom_tform_body( @@ -180,7 +180,7 @@ def _set_initial_localization_fiducial(self, *args): ko_tform_body=current_odom_tform_body, ) - def _set_initial_localization_waypoint(self, *args): + def set_initial_localization_waypoint(self, *args): """Trigger localization to a waypoint.""" # Take the first argument as the localization waypoint. if len(args) < 1: From 0ec85565f653680fdcdf1dcfd65dd93803faedda Mon Sep 17 00:00:00 2001 From: Ming Jie See Date: Fri, 9 Jun 2023 23:57:17 +0100 Subject: [PATCH 49/57] custom arm not found Exception --- spot_wrapper/wrapper.py | 245 +++++++++++----------------------------- 1 file changed, 68 insertions(+), 177 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 1fb5c6b4..5c03d507 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -96,6 +96,14 @@ def robotToLocalTime(timestamp, robot): return rtime +class MissingSpotArm(Exception): + """Raised when the arm is not available on the robot""" + + def __init__(self, message="Spot arm not available"): + # Call the base class constructor with the parameters it needs + super().__init__(message) + + class AsyncRobotState(AsyncPeriodicQuery): """Class to get robot state at regular intervals. get_robot_state_async query sent to the robot at every tick. Callback registered to defined callback function. @@ -107,9 +115,7 @@ class AsyncRobotState(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncRobotState, self).__init__( - "robot-state", client, logger, period_sec=1.0 / max(rate, 1.0) - ) + super(AsyncRobotState, self).__init__("robot-state", client, logger, period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -132,9 +138,7 @@ class AsyncMetrics(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncMetrics, self).__init__( - "robot-metrics", client, logger, period_sec=1.0 / max(rate, 1.0) - ) + super(AsyncMetrics, self).__init__("robot-metrics", client, logger, period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -157,9 +161,7 @@ class AsyncLease(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncLease, self).__init__( - "lease", client, logger, period_sec=1.0 / max(rate, 1.0) - ) + super(AsyncLease, self).__init__("lease", client, logger, period_sec=1.0 / max(rate, 1.0)) self._callback = None if rate > 0.0: self._callback = callback @@ -189,19 +191,13 @@ def __init__(self, client, logger, rate, spot_wrapper): def _start_query(self): if self._spot_wrapper._last_stand_command != None: try: - response = self._client.robot_command_feedback( - self._spot_wrapper._last_stand_command - ) - status = ( - response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status - ) + response = self._client.robot_command_feedback(self._spot_wrapper._last_stand_command) + status = response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status self._spot_wrapper._robot_params["is_sitting"] = False if status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING: self._spot_wrapper._robot_params["is_standing"] = True self._spot_wrapper._last_stand_command = None - elif ( - status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS - ): + elif status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS: self._spot_wrapper._robot_params["is_standing"] = False else: self._logger.warning("Stand command in unknown state") @@ -213,9 +209,7 @@ def _start_query(self): if self._spot_wrapper._last_sit_command != None: try: self._spot_wrapper._robot_params["is_standing"] = False - response = self._client.robot_command_feedback( - self._spot_wrapper._last_sit_command - ) + response = self._client.robot_command_feedback(self._spot_wrapper._last_sit_command) if ( response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status == basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING @@ -238,49 +232,31 @@ def _start_query(self): if self._spot_wrapper._last_trajectory_command != None: try: - response = self._client.robot_command_feedback( - self._spot_wrapper._last_trajectory_command - ) + response = self._client.robot_command_feedback(self._spot_wrapper._last_trajectory_command) status = ( response.feedback.synchronized_feedback.mobility_command_feedback.se2_trajectory_feedback.status ) # STATUS_AT_GOAL always means that the robot reached the goal. If the trajectory command did not # request precise positioning, then STATUS_NEAR_GOAL also counts as reaching the goal - if ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL - or ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL - and not self._spot_wrapper._last_trajectory_command_precise - ) + if status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL or ( + status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL + and not self._spot_wrapper._last_trajectory_command_precise ): self._spot_wrapper._robot_params["at_goal"] = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None self._spot_wrapper._trajectory_status_unknown = False - elif ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL - ): + elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL: is_moving = True - elif ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL - ): + elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL: is_moving = True self._spot_wrapper._robot_params["near_goal"] = True - elif ( - status - == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN - ): + elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN: self._spot_wrapper._trajectory_status_unknown = True self._spot_wrapper._last_trajectory_command = None else: self._logger.error( - "Received trajectory command status outside of expected range, value is {}".format( - status - ) + "Received trajectory command status outside of expected range, value is {}".format(status) ) self._spot_wrapper._last_trajectory_command = None except (ResponseError, RpcError) as e: @@ -314,9 +290,7 @@ class AsyncEStopMonitor(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, spot_wrapper): - super(AsyncEStopMonitor, self).__init__( - "estop_alive", client, logger, period_sec=1.0 / rate - ) + super(AsyncEStopMonitor, self).__init__("estop_alive", client, logger, period_sec=1.0 / rate) self._spot_wrapper = spot_wrapper def _start_query(self): @@ -325,20 +299,10 @@ def _start_query(self): return last_estop_status = self._spot_wrapper._estop_keepalive.status_queue.queue[-1] - if ( - last_estop_status[0] - == self._spot_wrapper._estop_keepalive.KeepAliveStatus.ERROR - ): - self._logger.error( - "Estop keepalive has an error: {}".format(last_estop_status[1]) - ) - elif ( - last_estop_status - == self._spot_wrapper._estop_keepalive.KeepAliveStatus.DISABLED - ): - self._logger.error( - "Estop keepalive is disabled: {}".format(last_estop_status[1]) - ) + if last_estop_status[0] == self._spot_wrapper._estop_keepalive.KeepAliveStatus.ERROR: + self._logger.error("Estop keepalive has an error: {}".format(last_estop_status[1])) + elif last_estop_status == self._spot_wrapper._estop_keepalive.KeepAliveStatus.DISABLED: + self._logger.error("Estop keepalive is disabled: {}".format(last_estop_status[1])) else: # estop keepalive is ok pass @@ -462,9 +426,7 @@ def __init__( self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) - authenticated = self.authenticate( - self._robot, self._username, self._password, self._logger - ) + authenticated = self.authenticate(self._robot, self._username, self._password, self._logger) if not authenticated: self._valid = False return @@ -479,65 +441,35 @@ def __init__( initialised = False while not initialised: try: - self._robot_state_client = self._robot.ensure_client( - RobotStateClient.default_service_name - ) - self._world_objects_client = self._robot.ensure_client( - WorldObjectClient.default_service_name - ) - self._robot_command_client = self._robot.ensure_client( - RobotCommandClient.default_service_name - ) - self._graph_nav_client = self._robot.ensure_client( - GraphNavClient.default_service_name - ) - self._map_processing_client = self._robot.ensure_client( - MapProcessingServiceClient.default_service_name - ) - self._power_client = self._robot.ensure_client( - PowerClient.default_service_name - ) - self._lease_client = self._robot.ensure_client( - LeaseClient.default_service_name - ) + self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name) + self._world_objects_client = self._robot.ensure_client(WorldObjectClient.default_service_name) + self._robot_command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) + self._graph_nav_client = self._robot.ensure_client(GraphNavClient.default_service_name) + self._map_processing_client = self._robot.ensure_client(MapProcessingServiceClient.default_service_name) + self._power_client = self._robot.ensure_client(PowerClient.default_service_name) + self._lease_client = self._robot.ensure_client(LeaseClient.default_service_name) self._lease_wallet = self._lease_client.lease_wallet - self._image_client = self._robot.ensure_client( - ImageClient.default_service_name - ) - self._estop_client = self._robot.ensure_client( - EstopClient.default_service_name - ) - self._docking_client = self._robot.ensure_client( - DockingClient.default_service_name - ) - self._spot_check_client = self._robot.ensure_client( - SpotCheckClient.default_service_name - ) + self._image_client = self._robot.ensure_client(ImageClient.default_service_name) + self._estop_client = self._robot.ensure_client(EstopClient.default_service_name) + self._docking_client = self._robot.ensure_client(DockingClient.default_service_name) + self._spot_check_client = self._robot.ensure_client(SpotCheckClient.default_service_name) try: - self._point_cloud_client = self._robot.ensure_client( - VELODYNE_SERVICE_NAME - ) + self._point_cloud_client = self._robot.ensure_client(VELODYNE_SERVICE_NAME) except UnregisteredServiceError as e: self._point_cloud_client = None self._logger.info("Velodyne point cloud service is not available.") - self._license_client = self._robot.ensure_client( - LicenseClient.default_service_name - ) + self._license_client = self._robot.ensure_client(LicenseClient.default_service_name) if HAVE_CHOREOGRAPHY_MODULE: - if self._license_client.get_feature_enabled( - [ChoreographyClient.license_name] - )[ChoreographyClient.license_name]: + if self._license_client.get_feature_enabled([ChoreographyClient.license_name])[ + ChoreographyClient.license_name + ]: self._sdk.register_service_client(ChoreographyClient) - self._choreography_client = self._robot.ensure_client( - ChoreographyClient.default_service_name - ) + self._choreography_client = self._robot.ensure_client(ChoreographyClient.default_service_name) self._is_licensed_for_choreography = True else: - self._logger.info( - f"Robot is not licensed for choreography: {e}" - ) + self._logger.info(f"Robot is not licensed for choreography: {e}") self._is_licensed_for_choreography = False self._choreography_client = None else: @@ -576,9 +508,7 @@ def __init__( sleep_secs = 15 self._logger.warning( "Unable to create client service: {}. This usually means the robot hasn't " - "finished booting yet. Will wait {} seconds and try again.".format( - e, sleep_secs - ) + "finished booting yet. Will wait {} seconds and try again.".format(e, sleep_secs) ) time.sleep(sleep_secs) @@ -602,12 +532,8 @@ def __init__( max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", None), ) - self._idle_task = AsyncIdle( - self._robot_command_client, self._logger, 10.0, self - ) - self._estop_monitor = AsyncEStopMonitor( - self._estop_client, self._logger, 20.0, self - ) + self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) + self._estop_monitor = AsyncEStopMonitor(self._estop_client, self._logger, 20.0, self) self._estop_endpoint = None self._estop_keepalive = None @@ -634,9 +560,7 @@ def __init__( "rates": self._rates, "callbacks": self._callbacks, } - self.spot_image = SpotImages( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + self.spot_image = SpotImages(self._robot, self._logger, self._robot_params, self._robot_clients) if self._robot.has_arm(): self._spot_arm = SpotArm( @@ -651,15 +575,9 @@ def __init__( else: self._spot_arm = None - self._spot_docking = SpotDocking( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._spot_graph_nav = SpotGraphNav( - self._robot, self._logger, self._robot_params, self._robot_clients - ) - self._spot_check = SpotCheck( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + self._spot_docking = SpotDocking(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_graph_nav = SpotGraphNav(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_check = SpotCheck(self._robot, self._logger, self._robot_params, self._robot_clients) self._spot_images = SpotImages( self._robot, self._logger, @@ -669,17 +587,13 @@ def __init__( ) if self._point_cloud_client: - self._spot_eap = SpotEAP( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + self._spot_eap = SpotEAP(self._robot, self._logger, self._robot_params, self._robot_clients) self._point_cloud_task = self._spot_eap.async_task robot_tasks.append(self._point_cloud_task) else: self._spot_eap = None - self._spot_world_objects = SpotWorldObjects( - self._robot, self._logger, self._robot_params, self._robot_clients - ) + self._spot_world_objects = SpotWorldObjects(self._robot, self._logger, self._robot_params, self._robot_clients) self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) @@ -747,7 +661,7 @@ def spot_images(self) -> SpotImages: def spot_arm(self) -> SpotArm: """Return SpotArm instance""" if not self._robot.has_arm(): - raise Exception("SpotArm is not available on this robot") + raise MissingSpotArm() else: return self._spot_arm @@ -876,10 +790,7 @@ def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" if self.lease is not None: for resource in self.lease: - if ( - resource.resource == "all-leases" - and SPOT_CLIENT_NAME in resource.lease_owner.client_name - ): + if resource.resource == "all-leases" and SPOT_CLIENT_NAME in resource.lease_owner.client_name: return True, "We already claimed the lease" try: @@ -908,9 +819,7 @@ def updateTasks(self): def resetEStop(self): """Get keepalive for eStop""" - self._estop_endpoint = EstopEndpoint( - self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout - ) + self._estop_endpoint = EstopEndpoint(self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout) self._estop_endpoint.force_simple_setup() # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) @@ -1023,17 +932,13 @@ def sit(self): @try_claim(power_on=True) def simple_stand(self, monitor_command=True): """If the e-stop is enabled, and the motor power is enabled, stand the robot up.""" - response = self._robot_command( - RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) - ) + response = self._robot_command(RobotCommandBuilder.synchro_stand_command(params=self._mobility_params)) if monitor_command: self._last_stand_command = response[2] return response[0], response[1] @try_claim(power_on=True) - def stand( - self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, body_roll=0 - ): + def stand(self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, body_roll=0): """ If the e-stop is enabled, and the motor power is enabled, stand the robot up. Executes a stand command, but one where the robot will assume the pose specified by the given parameters. @@ -1052,15 +957,11 @@ def stand( # If any of the orientation parameters are nonzero use them to pose the body body_orientation = EulerZXY(yaw=body_yaw, pitch=body_pitch, roll=body_roll) response = self._robot_command( - RobotCommandBuilder.synchro_stand_command( - body_height=body_height, footprint_R_body=body_orientation - ) + RobotCommandBuilder.synchro_stand_command(body_height=body_height, footprint_R_body=body_orientation) ) else: # Otherwise just use the mobility params - response = self._robot_command( - RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) - ) + response = self._robot_command(RobotCommandBuilder.synchro_stand_command(params=self._mobility_params)) if monitor_command: self._last_stand_command = response[2] @@ -1075,9 +976,7 @@ def battery_change_pose(self, dir_hint: int = 1): dir_hint: 1 rolls to the right side of the robot, 2 to the left """ if self._robot_params["is_sitting"]: - response = self._robot_command( - RobotCommandBuilder.battery_change_pose_command(dir_hint) - ) + response = self._robot_command(RobotCommandBuilder.battery_change_pose_command(dir_hint)) return response[0], response[1] return False, "Call sit before trying to roll over" @@ -1090,9 +989,7 @@ def safe_power_off(self): def clear_behavior_fault(self, id): """Clear the behavior fault defined by id.""" try: - rid = self._robot_command_client.clear_behavior_fault( - behavior_fault_id=id, lease=None - ) + rid = self._robot_command_client.clear_behavior_fault(behavior_fault_id=id, lease=None) return True, "Success", rid except Exception as e: return False, f"Exception while clearing behavior fault: {e}", None @@ -1145,9 +1042,7 @@ def velocity_cmd( """ end_time = time.time() + cmd_duration response = self._robot_command( - RobotCommandBuilder.synchro_velocity_command( - v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params - ), + RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), end_time_secs=end_time, timesync_endpoint=self._robot.time_sync.endpoint, ) @@ -1229,9 +1124,7 @@ def trajectory_cmd( self._last_trajectory_command = response[2] return response[0], response[1] - def robot_command( - self, robot_command: robot_command_pb2.RobotCommand - ) -> typing.Tuple[bool, str]: + def robot_command(self, robot_command: robot_command_pb2.RobotCommand) -> typing.Tuple[bool, str]: end_time = time.time() + MAX_COMMAND_DURATION return self._robot_command( robot_command, @@ -1239,9 +1132,7 @@ def robot_command( timesync_endpoint=self._robot.time_sync.endpoint, ) - def get_robot_command_feedback( - self, cmd_id: int - ) -> robot_command_pb2.RobotCommandFeedbackResponse: + def get_robot_command_feedback(self, cmd_id: int) -> robot_command_pb2.RobotCommandFeedbackResponse: return self._robot_command_client.robot_command_feedback(cmd_id) def check_is_powered_on(self): From 651fed617ef016c3a33e007b8097a4e5d07e8232 Mon Sep 17 00:00:00 2001 From: Ming Jie See Date: Sat, 10 Jun 2023 00:03:31 +0100 Subject: [PATCH 50/57] _get_lease private method in graphNav --- spot_wrapper/spot_graph_nav.py | 242 ++++++++++----------------------- 1 file changed, 71 insertions(+), 171 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index fac56cfc..7282025c 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -10,11 +10,7 @@ from bosdyn.api.graph_nav import nav_pb2 from bosdyn.client.frame_helpers import get_odom_tform_body from bosdyn.client.graph_nav import GraphNavClient -from bosdyn.client.lease import ( - LeaseClient, - LeaseWallet, - LeaseKeepAlive, -) +from bosdyn.client.lease import LeaseClient, LeaseWallet, LeaseKeepAlive, Lease from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.robot import Robot from bosdyn.client.robot_state import RobotStateClient @@ -32,9 +28,7 @@ def __init__( self._robot = robot self._logger = logger self._graph_nav_client: GraphNavClient = robot_clients["graph_nav_client"] - self._map_processing_client: MapProcessingServiceClient = robot_clients[ - "map_processing_client" - ] + self._map_processing_client: MapProcessingServiceClient = robot_clients["map_processing_client"] self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] self._lease_client: LeaseClient = robot_clients["lease_client"] self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet @@ -42,6 +36,10 @@ def __init__( self._init_current_graph_nav_state() + def _get_lease(self) -> Lease: + self._lease = self._lease_wallet.get_lease() + return self._lease + def _init_current_graph_nav_state(self): # Store the most recent knowledge of the state of the robot based on rpc calls. self._current_graph = None @@ -49,9 +47,7 @@ def _init_current_graph_nav_state(self): self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() - self._current_anchored_world_objects = ( - dict() - ) # maps object id to a (wo, waypoint, fiducial) + self._current_anchored_world_objects = dict() # maps object id to a (wo, waypoint, fiducial) self._current_anchors = dict() # maps anchor id to anchor def list_graph(self) -> typing.List[str]: @@ -61,12 +57,7 @@ def list_graph(self) -> typing.List[str]: """ ids, eds = self._list_graph_waypoint_and_edge_ids() - return [ - v - for k, v in sorted( - ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) - ) - ] + return [v for k, v in sorted(ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")))] def navigate_initial_localization( self, @@ -94,16 +85,14 @@ def navigate_initial_localization( self._powered_on = self._started_powered_on # Claim lease, power on robot, start graphnav. - self._lease = self._lease_wallet.get_lease() + self._lease = self._get_lease() self._lease_keepalive = LeaseKeepAlive(self._lease_client) if upload_filepath: self.clear_graph() self.upload_graph_and_snapshots(upload_filepath) else: self._download_current_graph() - self._logger.info( - "Re-using existing graph on robot. Check that the correct graph is loaded!" - ) + self._logger.info("Re-using existing graph on robot. Check that the correct graph is loaded!") if initial_localization_fiducial: self.set_initial_localization_fiducial() if initial_localization_waypoint: @@ -141,9 +130,7 @@ def download_navigation_graph(self, download_path: str) -> typing.List[str]: self._download_full_graph() return self.list_graph() - def navigation_close_loops( - self, close_fiducial_loops: bool, close_odometry_loops: bool - ) -> typing.Tuple[bool, str]: + def navigation_close_loops(self, close_fiducial_loops: bool, close_odometry_loops: bool) -> typing.Tuple[bool, str]: return self._auto_close_loops(close_fiducial_loops, close_odometry_loops) def optmize_anchoring(self) -> typing.Tuple[bool, str]: @@ -159,19 +146,13 @@ def _get_localization_state(self, *args): """Get the current localization and state of the robot.""" state = self._graph_nav_client.get_localization_state() self._logger.info(f"Got localization: \n{str(state.localization)}") - odom_tform_body = get_odom_tform_body( - state.robot_kinematics.transforms_snapshot - ) - self._logger.info( - f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}" - ) + odom_tform_body = get_odom_tform_body(state.robot_kinematics.transforms_snapshot) + self._logger.info(f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}") def set_initial_localization_fiducial(self, *args): """Trigger localization when near a fiducial.""" robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() + current_odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot).to_proto() # Create an empty instance for initial localization since we are asking it to localize # based on the nearest fiducial. localization = nav_pb2.Localization() @@ -198,9 +179,7 @@ def set_initial_localization_waypoint(self, *args): return robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body( - robot_state.kinematic_state.transforms_snapshot - ).to_proto() + current_odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot).to_proto() # Create an initial localization to the specified waypoint as the identity. localization = nav_pb2.Localization() localization.waypoint_id = destination_waypoint @@ -230,9 +209,7 @@ def _download_full_graph(self, *args): return self._write_full_graph(graph) self._logger.info( - "Graph downloaded with {} waypoints and {} edges".format( - len(graph.waypoints), len(graph.edges) - ) + "Graph downloaded with {} waypoints and {} edges".format(len(graph.waypoints), len(graph.edges)) ) # Download the waypoint and edge snapshots. self._download_and_write_waypoint_snapshots(graph.waypoints) @@ -250,14 +227,10 @@ def _download_and_write_waypoint_snapshots(self, waypoints): if len(waypoint.snapshot_id) == 0: continue try: - waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot( - waypoint.snapshot_id - ) + waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot(waypoint.snapshot_id) except Exception: # Failure in downloading waypoint snapshot. Continue to next snapshot. - self._logger.error( - "Failed to download waypoint snapshot: " + waypoint.snapshot_id - ) + self._logger.error("Failed to download waypoint snapshot: " + waypoint.snapshot_id) continue self._write_bytes( self._download_filepath + "/waypoint_snapshots", @@ -280,14 +253,10 @@ def _download_and_write_edge_snapshots(self, edges): continue num_to_download += 1 try: - edge_snapshot = self._graph_nav_client.download_edge_snapshot( - edge.snapshot_id - ) + edge_snapshot = self._graph_nav_client.download_edge_snapshot(edge.snapshot_id) except Exception: # Failure in downloading edge snapshot. Continue to next snapshot. - self._logger.error( - "Failed to download edge snapshot: " + edge.snapshot_id - ) + self._logger.error("Failed to download edge snapshot: " + edge.snapshot_id) continue self._write_bytes( self._download_filepath + "/edge_snapshots", @@ -296,9 +265,7 @@ def _download_and_write_edge_snapshots(self, edges): ) num_edge_snapshots_downloaded += 1 self._logger.info( - "Downloaded {} of the total {} edge snapshots.".format( - num_edge_snapshots_downloaded, num_to_download - ) + "Downloaded {} of the total {} edge snapshots.".format(num_edge_snapshots_downloaded, num_to_download) ) def _write_bytes(self, filepath: str, filename: str, data): @@ -314,9 +281,7 @@ def _list_graph_waypoint_and_edge_ids(self, *args): # Download current graph graph = self._download_current_graph() - localization_id = ( - self._graph_nav_client.get_localization_state().localization.waypoint_id - ) + localization_id = self._graph_nav_client.get_localization_state().localization.waypoint_id # Update and print waypoints and edges ( @@ -342,17 +307,13 @@ def upload_graph_and_snapshots(self, upload_filepath: str): # Load the waypoint snapshots from disk. if len(waypoint.snapshot_id) == 0: continue - waypoint_filepath = os.path.join( - upload_filepath, "waypoint_snapshots", waypoint.snapshot_id - ) + waypoint_filepath = os.path.join(upload_filepath, "waypoint_snapshots", waypoint.snapshot_id) if not os.path.exists(waypoint_filepath): continue with open(waypoint_filepath, "rb") as snapshot_file: waypoint_snapshot = map_pb2.WaypointSnapshot() waypoint_snapshot.ParseFromString(snapshot_file.read()) - self._current_waypoint_snapshots[ - waypoint_snapshot.id - ] = waypoint_snapshot + self._current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot for fiducial in waypoint_snapshot.objects: if not fiducial.HasField("apriltag_properties"): @@ -374,9 +335,7 @@ def upload_graph_and_snapshots(self, upload_filepath: str): # Load the edge snapshots from disk. if len(edge.snapshot_id) == 0: continue - edge_filepath = os.path.join( - upload_filepath, "edge_snapshots", edge.snapshot_id - ) + edge_filepath = os.path.join(upload_filepath, "edge_snapshots", edge.snapshot_id) if not os.path.exists(edge_filepath): continue with open(edge_filepath, "rb") as snapshot_file: @@ -394,9 +353,7 @@ def upload_graph_and_snapshots(self, upload_filepath: str): ) return - self._graph_nav_client.upload_graph( - lease=self._lease.lease_proto, graph=self._current_graph - ) + self._graph_nav_client.upload_graph(lease=self._lease.lease_proto, graph=self._current_graph) # Upload the snapshots to the robot. for waypoint_snapshot in self._current_waypoint_snapshots.values(): self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) @@ -418,7 +375,7 @@ def upload_graph_and_snapshots(self, upload_filepath: str): def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: """Navigate to a specific waypoint.""" - self._lease = self._lease_wallet.get_lease() + self._lease = self._get_lease() destination_waypoint = self._find_unique_waypoint_id( waypoint_id, self._current_graph, @@ -426,9 +383,7 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: self._logger, ) if not destination_waypoint: - self._logger.error( - "Failed to find the appropriate unique waypoint id for the navigation command." - ) + self._logger.error("Failed to find the appropriate unique waypoint id for the navigation command.") return ( False, "Failed to find the appropriate unique waypoint id for the navigation command.", @@ -445,9 +400,7 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: while not is_finished: # Issue the navigation command about twice a second such that it is easy to terminate the # navigation command (with estop or killing the program). - nav_to_cmd_id = self._graph_nav_client.navigate_to( - destination_waypoint, 1.0, leases=[sublease.lease_proto] - ) + nav_to_cmd_id = self._graph_nav_client.navigate_to(destination_waypoint, 1.0, leases=[sublease.lease_proto]) time.sleep(0.5) # Sleep for half a second to allow for command execution. # Poll the robot for feedback to determine if the navigation command is complete. is_finished = self._check_success(nav_to_cmd_id) @@ -456,10 +409,7 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: self._lease_keepalive = LeaseKeepAlive(self._lease_client) status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): + if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: return True, "Successfully completed the navigation commands!" elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: return ( @@ -471,17 +421,12 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: False, "Robot got stuck when navigating the route, the robot will now sit down.", ) - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: return False, "Robot is impaired." else: return False, "Navigation command is not complete yet." - def _navigate_route( - self, waypoint_ids: typing.List[str] - ) -> typing.Tuple[bool, str]: + def _navigate_route(self, waypoint_ids: typing.List[str]) -> typing.Tuple[bool, str]: """Navigate through a specific route of waypoints. Note that each waypoint must have an edge between them, aka be adjacent. """ @@ -493,9 +438,7 @@ def _navigate_route( self._logger, ) if not waypoint_ids[i]: - self._logger.error( - "navigate_route: Failed to find the unique waypoint id." - ) + self._logger.error("navigate_route: Failed to find the unique waypoint id.") return False, "Failed to find the unique waypoint id." edge_ids_list = [] @@ -508,9 +451,7 @@ def _navigate_route( if edge_id is not None: edge_ids_list.append(edge_id) else: - self._logger.error( - f"Failed to find an edge between waypoints: {start_wp} and {end_wp}" - ) + self._logger.error(f"Failed to find an edge between waypoints: {start_wp} and {end_wp}") return ( False, f"Failed to find an edge between waypoints: {start_wp} and {end_wp}", @@ -541,6 +482,7 @@ def _navigate_route( def clear_graph(self, *args) -> bool: """Clear the state of the map on the robot, removing all waypoints and edges.""" + self._lease = self._get_lease() result = self._graph_nav_client.clear_graph(lease=self._lease.lease_proto) self._init_current_graph_nav_state() return result @@ -551,26 +493,16 @@ def _check_success(self, command_id: int = -1) -> bool: # No command, so we have not status to check. return False status = self._graph_nav_client.navigation_feedback(command_id) - if ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL - ): + if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: # Successfully completed the navigation commands! return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - self._logger.error( - "Robot got lost when navigating the route, the robot will now sit down." - ) + self._logger.error("Robot got lost when navigating the route, the robot will now sit down.") return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - self._logger.error( - "Robot got stuck when navigating the route, the robot will now sit down." - ) + self._logger.error("Robot got stuck when navigating the route, the robot will now sit down.") return True - elif ( - status.status - == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED - ): + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: self._logger.error("Robot is impaired.") return True else: @@ -589,74 +521,49 @@ def _match_edge( for edge_from_id in current_edges[edge_to_id]: if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint2, to_waypoint=waypoint1 - ) + return map_pb2.Edge.Id(from_waypoint=waypoint2, to_waypoint=waypoint1) elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id( - from_waypoint=waypoint1, to_waypoint=waypoint2 - ) + return map_pb2.Edge.Id(from_waypoint=waypoint1, to_waypoint=waypoint2) return None - def _auto_close_loops( - self, close_fiducial_loops: bool, close_odometry_loops: bool, *args - ): + def _auto_close_loops(self, close_fiducial_loops: bool, close_odometry_loops: bool, *args): """Automatically find and close all loops in the graph.""" - response: map_processing_pb2.ProcessTopologyResponse = ( - self._map_processing_client.process_topology( - params=map_processing_pb2.ProcessTopologyRequest.Params( - do_fiducial_loop_closure=wrappers_pb2.BoolValue( - value=close_fiducial_loops - ), - do_odometry_loop_closure=wrappers_pb2.BoolValue( - value=close_odometry_loops - ), - ), - modify_map_on_server=True, - ) - ) - self._logger.info( - "Created {} new edge(s).".format(len(response.new_subgraph.edges)) + response: map_processing_pb2.ProcessTopologyResponse = self._map_processing_client.process_topology( + params=map_processing_pb2.ProcessTopologyRequest.Params( + do_fiducial_loop_closure=wrappers_pb2.BoolValue(value=close_fiducial_loops), + do_odometry_loop_closure=wrappers_pb2.BoolValue(value=close_odometry_loops), + ), + modify_map_on_server=True, ) + self._logger.info("Created {} new edge(s).".format(len(response.new_subgraph.edges))) if response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_OK: return True, "Successfully closed loops." - elif ( - response.status - == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS - ): + elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: return False, "Missing waypoint snapshots." - elif ( - response.status - == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH - ): + elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH: return False, "Invalid graph." - elif ( - response.status - == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING - ): + elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: return False, "Map modified during processing." else: return False, "Unknown error during map processing." def _optimize_anchoring(self, *args): """Call anchoring optimization on the server, producing a globally optimal reference frame for waypoints to be expressed in.""" - response: map_processing_pb2.ProcessAnchoringResponse = ( - self._map_processing_client.process_anchoring( - params=map_processing_pb2.ProcessAnchoringRequest.Params(), - modify_anchoring_on_server=True, - stream_intermediate_results=False, - ) + response: map_processing_pb2.ProcessAnchoringResponse = self._map_processing_client.process_anchoring( + params=map_processing_pb2.ProcessAnchoringRequest.Params(), + modify_anchoring_on_server=True, + stream_intermediate_results=False, ) if response.status == map_processing_pb2.ProcessAnchoringResponse.STATUS_OK: - self._logger.info( - "Optimized anchoring after {} iteration(s).".format(response.iteration) - ) + self._logger.info("Optimized anchoring after {} iteration(s).".format(response.iteration)) return True, "Successfully optimized anchoring." else: self._logger.error("Error optimizing {}".format(response)) status_mapping = { - map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: "Missing waypoint snapshots.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: ( + "Missing waypoint snapshots." + ), map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAPH: "Invalid graph.", map_processing_pb2.ProcessAnchoringResponse.STATUS_OPTIMIZATION_FAILURE: "Optimization failure.", map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_PARAMS: "Invalid parameters.", @@ -664,8 +571,12 @@ def _optimize_anchoring(self, *args): map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_ITERATIONS: "Max iterations, timeout.", map_processing_pb2.ProcessAnchoringResponse.STATUS_MAX_TIME: "Max time reached, timeout.", map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_HINTS: "Invalid hints.", - map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: "Map modified during processing.", - map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT: "Invalid gravity alignment.", + map_processing_pb2.ProcessAnchoringResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: ( + "Map modified during processing." + ), + map_processing_pb2.ProcessAnchoringResponse.STATUS_INVALID_GRAVITY_ALIGNMENT: ( + "Invalid gravity alignment." + ), } if response.status in status_mapping: return False, status_mapping[response.status] @@ -747,17 +658,12 @@ def _update_waypoints_and_edges( # Determine the timestamp that this waypoint was created at. timestamp = -1.0 try: - timestamp = ( - waypoint.annotations.creation_time.seconds - + waypoint.annotations.creation_time.nanos / 1e9 - ) + timestamp = waypoint.annotations.creation_time.seconds + waypoint.annotations.creation_time.nanos / 1e9 except: # Must be operating on an older graph nav map, since the creation_time is not # available within the waypoint annotations message. pass - waypoint_to_timestamp.append( - (waypoint.id, timestamp, waypoint.annotations.name) - ) + waypoint_to_timestamp.append((waypoint.id, timestamp, waypoint.annotations.name)) # Determine how many waypoints have the same short code. short_code = self._id_to_short_code(waypoint.id) @@ -779,17 +685,13 @@ def _update_waypoints_and_edges( # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, # fallback to sorting by annotation name. - waypoint_to_timestamp = sorted( - waypoint_to_timestamp, key=lambda x: (x[1], x[2]) - ) + waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from # when the waypoint was created. logger.info("%d waypoints:" % len(graph.waypoints)) for waypoint in waypoint_to_timestamp: - self._pretty_print_waypoints( - waypoint[0], waypoint[2], short_code_to_count, localization_id, logger - ) + self._pretty_print_waypoints(waypoint[0], waypoint[2], short_code_to_count, localization_id, logger) for edge in graph.edges: if edge.id.to_waypoint in edges: @@ -797,8 +699,6 @@ def _update_waypoints_and_edges( edges[edge.id.to_waypoint].append(edge.id.from_waypoint) else: edges[edge.id.to_waypoint] = [edge.id.from_waypoint] - logger.info( - f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}" - ) + logger.info(f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}") return name_to_id, edges From d64f65faa6e026fd5f7af824573ba40e307bad73 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 10 Jun 2023 17:07:57 +0100 Subject: [PATCH 51/57] fix black --- spot_wrapper/spot_graph_nav.py | 215 ++++++++++++++++++++++-------- spot_wrapper/wrapper.py | 235 ++++++++++++++++++++++++--------- 2 files changed, 337 insertions(+), 113 deletions(-) diff --git a/spot_wrapper/spot_graph_nav.py b/spot_wrapper/spot_graph_nav.py index 7282025c..ff383900 100644 --- a/spot_wrapper/spot_graph_nav.py +++ b/spot_wrapper/spot_graph_nav.py @@ -28,7 +28,9 @@ def __init__( self._robot = robot self._logger = logger self._graph_nav_client: GraphNavClient = robot_clients["graph_nav_client"] - self._map_processing_client: MapProcessingServiceClient = robot_clients["map_processing_client"] + self._map_processing_client: MapProcessingServiceClient = robot_clients[ + "map_processing_client" + ] self._robot_state_client: RobotStateClient = robot_clients["robot_state_client"] self._lease_client: LeaseClient = robot_clients["lease_client"] self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet @@ -47,7 +49,9 @@ def _init_current_graph_nav_state(self): self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot self._current_edge_snapshots = dict() # maps id to edge snapshot self._current_annotation_name_to_wp_id = dict() - self._current_anchored_world_objects = dict() # maps object id to a (wo, waypoint, fiducial) + self._current_anchored_world_objects = ( + dict() + ) # maps object id to a (wo, waypoint, fiducial) self._current_anchors = dict() # maps anchor id to anchor def list_graph(self) -> typing.List[str]: @@ -57,7 +61,12 @@ def list_graph(self) -> typing.List[str]: """ ids, eds = self._list_graph_waypoint_and_edge_ids() - return [v for k, v in sorted(ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")))] + return [ + v + for k, v in sorted( + ids.items(), key=lambda id: int(id[0].replace("waypoint_", "")) + ) + ] def navigate_initial_localization( self, @@ -92,7 +101,9 @@ def navigate_initial_localization( self.upload_graph_and_snapshots(upload_filepath) else: self._download_current_graph() - self._logger.info("Re-using existing graph on robot. Check that the correct graph is loaded!") + self._logger.info( + "Re-using existing graph on robot. Check that the correct graph is loaded!" + ) if initial_localization_fiducial: self.set_initial_localization_fiducial() if initial_localization_waypoint: @@ -130,7 +141,9 @@ def download_navigation_graph(self, download_path: str) -> typing.List[str]: self._download_full_graph() return self.list_graph() - def navigation_close_loops(self, close_fiducial_loops: bool, close_odometry_loops: bool) -> typing.Tuple[bool, str]: + def navigation_close_loops( + self, close_fiducial_loops: bool, close_odometry_loops: bool + ) -> typing.Tuple[bool, str]: return self._auto_close_loops(close_fiducial_loops, close_odometry_loops) def optmize_anchoring(self) -> typing.Tuple[bool, str]: @@ -146,13 +159,19 @@ def _get_localization_state(self, *args): """Get the current localization and state of the robot.""" state = self._graph_nav_client.get_localization_state() self._logger.info(f"Got localization: \n{str(state.localization)}") - odom_tform_body = get_odom_tform_body(state.robot_kinematics.transforms_snapshot) - self._logger.info(f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}") + odom_tform_body = get_odom_tform_body( + state.robot_kinematics.transforms_snapshot + ) + self._logger.info( + f"Got robot state in kinematic odometry frame: \n{str(odom_tform_body)}" + ) def set_initial_localization_fiducial(self, *args): """Trigger localization when near a fiducial.""" robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot).to_proto() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() # Create an empty instance for initial localization since we are asking it to localize # based on the nearest fiducial. localization = nav_pb2.Localization() @@ -179,7 +198,9 @@ def set_initial_localization_waypoint(self, *args): return robot_state = self._robot_state_client.get_robot_state() - current_odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot).to_proto() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot + ).to_proto() # Create an initial localization to the specified waypoint as the identity. localization = nav_pb2.Localization() localization.waypoint_id = destination_waypoint @@ -209,7 +230,9 @@ def _download_full_graph(self, *args): return self._write_full_graph(graph) self._logger.info( - "Graph downloaded with {} waypoints and {} edges".format(len(graph.waypoints), len(graph.edges)) + "Graph downloaded with {} waypoints and {} edges".format( + len(graph.waypoints), len(graph.edges) + ) ) # Download the waypoint and edge snapshots. self._download_and_write_waypoint_snapshots(graph.waypoints) @@ -227,10 +250,14 @@ def _download_and_write_waypoint_snapshots(self, waypoints): if len(waypoint.snapshot_id) == 0: continue try: - waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot(waypoint.snapshot_id) + waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot( + waypoint.snapshot_id + ) except Exception: # Failure in downloading waypoint snapshot. Continue to next snapshot. - self._logger.error("Failed to download waypoint snapshot: " + waypoint.snapshot_id) + self._logger.error( + "Failed to download waypoint snapshot: " + waypoint.snapshot_id + ) continue self._write_bytes( self._download_filepath + "/waypoint_snapshots", @@ -253,10 +280,14 @@ def _download_and_write_edge_snapshots(self, edges): continue num_to_download += 1 try: - edge_snapshot = self._graph_nav_client.download_edge_snapshot(edge.snapshot_id) + edge_snapshot = self._graph_nav_client.download_edge_snapshot( + edge.snapshot_id + ) except Exception: # Failure in downloading edge snapshot. Continue to next snapshot. - self._logger.error("Failed to download edge snapshot: " + edge.snapshot_id) + self._logger.error( + "Failed to download edge snapshot: " + edge.snapshot_id + ) continue self._write_bytes( self._download_filepath + "/edge_snapshots", @@ -265,7 +296,9 @@ def _download_and_write_edge_snapshots(self, edges): ) num_edge_snapshots_downloaded += 1 self._logger.info( - "Downloaded {} of the total {} edge snapshots.".format(num_edge_snapshots_downloaded, num_to_download) + "Downloaded {} of the total {} edge snapshots.".format( + num_edge_snapshots_downloaded, num_to_download + ) ) def _write_bytes(self, filepath: str, filename: str, data): @@ -281,7 +314,9 @@ def _list_graph_waypoint_and_edge_ids(self, *args): # Download current graph graph = self._download_current_graph() - localization_id = self._graph_nav_client.get_localization_state().localization.waypoint_id + localization_id = ( + self._graph_nav_client.get_localization_state().localization.waypoint_id + ) # Update and print waypoints and edges ( @@ -307,13 +342,17 @@ def upload_graph_and_snapshots(self, upload_filepath: str): # Load the waypoint snapshots from disk. if len(waypoint.snapshot_id) == 0: continue - waypoint_filepath = os.path.join(upload_filepath, "waypoint_snapshots", waypoint.snapshot_id) + waypoint_filepath = os.path.join( + upload_filepath, "waypoint_snapshots", waypoint.snapshot_id + ) if not os.path.exists(waypoint_filepath): continue with open(waypoint_filepath, "rb") as snapshot_file: waypoint_snapshot = map_pb2.WaypointSnapshot() waypoint_snapshot.ParseFromString(snapshot_file.read()) - self._current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot + self._current_waypoint_snapshots[ + waypoint_snapshot.id + ] = waypoint_snapshot for fiducial in waypoint_snapshot.objects: if not fiducial.HasField("apriltag_properties"): @@ -335,7 +374,9 @@ def upload_graph_and_snapshots(self, upload_filepath: str): # Load the edge snapshots from disk. if len(edge.snapshot_id) == 0: continue - edge_filepath = os.path.join(upload_filepath, "edge_snapshots", edge.snapshot_id) + edge_filepath = os.path.join( + upload_filepath, "edge_snapshots", edge.snapshot_id + ) if not os.path.exists(edge_filepath): continue with open(edge_filepath, "rb") as snapshot_file: @@ -353,7 +394,9 @@ def upload_graph_and_snapshots(self, upload_filepath: str): ) return - self._graph_nav_client.upload_graph(lease=self._lease.lease_proto, graph=self._current_graph) + self._graph_nav_client.upload_graph( + lease=self._lease.lease_proto, graph=self._current_graph + ) # Upload the snapshots to the robot. for waypoint_snapshot in self._current_waypoint_snapshots.values(): self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) @@ -383,7 +426,9 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: self._logger, ) if not destination_waypoint: - self._logger.error("Failed to find the appropriate unique waypoint id for the navigation command.") + self._logger.error( + "Failed to find the appropriate unique waypoint id for the navigation command." + ) return ( False, "Failed to find the appropriate unique waypoint id for the navigation command.", @@ -400,7 +445,9 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: while not is_finished: # Issue the navigation command about twice a second such that it is easy to terminate the # navigation command (with estop or killing the program). - nav_to_cmd_id = self._graph_nav_client.navigate_to(destination_waypoint, 1.0, leases=[sublease.lease_proto]) + nav_to_cmd_id = self._graph_nav_client.navigate_to( + destination_waypoint, 1.0, leases=[sublease.lease_proto] + ) time.sleep(0.5) # Sleep for half a second to allow for command execution. # Poll the robot for feedback to determine if the navigation command is complete. is_finished = self._check_success(nav_to_cmd_id) @@ -409,7 +456,10 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: self._lease_keepalive = LeaseKeepAlive(self._lease_client) status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) - if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): return True, "Successfully completed the navigation commands!" elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: return ( @@ -421,12 +471,17 @@ def _navigate_to(self, waypoint_id: str) -> typing.Tuple[bool, str]: False, "Robot got stuck when navigating the route, the robot will now sit down.", ) - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): return False, "Robot is impaired." else: return False, "Navigation command is not complete yet." - def _navigate_route(self, waypoint_ids: typing.List[str]) -> typing.Tuple[bool, str]: + def _navigate_route( + self, waypoint_ids: typing.List[str] + ) -> typing.Tuple[bool, str]: """Navigate through a specific route of waypoints. Note that each waypoint must have an edge between them, aka be adjacent. """ @@ -438,7 +493,9 @@ def _navigate_route(self, waypoint_ids: typing.List[str]) -> typing.Tuple[bool, self._logger, ) if not waypoint_ids[i]: - self._logger.error("navigate_route: Failed to find the unique waypoint id.") + self._logger.error( + "navigate_route: Failed to find the unique waypoint id." + ) return False, "Failed to find the unique waypoint id." edge_ids_list = [] @@ -451,7 +508,9 @@ def _navigate_route(self, waypoint_ids: typing.List[str]) -> typing.Tuple[bool, if edge_id is not None: edge_ids_list.append(edge_id) else: - self._logger.error(f"Failed to find an edge between waypoints: {start_wp} and {end_wp}") + self._logger.error( + f"Failed to find an edge between waypoints: {start_wp} and {end_wp}" + ) return ( False, f"Failed to find an edge between waypoints: {start_wp} and {end_wp}", @@ -493,16 +552,26 @@ def _check_success(self, command_id: int = -1) -> bool: # No command, so we have not status to check. return False status = self._graph_nav_client.navigation_feedback(command_id) - if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): # Successfully completed the navigation commands! return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: - self._logger.error("Robot got lost when navigating the route, the robot will now sit down.") + self._logger.error( + "Robot got lost when navigating the route, the robot will now sit down." + ) return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: - self._logger.error("Robot got stuck when navigating the route, the robot will now sit down.") + self._logger.error( + "Robot got stuck when navigating the route, the robot will now sit down." + ) return True - elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): self._logger.error("Robot is impaired.") return True else: @@ -521,42 +590,69 @@ def _match_edge( for edge_from_id in current_edges[edge_to_id]: if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id(from_waypoint=waypoint2, to_waypoint=waypoint1) + return map_pb2.Edge.Id( + from_waypoint=waypoint2, to_waypoint=waypoint1 + ) elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): # This edge matches the pair of waypoints! Add it the edge list and continue. - return map_pb2.Edge.Id(from_waypoint=waypoint1, to_waypoint=waypoint2) + return map_pb2.Edge.Id( + from_waypoint=waypoint1, to_waypoint=waypoint2 + ) return None - def _auto_close_loops(self, close_fiducial_loops: bool, close_odometry_loops: bool, *args): + def _auto_close_loops( + self, close_fiducial_loops: bool, close_odometry_loops: bool, *args + ): """Automatically find and close all loops in the graph.""" - response: map_processing_pb2.ProcessTopologyResponse = self._map_processing_client.process_topology( - params=map_processing_pb2.ProcessTopologyRequest.Params( - do_fiducial_loop_closure=wrappers_pb2.BoolValue(value=close_fiducial_loops), - do_odometry_loop_closure=wrappers_pb2.BoolValue(value=close_odometry_loops), - ), - modify_map_on_server=True, + response: map_processing_pb2.ProcessTopologyResponse = ( + self._map_processing_client.process_topology( + params=map_processing_pb2.ProcessTopologyRequest.Params( + do_fiducial_loop_closure=wrappers_pb2.BoolValue( + value=close_fiducial_loops + ), + do_odometry_loop_closure=wrappers_pb2.BoolValue( + value=close_odometry_loops + ), + ), + modify_map_on_server=True, + ) + ) + self._logger.info( + "Created {} new edge(s).".format(len(response.new_subgraph.edges)) ) - self._logger.info("Created {} new edge(s).".format(len(response.new_subgraph.edges))) if response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_OK: return True, "Successfully closed loops." - elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS: + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_MISSING_WAYPOINT_SNAPSHOTS + ): return False, "Missing waypoint snapshots." - elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH: + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_INVALID_GRAPH + ): return False, "Invalid graph." - elif response.status == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING: + elif ( + response.status + == map_processing_pb2.ProcessTopologyResponse.STATUS_MAP_MODIFIED_DURING_PROCESSING + ): return False, "Map modified during processing." else: return False, "Unknown error during map processing." def _optimize_anchoring(self, *args): """Call anchoring optimization on the server, producing a globally optimal reference frame for waypoints to be expressed in.""" - response: map_processing_pb2.ProcessAnchoringResponse = self._map_processing_client.process_anchoring( - params=map_processing_pb2.ProcessAnchoringRequest.Params(), - modify_anchoring_on_server=True, - stream_intermediate_results=False, + response: map_processing_pb2.ProcessAnchoringResponse = ( + self._map_processing_client.process_anchoring( + params=map_processing_pb2.ProcessAnchoringRequest.Params(), + modify_anchoring_on_server=True, + stream_intermediate_results=False, + ) ) if response.status == map_processing_pb2.ProcessAnchoringResponse.STATUS_OK: - self._logger.info("Optimized anchoring after {} iteration(s).".format(response.iteration)) + self._logger.info( + "Optimized anchoring after {} iteration(s).".format(response.iteration) + ) return True, "Successfully optimized anchoring." else: self._logger.error("Error optimizing {}".format(response)) @@ -658,12 +754,17 @@ def _update_waypoints_and_edges( # Determine the timestamp that this waypoint was created at. timestamp = -1.0 try: - timestamp = waypoint.annotations.creation_time.seconds + waypoint.annotations.creation_time.nanos / 1e9 + timestamp = ( + waypoint.annotations.creation_time.seconds + + waypoint.annotations.creation_time.nanos / 1e9 + ) except: # Must be operating on an older graph nav map, since the creation_time is not # available within the waypoint annotations message. pass - waypoint_to_timestamp.append((waypoint.id, timestamp, waypoint.annotations.name)) + waypoint_to_timestamp.append( + (waypoint.id, timestamp, waypoint.annotations.name) + ) # Determine how many waypoints have the same short code. short_code = self._id_to_short_code(waypoint.id) @@ -685,13 +786,17 @@ def _update_waypoints_and_edges( # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, # fallback to sorting by annotation name. - waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) + waypoint_to_timestamp = sorted( + waypoint_to_timestamp, key=lambda x: (x[1], x[2]) + ) # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from # when the waypoint was created. logger.info("%d waypoints:" % len(graph.waypoints)) for waypoint in waypoint_to_timestamp: - self._pretty_print_waypoints(waypoint[0], waypoint[2], short_code_to_count, localization_id, logger) + self._pretty_print_waypoints( + waypoint[0], waypoint[2], short_code_to_count, localization_id, logger + ) for edge in graph.edges: if edge.id.to_waypoint in edges: @@ -699,6 +804,8 @@ def _update_waypoints_and_edges( edges[edge.id.to_waypoint].append(edge.id.from_waypoint) else: edges[edge.id.to_waypoint] = [edge.id.from_waypoint] - logger.info(f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}") + logger.info( + f"(Edge) from waypoint id: {edge.id.from_waypoint} and to waypoint id: {edge.id.to_waypoint}" + ) return name_to_id, edges diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 5c03d507..5952c9aa 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -115,7 +115,9 @@ class AsyncRobotState(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncRobotState, self).__init__("robot-state", client, logger, period_sec=1.0 / max(rate, 1.0)) + super(AsyncRobotState, self).__init__( + "robot-state", client, logger, period_sec=1.0 / max(rate, 1.0) + ) self._callback = None if rate > 0.0: self._callback = callback @@ -138,7 +140,9 @@ class AsyncMetrics(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncMetrics, self).__init__("robot-metrics", client, logger, period_sec=1.0 / max(rate, 1.0)) + super(AsyncMetrics, self).__init__( + "robot-metrics", client, logger, period_sec=1.0 / max(rate, 1.0) + ) self._callback = None if rate > 0.0: self._callback = callback @@ -161,7 +165,9 @@ class AsyncLease(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, callback): - super(AsyncLease, self).__init__("lease", client, logger, period_sec=1.0 / max(rate, 1.0)) + super(AsyncLease, self).__init__( + "lease", client, logger, period_sec=1.0 / max(rate, 1.0) + ) self._callback = None if rate > 0.0: self._callback = callback @@ -191,13 +197,19 @@ def __init__(self, client, logger, rate, spot_wrapper): def _start_query(self): if self._spot_wrapper._last_stand_command != None: try: - response = self._client.robot_command_feedback(self._spot_wrapper._last_stand_command) - status = response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status + response = self._client.robot_command_feedback( + self._spot_wrapper._last_stand_command + ) + status = ( + response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status + ) self._spot_wrapper._robot_params["is_sitting"] = False if status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING: self._spot_wrapper._robot_params["is_standing"] = True self._spot_wrapper._last_stand_command = None - elif status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS: + elif ( + status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS + ): self._spot_wrapper._robot_params["is_standing"] = False else: self._logger.warning("Stand command in unknown state") @@ -209,7 +221,9 @@ def _start_query(self): if self._spot_wrapper._last_sit_command != None: try: self._spot_wrapper._robot_params["is_standing"] = False - response = self._client.robot_command_feedback(self._spot_wrapper._last_sit_command) + response = self._client.robot_command_feedback( + self._spot_wrapper._last_sit_command + ) if ( response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status == basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING @@ -232,31 +246,49 @@ def _start_query(self): if self._spot_wrapper._last_trajectory_command != None: try: - response = self._client.robot_command_feedback(self._spot_wrapper._last_trajectory_command) + response = self._client.robot_command_feedback( + self._spot_wrapper._last_trajectory_command + ) status = ( response.feedback.synchronized_feedback.mobility_command_feedback.se2_trajectory_feedback.status ) # STATUS_AT_GOAL always means that the robot reached the goal. If the trajectory command did not # request precise positioning, then STATUS_NEAR_GOAL also counts as reaching the goal - if status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL or ( - status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL - and not self._spot_wrapper._last_trajectory_command_precise + if ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL + or ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL + and not self._spot_wrapper._last_trajectory_command_precise + ) ): self._spot_wrapper._robot_params["at_goal"] = True # Clear the command once at the goal self._spot_wrapper._last_trajectory_command = None self._spot_wrapper._trajectory_status_unknown = False - elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL: + elif ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL + ): is_moving = True - elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL: + elif ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL + ): is_moving = True self._spot_wrapper._robot_params["near_goal"] = True - elif status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN: + elif ( + status + == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN + ): self._spot_wrapper._trajectory_status_unknown = True self._spot_wrapper._last_trajectory_command = None else: self._logger.error( - "Received trajectory command status outside of expected range, value is {}".format(status) + "Received trajectory command status outside of expected range, value is {}".format( + status + ) ) self._spot_wrapper._last_trajectory_command = None except (ResponseError, RpcError) as e: @@ -290,7 +322,9 @@ class AsyncEStopMonitor(AsyncPeriodicQuery): """ def __init__(self, client, logger, rate, spot_wrapper): - super(AsyncEStopMonitor, self).__init__("estop_alive", client, logger, period_sec=1.0 / rate) + super(AsyncEStopMonitor, self).__init__( + "estop_alive", client, logger, period_sec=1.0 / rate + ) self._spot_wrapper = spot_wrapper def _start_query(self): @@ -299,10 +333,20 @@ def _start_query(self): return last_estop_status = self._spot_wrapper._estop_keepalive.status_queue.queue[-1] - if last_estop_status[0] == self._spot_wrapper._estop_keepalive.KeepAliveStatus.ERROR: - self._logger.error("Estop keepalive has an error: {}".format(last_estop_status[1])) - elif last_estop_status == self._spot_wrapper._estop_keepalive.KeepAliveStatus.DISABLED: - self._logger.error("Estop keepalive is disabled: {}".format(last_estop_status[1])) + if ( + last_estop_status[0] + == self._spot_wrapper._estop_keepalive.KeepAliveStatus.ERROR + ): + self._logger.error( + "Estop keepalive has an error: {}".format(last_estop_status[1]) + ) + elif ( + last_estop_status + == self._spot_wrapper._estop_keepalive.KeepAliveStatus.DISABLED + ): + self._logger.error( + "Estop keepalive is disabled: {}".format(last_estop_status[1]) + ) else: # estop keepalive is ok pass @@ -426,7 +470,9 @@ def __init__( self._logger.info("Initialising robot at {}".format(self._hostname)) self._robot = self._sdk.create_robot(self._hostname) - authenticated = self.authenticate(self._robot, self._username, self._password, self._logger) + authenticated = self.authenticate( + self._robot, self._username, self._password, self._logger + ) if not authenticated: self._valid = False return @@ -441,35 +487,65 @@ def __init__( initialised = False while not initialised: try: - self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name) - self._world_objects_client = self._robot.ensure_client(WorldObjectClient.default_service_name) - self._robot_command_client = self._robot.ensure_client(RobotCommandClient.default_service_name) - self._graph_nav_client = self._robot.ensure_client(GraphNavClient.default_service_name) - self._map_processing_client = self._robot.ensure_client(MapProcessingServiceClient.default_service_name) - self._power_client = self._robot.ensure_client(PowerClient.default_service_name) - self._lease_client = self._robot.ensure_client(LeaseClient.default_service_name) + self._robot_state_client = self._robot.ensure_client( + RobotStateClient.default_service_name + ) + self._world_objects_client = self._robot.ensure_client( + WorldObjectClient.default_service_name + ) + self._robot_command_client = self._robot.ensure_client( + RobotCommandClient.default_service_name + ) + self._graph_nav_client = self._robot.ensure_client( + GraphNavClient.default_service_name + ) + self._map_processing_client = self._robot.ensure_client( + MapProcessingServiceClient.default_service_name + ) + self._power_client = self._robot.ensure_client( + PowerClient.default_service_name + ) + self._lease_client = self._robot.ensure_client( + LeaseClient.default_service_name + ) self._lease_wallet = self._lease_client.lease_wallet - self._image_client = self._robot.ensure_client(ImageClient.default_service_name) - self._estop_client = self._robot.ensure_client(EstopClient.default_service_name) - self._docking_client = self._robot.ensure_client(DockingClient.default_service_name) - self._spot_check_client = self._robot.ensure_client(SpotCheckClient.default_service_name) + self._image_client = self._robot.ensure_client( + ImageClient.default_service_name + ) + self._estop_client = self._robot.ensure_client( + EstopClient.default_service_name + ) + self._docking_client = self._robot.ensure_client( + DockingClient.default_service_name + ) + self._spot_check_client = self._robot.ensure_client( + SpotCheckClient.default_service_name + ) try: - self._point_cloud_client = self._robot.ensure_client(VELODYNE_SERVICE_NAME) + self._point_cloud_client = self._robot.ensure_client( + VELODYNE_SERVICE_NAME + ) except UnregisteredServiceError as e: self._point_cloud_client = None self._logger.info("Velodyne point cloud service is not available.") - self._license_client = self._robot.ensure_client(LicenseClient.default_service_name) + self._license_client = self._robot.ensure_client( + LicenseClient.default_service_name + ) if HAVE_CHOREOGRAPHY_MODULE: - if self._license_client.get_feature_enabled([ChoreographyClient.license_name])[ - ChoreographyClient.license_name - ]: + if self._license_client.get_feature_enabled( + [ChoreographyClient.license_name] + )[ChoreographyClient.license_name]: self._sdk.register_service_client(ChoreographyClient) - self._choreography_client = self._robot.ensure_client(ChoreographyClient.default_service_name) + self._choreography_client = self._robot.ensure_client( + ChoreographyClient.default_service_name + ) self._is_licensed_for_choreography = True else: - self._logger.info(f"Robot is not licensed for choreography: {e}") + self._logger.info( + f"Robot is not licensed for choreography: {e}" + ) self._is_licensed_for_choreography = False self._choreography_client = None else: @@ -508,7 +584,9 @@ def __init__( sleep_secs = 15 self._logger.warning( "Unable to create client service: {}. This usually means the robot hasn't " - "finished booting yet. Will wait {} seconds and try again.".format(e, sleep_secs) + "finished booting yet. Will wait {} seconds and try again.".format( + e, sleep_secs + ) ) time.sleep(sleep_secs) @@ -532,8 +610,12 @@ def __init__( max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", None), ) - self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) - self._estop_monitor = AsyncEStopMonitor(self._estop_client, self._logger, 20.0, self) + self._idle_task = AsyncIdle( + self._robot_command_client, self._logger, 10.0, self + ) + self._estop_monitor = AsyncEStopMonitor( + self._estop_client, self._logger, 20.0, self + ) self._estop_endpoint = None self._estop_keepalive = None @@ -560,7 +642,9 @@ def __init__( "rates": self._rates, "callbacks": self._callbacks, } - self.spot_image = SpotImages(self._robot, self._logger, self._robot_params, self._robot_clients) + self.spot_image = SpotImages( + self._robot, self._logger, self._robot_params, self._robot_clients + ) if self._robot.has_arm(): self._spot_arm = SpotArm( @@ -575,9 +659,15 @@ def __init__( else: self._spot_arm = None - self._spot_docking = SpotDocking(self._robot, self._logger, self._robot_params, self._robot_clients) - self._spot_graph_nav = SpotGraphNav(self._robot, self._logger, self._robot_params, self._robot_clients) - self._spot_check = SpotCheck(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_docking = SpotDocking( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_graph_nav = SpotGraphNav( + self._robot, self._logger, self._robot_params, self._robot_clients + ) + self._spot_check = SpotCheck( + self._robot, self._logger, self._robot_params, self._robot_clients + ) self._spot_images = SpotImages( self._robot, self._logger, @@ -587,13 +677,17 @@ def __init__( ) if self._point_cloud_client: - self._spot_eap = SpotEAP(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_eap = SpotEAP( + self._robot, self._logger, self._robot_params, self._robot_clients + ) self._point_cloud_task = self._spot_eap.async_task robot_tasks.append(self._point_cloud_task) else: self._spot_eap = None - self._spot_world_objects = SpotWorldObjects(self._robot, self._logger, self._robot_params, self._robot_clients) + self._spot_world_objects = SpotWorldObjects( + self._robot, self._logger, self._robot_params, self._robot_clients + ) self._world_objects_task = self._spot_world_objects.async_task robot_tasks.append(self._world_objects_task) @@ -790,7 +884,10 @@ def claim(self) -> typing.Tuple[bool, str]: """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" if self.lease is not None: for resource in self.lease: - if resource.resource == "all-leases" and SPOT_CLIENT_NAME in resource.lease_owner.client_name: + if ( + resource.resource == "all-leases" + and SPOT_CLIENT_NAME in resource.lease_owner.client_name + ): return True, "We already claimed the lease" try: @@ -819,7 +916,9 @@ def updateTasks(self): def resetEStop(self): """Get keepalive for eStop""" - self._estop_endpoint = EstopEndpoint(self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout) + self._estop_endpoint = EstopEndpoint( + self._estop_client, SPOT_CLIENT_NAME, self._estop_timeout + ) self._estop_endpoint.force_simple_setup() # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) @@ -932,13 +1031,17 @@ def sit(self): @try_claim(power_on=True) def simple_stand(self, monitor_command=True): """If the e-stop is enabled, and the motor power is enabled, stand the robot up.""" - response = self._robot_command(RobotCommandBuilder.synchro_stand_command(params=self._mobility_params)) + response = self._robot_command( + RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) + ) if monitor_command: self._last_stand_command = response[2] return response[0], response[1] @try_claim(power_on=True) - def stand(self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, body_roll=0): + def stand( + self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, body_roll=0 + ): """ If the e-stop is enabled, and the motor power is enabled, stand the robot up. Executes a stand command, but one where the robot will assume the pose specified by the given parameters. @@ -957,11 +1060,15 @@ def stand(self, monitor_command=True, body_height=0, body_yaw=0, body_pitch=0, b # If any of the orientation parameters are nonzero use them to pose the body body_orientation = EulerZXY(yaw=body_yaw, pitch=body_pitch, roll=body_roll) response = self._robot_command( - RobotCommandBuilder.synchro_stand_command(body_height=body_height, footprint_R_body=body_orientation) + RobotCommandBuilder.synchro_stand_command( + body_height=body_height, footprint_R_body=body_orientation + ) ) else: # Otherwise just use the mobility params - response = self._robot_command(RobotCommandBuilder.synchro_stand_command(params=self._mobility_params)) + response = self._robot_command( + RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) + ) if monitor_command: self._last_stand_command = response[2] @@ -976,7 +1083,9 @@ def battery_change_pose(self, dir_hint: int = 1): dir_hint: 1 rolls to the right side of the robot, 2 to the left """ if self._robot_params["is_sitting"]: - response = self._robot_command(RobotCommandBuilder.battery_change_pose_command(dir_hint)) + response = self._robot_command( + RobotCommandBuilder.battery_change_pose_command(dir_hint) + ) return response[0], response[1] return False, "Call sit before trying to roll over" @@ -989,7 +1098,9 @@ def safe_power_off(self): def clear_behavior_fault(self, id): """Clear the behavior fault defined by id.""" try: - rid = self._robot_command_client.clear_behavior_fault(behavior_fault_id=id, lease=None) + rid = self._robot_command_client.clear_behavior_fault( + behavior_fault_id=id, lease=None + ) return True, "Success", rid except Exception as e: return False, f"Exception while clearing behavior fault: {e}", None @@ -1042,7 +1153,9 @@ def velocity_cmd( """ end_time = time.time() + cmd_duration response = self._robot_command( - RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), + RobotCommandBuilder.synchro_velocity_command( + v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params + ), end_time_secs=end_time, timesync_endpoint=self._robot.time_sync.endpoint, ) @@ -1124,7 +1237,9 @@ def trajectory_cmd( self._last_trajectory_command = response[2] return response[0], response[1] - def robot_command(self, robot_command: robot_command_pb2.RobotCommand) -> typing.Tuple[bool, str]: + def robot_command( + self, robot_command: robot_command_pb2.RobotCommand + ) -> typing.Tuple[bool, str]: end_time = time.time() + MAX_COMMAND_DURATION return self._robot_command( robot_command, @@ -1132,7 +1247,9 @@ def robot_command(self, robot_command: robot_command_pb2.RobotCommand) -> typing timesync_endpoint=self._robot.time_sync.endpoint, ) - def get_robot_command_feedback(self, cmd_id: int) -> robot_command_pb2.RobotCommandFeedbackResponse: + def get_robot_command_feedback( + self, cmd_id: int + ) -> robot_command_pb2.RobotCommandFeedbackResponse: return self._robot_command_client.robot_command_feedback(cmd_id) def check_is_powered_on(self): From 7843a486e4c770cc96e6cba7dd587aadb484f5b3 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 10 Jun 2023 17:09:49 +0100 Subject: [PATCH 52/57] import ordering and removal of unused imports --- spot_wrapper/cam_wrapper.py | 14 +++++++------- spot_wrapper/spot_arm.py | 26 +++++++++++++------------- spot_wrapper/spot_check.py | 8 ++++---- spot_wrapper/spot_dance.py | 2 +- spot_wrapper/spot_docking.py | 6 +++--- spot_wrapper/spot_eap.py | 3 +-- spot_wrapper/spot_images.py | 6 +++--- spot_wrapper/spot_world_objects.py | 2 +- spot_wrapper/wrapper.py | 15 ++++++--------- 9 files changed, 39 insertions(+), 43 deletions(-) diff --git a/spot_wrapper/cam_wrapper.py b/spot_wrapper/cam_wrapper.py index 00460422..ea348f44 100644 --- a/spot_wrapper/cam_wrapper.py +++ b/spot_wrapper/cam_wrapper.py @@ -10,19 +10,19 @@ import cv2 import numpy as np from aiortc import RTCConfiguration +from bosdyn.api.spot_cam import audio_pb2 +from bosdyn.api.spot_cam.ptz_pb2 import PtzDescription, PtzVelocity, PtzPosition from bosdyn.client import Robot from bosdyn.client import spot_cam +from bosdyn.client.payload import PayloadClient +from bosdyn.client.spot_cam.audio import AudioClient from bosdyn.client.spot_cam.compositor import CompositorClient +from bosdyn.client.spot_cam.health import HealthClient from bosdyn.client.spot_cam.lighting import LightingClient +from bosdyn.client.spot_cam.media_log import MediaLogClient from bosdyn.client.spot_cam.power import PowerClient -from bosdyn.client.spot_cam.health import HealthClient -from bosdyn.client.spot_cam.audio import AudioClient -from bosdyn.client.spot_cam.streamquality import StreamQualityClient from bosdyn.client.spot_cam.ptz import PtzClient -from bosdyn.client.spot_cam.media_log import MediaLogClient -from bosdyn.client.payload import PayloadClient -from bosdyn.api.spot_cam.ptz_pb2 import PtzDescription, PtzVelocity, PtzPosition -from bosdyn.api.spot_cam import audio_pb2 +from bosdyn.client.spot_cam.streamquality import StreamQualityClient from spot_wrapper.cam_webrtc_client import WebRTCClient from spot_wrapper.wrapper import SpotWrapper diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index bfb34095..9b143356 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -1,22 +1,22 @@ +import logging import time import typing -import logging -from bosdyn.util import seconds_to_duration -from bosdyn.client.async_tasks import AsyncPeriodicQuery -from bosdyn.client import robot_command -from bosdyn.client.robot import Robot -from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient -from bosdyn.client.image import ImageClient, build_image_request -from bosdyn.client.manipulation_api_client import ManipulationApiClient -from bosdyn.api import robot_command_pb2 from bosdyn.api import arm_command_pb2 -from bosdyn.api import synchronized_command_pb2 from bosdyn.api import geometry_pb2 -from bosdyn.api import trajectory_pb2 -from bosdyn.api import manipulation_api_pb2 from bosdyn.api import image_pb2 +from bosdyn.api import manipulation_api_pb2 +from bosdyn.api import robot_command_pb2 +from bosdyn.api import synchronized_command_pb2 +from bosdyn.api import trajectory_pb2 +from bosdyn.client import robot_command +from bosdyn.client.async_tasks import AsyncPeriodicQuery +from bosdyn.client.image import ImageClient, build_image_request +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.robot import Robot +from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.util import seconds_to_duration from google.protobuf.duration_pb2 import Duration """List of hand image sources for asynchronous periodic query""" diff --git a/spot_wrapper/spot_check.py b/spot_wrapper/spot_check.py index ddeab131..0a5eec1e 100644 --- a/spot_wrapper/spot_check.py +++ b/spot_wrapper/spot_check.py @@ -1,14 +1,14 @@ -import typing import logging import time +import typing -from bosdyn.client.robot import Robot +from bosdyn.api import header_pb2 from bosdyn.client import robot_command +from bosdyn.client.lease import LeaseClient, LeaseWallet, Lease +from bosdyn.client.robot import Robot from bosdyn.client.spot_check import SpotCheckClient, run_spot_check from bosdyn.client.spot_check import spot_check_pb2 -from bosdyn.api import header_pb2 from google.protobuf.timestamp_pb2 import Timestamp -from bosdyn.client.lease import LeaseClient, LeaseWallet, Lease class SpotCheck: diff --git a/spot_wrapper/spot_dance.py b/spot_wrapper/spot_dance.py index e4882de3..485e5859 100644 --- a/spot_wrapper/spot_dance.py +++ b/spot_wrapper/spot_dance.py @@ -1,12 +1,12 @@ import time +from bosdyn.choreography.client.choreography import ChoreographyClient from bosdyn.choreography.client.choreography import ( load_choreography_sequence_from_txt_file, ) from bosdyn.client import ResponseError from bosdyn.client.exceptions import UnauthenticatedError from bosdyn.client.robot import Robot -from bosdyn.choreography.client.choreography import ChoreographyClient class SpotDance: diff --git a/spot_wrapper/spot_docking.py b/spot_wrapper/spot_docking.py index eb6eec7e..1eaf91f7 100644 --- a/spot_wrapper/spot_docking.py +++ b/spot_wrapper/spot_docking.py @@ -1,10 +1,10 @@ -import typing import logging +import typing -from bosdyn.client.robot import Robot +from bosdyn.api.docking import docking_pb2 from bosdyn.client import robot_command from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock -from bosdyn.api.docking import docking_pb2 +from bosdyn.client.robot import Robot class SpotDocking: diff --git a/spot_wrapper/spot_eap.py b/spot_wrapper/spot_eap.py index 05557655..548d6ff1 100644 --- a/spot_wrapper/spot_eap.py +++ b/spot_wrapper/spot_eap.py @@ -1,10 +1,9 @@ -import typing import logging +import typing from bosdyn.client.async_tasks import AsyncPeriodicQuery from bosdyn.client.point_cloud import PointCloudClient, build_pc_request from bosdyn.client.robot import Robot -from bosdyn.client import robot_command """List of point cloud sources""" point_cloud_sources = ["velodyne-point-cloud"] diff --git a/spot_wrapper/spot_images.py b/spot_wrapper/spot_images.py index 563a5291..5e3c6476 100644 --- a/spot_wrapper/spot_images.py +++ b/spot_wrapper/spot_images.py @@ -1,15 +1,15 @@ -import typing import logging +import typing from collections import namedtuple from dataclasses import dataclass -from bosdyn.client.robot import Robot +from bosdyn.api import image_pb2 from bosdyn.client.image import ( ImageClient, build_image_request, UnsupportedPixelFormatRequestedError, ) -from bosdyn.api import image_pb2 +from bosdyn.client.robot import Robot """List of body image sources for periodic query""" CAMERA_IMAGE_SOURCES = [ diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py index da7061a3..ae228729 100644 --- a/spot_wrapper/spot_world_objects.py +++ b/spot_wrapper/spot_world_objects.py @@ -1,5 +1,5 @@ -import typing import logging +import typing from bosdyn.client.async_tasks import AsyncPeriodicQuery from bosdyn.client.robot import Robot diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 5952c9aa..70002f57 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -6,38 +6,35 @@ import bosdyn.client.auth from bosdyn.api import image_pb2 +from bosdyn.api import lease_pb2 +from bosdyn.api import point_cloud_pb2 from bosdyn.api import robot_command_pb2 from bosdyn.api import robot_state_pb2 from bosdyn.api import world_object_pb2 -from bosdyn.api import lease_pb2 -from bosdyn.api import point_cloud_pb2 from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 from bosdyn.client import create_standard_sdk, ResponseError, RpcError from bosdyn.client import frame_helpers from bosdyn.client import math_helpers -from bosdyn.client import robot_command -from bosdyn.client.robot import UnregisteredServiceError -from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks from bosdyn.client.docking import DockingClient from bosdyn.client.estop import ( EstopClient, EstopEndpoint, EstopKeepAlive, - MotorsOnError, ) from bosdyn.client.graph_nav import GraphNavClient -from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.image import ImageClient from bosdyn.client.lease import LeaseClient, LeaseKeepAlive +from bosdyn.client.license import LicenseClient from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.map_processing import MapProcessingServiceClient from bosdyn.client.power import PowerClient from bosdyn.client.robot import UnregisteredServiceError from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder from bosdyn.client.robot_state import RobotStateClient -from bosdyn.client.world_object import WorldObjectClient from bosdyn.client.spot_check import SpotCheckClient -from bosdyn.client.license import LicenseClient +from bosdyn.client.time_sync import TimeSyncEndpoint +from bosdyn.client.world_object import WorldObjectClient try: from bosdyn.choreography.client.choreography import ( From e6df4814f459baebc3d7453bac98b61164f49949 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sat, 10 Jun 2023 17:20:18 +0100 Subject: [PATCH 53/57] wait for arm commands to complete rather than sleeping, using block_until_arm_arrives SDK function --- spot_wrapper/spot_arm.py | 68 +++++++++++++++++++++------------------- 1 file changed, 35 insertions(+), 33 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 9b143356..62a61123 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -14,7 +14,11 @@ from bosdyn.client.image import ImageClient, build_image_request from bosdyn.client.manipulation_api_client import ManipulationApiClient from bosdyn.client.robot import Robot -from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.robot_command import ( + RobotCommandBuilder, + RobotCommandClient, + block_until_arm_arrives, +) from bosdyn.client.robot_state import RobotStateClient from bosdyn.util import seconds_to_duration from google.protobuf.duration_pb2 import Duration @@ -180,6 +184,19 @@ def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: return True, "Spot has an arm, is powered on, and standing" + def wait_for_arm_command_to_complete(self, cmd_id, timeout_sec=None): + """ + Wrapper around the SDK function for convenience + + Args: + cmd_id: ID of the command that we are waiting on + timeout_sec: After this time, timeout regardless of what the robot state is + + """ + block_until_arm_arrives( + self._robot_command_client, cmd_id=cmd_id, timeout_sec=timeout_sec + ) + def arm_stow(self) -> typing.Tuple[bool, str]: try: success, msg = self.ensure_arm_power_and_stand() @@ -191,9 +208,9 @@ def arm_stow(self) -> typing.Tuple[bool, str]: stow = RobotCommandBuilder.arm_stow_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(stow) + cmd_id = self._robot_command_client.robot_command(stow) self._logger.info("Command stow issued") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while trying to stow: {e}" @@ -211,9 +228,9 @@ def arm_unstow(self) -> typing.Tuple[bool, str]: unstow = RobotCommandBuilder.arm_ready_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(unstow) + cmd_id = self._robot_command_client.robot_command(unstow) self._logger.info("Command unstow issued") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while trying to unstow: {e}" @@ -231,9 +248,9 @@ def arm_carry(self) -> typing.Tuple[bool, str]: carry = RobotCommandBuilder.arm_carry_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(carry) + cmd_id = self._robot_command_client.robot_command(carry) self._logger.info("Command carry issued") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while carry mode was issued: {e}" @@ -322,19 +339,7 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: # Send the request cmd_id = self._robot_command_client.robot_command(arm_command) - - # Query for feedback to determine how long it will take - feedback_resp = self._robot_command_client.robot_command_feedback( - cmd_id - ) - joint_move_feedback = ( - feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback - ) - time_to_goal: Duration = joint_move_feedback.time_to_goal - time_to_goal_in_seconds: float = time_to_goal.seconds + ( - float(time_to_goal.nanos) / float(10**9) - ) - time.sleep(time_to_goal_in_seconds) + self.wait_for_arm_command_to_complete(cmd_id) return True, "Spot Arm moved successfully" except Exception as e: @@ -398,11 +403,9 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: ) # Send the request - self._robot_command_client.robot_command(robot_command) + cmd_id = self._robot_command_client.robot_command(robot_command) self._logger.info("Force trajectory command sent") - - time.sleep(float(traj_duration) + 1.0) - + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured during arm movement: {e}" @@ -419,9 +422,9 @@ def gripper_open(self) -> typing.Tuple[bool, str]: command = RobotCommandBuilder.claw_gripper_open_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) + cmd_id = self._robot_command_client.robot_command(command) self._logger.info("Command gripper open sent") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while gripper was moving: {e}" @@ -439,9 +442,9 @@ def gripper_close(self) -> typing.Tuple[bool, str]: command = RobotCommandBuilder.claw_gripper_close_command() # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) + cmd_id = self._robot_command_client.robot_command(command) self._logger.info("Command gripper close sent") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while gripper was moving: {e}" @@ -467,9 +470,9 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: command = RobotCommandBuilder.claw_gripper_open_angle_command(angle) # Command issue with RobotCommandClient - self._robot_command_client.robot_command(command) + cmd_id = self._robot_command_client.robot_command(command) self._logger.info("Command gripper open angle sent") - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return False, f"Exception occured while gripper was moving: {e}" @@ -533,10 +536,9 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: command = RobotCommandBuilder.build_synchro_command(robot_command) # Send the request - self._robot_command_client.robot_command(robot_command) + cmd_id = self._robot_command_client.robot_command(robot_command) self._logger.info("Moving arm to position.") - - time.sleep(2.0) + self.wait_for_arm_command_to_complete(cmd_id) except Exception as e: return ( From ea44635319e216081fd0fbc71e48c7ff7dfe9b05 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 18 Jun 2023 13:30:55 +0100 Subject: [PATCH 54/57] no longer use convenience dict to access robot params in wrapper class --- spot_wrapper/wrapper.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 70002f57..9ed103ec 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -794,7 +794,7 @@ def is_valid(self) -> bool: @property def id(self) -> str: """Return robot's ID""" - return self._robot_params["robot_id"] + return self._robot_id @property def robot_state(self) -> robot_state_pb2.RobotState: @@ -829,25 +829,25 @@ def point_clouds(self) -> typing.List[point_cloud_pb2.PointCloudResponse]: @property def is_standing(self) -> bool: """Return boolean of standing state""" - return self._robot_params["is_standing"] + return self._is_standing @property def is_sitting(self) -> bool: """Return boolean of standing state""" - return self._robot_params["is_sitting"] + return self._is_sitting @property def is_moving(self) -> bool: """Return boolean of walking state""" - return self._robot_params["is_moving"] + return self._is_moving @property def near_goal(self) -> bool: - return self._robot_params["near_goal"] + return self._near_goal @property def at_goal(self) -> bool: - return self._robot_params["at_goal"] + return self._at_goal def is_estopped(self, timeout=None) -> bool: return self._robot.is_estopped(timeout=timeout) @@ -888,7 +888,7 @@ def claim(self) -> typing.Tuple[bool, str]: return True, "We already claimed the lease" try: - self._robot_params["robot_id"] = self._robot.get_id() + self._robot_id = self._robot.get_id() self.getLease() if self._start_estop and not self.check_is_powered_on(): # If we are requested to start the estop, and the robot is not already powered on, then we reset the From 7e9515c79cc2e21e714f41910c7f5f2c47e951e3 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 18 Jun 2023 13:32:37 +0100 Subject: [PATCH 55/57] missed some robot params usages in wrapper --- spot_wrapper/wrapper.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9ed103ec..5ac4a195 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1079,7 +1079,7 @@ def battery_change_pose(self, dir_hint: int = 1): Args: dir_hint: 1 rolls to the right side of the robot, 2 to the left """ - if self._robot_params["is_sitting"]: + if self._is_sitting: response = self._robot_command( RobotCommandBuilder.battery_change_pose_command(dir_hint) ) @@ -1187,8 +1187,8 @@ def trajectory_cmd( if mobility_params is None: mobility_params = self._mobility_params self._trajectory_status_unknown = False - self._robot_params["at_goal"] = False - self._robot_params["near_goal"] = False + self._at_goal = False + self._near_goal = False self._last_trajectory_command_precise = precise_position self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration From 776e84009a3fefd0949397d57316cbb0b45e1399 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 18 Jun 2023 14:31:23 +0100 Subject: [PATCH 56/57] add block_until_manipulation_completes method --- spot_wrapper/spot_arm.py | 101 ++++++++++++++++++++++++++------------- 1 file changed, 67 insertions(+), 34 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index 62a61123..ebb21ca7 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -548,7 +548,62 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: return True, "Moved arm successfully" - def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): + @staticmethod + def block_until_manipulation_completes( + manipulation_client: ManipulationApiClient, + cmd_id: int, + timeout_sec: float = None, + ): + """ + Helper that blocks until the arm achieves a finishing state for the specific manipulation command. + + Args: + manipulation_client: manipulation client, used to request feedback + cmd_id: command ID returned by the robot when the arm movement command was sent. + timeout_sec: optional number of seconds after which we'll return no matter what + the robot's state is. + + Returns: + True if successfully completed the grasp, False if the grasp failed + """ + if timeout_sec is not None: + start_time = time.time() + end_time = start_time + timeout_sec + now = time.time() + + while timeout_sec is None or now < end_time: + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_id + ) + + # Send the request + response = manipulation_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request + ) + manipulation_state = response.current_state + # TODO: Incorporate more of the feedback states if needed for different grasp commands. + if manipulation_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED: + return True + elif manipulation_state == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED: + return False + + time.sleep(0.1) + now = time.time() + return False + + def grasp_3d( + self, frame: str, object_rt_frame: typing.List[float] + ) -> typing.Tuple[bool, str]: + """ + Attempt to grasp an object + + Args: + frame: Frame in the which the object_rt_frame vector is given + object_rt_frame: xyz position of the object in the given frame + + Returns: + Bool indicating success, and a message with information. + """ try: frm = str(frame) pos = geometry_pb2.Vec3( @@ -566,39 +621,17 @@ def grasp_3d(self, frame: str, object_rt_frame: typing.List[float]): manipulation_api_request=grasp_request ) - # Get feedback from the robot - while True: - feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( - manipulation_cmd_id=cmd_response.manipulation_cmd_id - ) - - # Send the request - response = ( - self._manipulation_api_client.manipulation_api_feedback_command( - manipulation_api_feedback_request=feedback_request - ) - ) - - print( - "Current state: ", - manipulation_api_pb2.ManipulationFeedbackState.Name( - response.current_state - ), - ) - - if ( - response.current_state - == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED - or response.current_state - == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED - ): - break - - time.sleep(0.25) - - self._robot.logger.info("Finished grasp.") + success = self.block_until_manipulation_completes( + self._manipulation_api_client, cmd_response.cmd_id + ) + if success: + msg = "Grasped successfully" + self._logger.info(msg) + return True, msg + else: + msg = "Grasp failed." + self._logger.info(msg) + return False, msg except Exception as e: return False, f"An error occured while trying to grasp from pose {e}" - - return True, "Grasped successfully" From ef2174d8246c80e7dc6dad12aa8615d71da83b8f Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 18 Jun 2023 15:02:56 +0100 Subject: [PATCH 57/57] improve arm module comments, manipulation request actually makes use of the timesync endpoint and end time --- spot_wrapper/spot_arm.py | 88 +++++++++++++++++++++++++++++++--------- 1 file changed, 69 insertions(+), 19 deletions(-) diff --git a/spot_wrapper/spot_arm.py b/spot_wrapper/spot_arm.py index ebb21ca7..4a599021 100644 --- a/spot_wrapper/spot_arm.py +++ b/spot_wrapper/spot_arm.py @@ -20,8 +20,8 @@ block_until_arm_arrives, ) from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client.time_sync import TimeSyncEndpoint from bosdyn.util import seconds_to_duration -from google.protobuf.duration_pb2 import Duration """List of hand image sources for asynchronous periodic query""" HAND_IMAGE_SOURCES = [ @@ -95,12 +95,14 @@ def __init__( self._rates: typing.Dict[str, float] = robot_params["rates"] self._callbacks: typing.Dict[str, typing.Callable] = robot_params["callbacks"] + self._hand_image_requests = [] self._hand_image_task: AsyncImageService = None self.initialize_hand_image_service() def initialize_hand_image_service(self): - self._hand_image_requests = [] - + """ + Initialises the hand image task for retrieving the hand image + """ for source in HAND_IMAGE_SOURCES: self._hand_image_requests.append( build_image_request(source, image_format=image_pb2.Image.FORMAT_RAW) @@ -114,19 +116,21 @@ def initialize_hand_image_service(self): self._hand_image_requests, ) - self.camera_task_name_to_task_mapping: typing.Dict[str, AsyncImageService] = { - "hand_image": self._hand_image_task, - } - @property - def hand_image_task(self): + def hand_image_task(self) -> AsyncImageService: + """ + Get the hand image task + + Returns: + Async image service for getting the hand image + """ return self._hand_image_task def _manipulation_request( self, request_proto: manipulation_api_pb2, - end_time_secs=None, - timesync_endpoint=None, + end_time_secs: typing.Optional[float] = None, + timesync_endpoint: typing.Optional[TimeSyncEndpoint] = None, ): """Generic function for sending requests to the manipulation api of a robot. Args: @@ -134,7 +138,9 @@ def _manipulation_request( """ try: command_id = self._manipulation_api_client.manipulation_api_command( - manipulation_api_request=request_proto + manipulation_api_request=request_proto, + end_time_secs=end_time_secs, + timesync_endpoint=timesync_endpoint, ).manipulation_cmd_id return True, "Success", command_id @@ -186,7 +192,7 @@ def ensure_arm_power_and_stand(self) -> typing.Tuple[bool, str]: def wait_for_arm_command_to_complete(self, cmd_id, timeout_sec=None): """ - Wrapper around the SDK function for convenience + Wait until a command issued to the arm complets. Wrapper around the SDK function for convenience Args: cmd_id: ID of the command that we are waiting on @@ -198,6 +204,12 @@ def wait_for_arm_command_to_complete(self, cmd_id, timeout_sec=None): ) def arm_stow(self) -> typing.Tuple[bool, str]: + """ + Moves the arm to the stowed position + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -218,6 +230,12 @@ def arm_stow(self) -> typing.Tuple[bool, str]: return True, "Stow arm success" def arm_unstow(self) -> typing.Tuple[bool, str]: + """ + Unstows the arm + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -345,7 +363,7 @@ def arm_joint_move(self, joint_targets) -> typing.Tuple[bool, str]: except Exception as e: return False, f"Exception occured during arm movement: {e}" - def create_wrench_from_msg(self, forces, torques): + def create_wrench_from_forces_and_torques(self, forces, torques): force = geometry_pb2.Vec3(x=forces[0], y=forces[1], z=forces[2]) torque = geometry_pb2.Vec3(x=torques[0], y=torques[1], z=torques[2]) return geometry_pb2.Wrench(force=force, torque=torque) @@ -361,14 +379,18 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: traj_duration = data.duration # first point on trajectory - wrench0 = self.create_wrench_from_msg(data.forces_pt0, data.torques_pt0) + wrench0 = self.create_wrench_from_forces_and_torques( + data.forces_pt0, data.torques_pt0 + ) t0 = seconds_to_duration(0) traj_point0 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench0, time_since_reference=t0 ) # Second point on the trajectory - wrench1 = self.create_wrench_from_msg(data.forces_pt1, data.torques_pt1) + wrench1 = self.create_wrench_from_forces_and_torques( + data.forces_pt1, data.torques_pt1 + ) t1 = seconds_to_duration(traj_duration) traj_point1 = trajectory_pb2.WrenchTrajectoryPoint( wrench=wrench1, time_since_reference=t1 @@ -412,6 +434,12 @@ def force_trajectory(self, data) -> typing.Tuple[bool, str]: return True, "Moved arm successfully" def gripper_open(self) -> typing.Tuple[bool, str]: + """ + Fully opens the gripper + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -432,6 +460,12 @@ def gripper_open(self) -> typing.Tuple[bool, str]: return True, "Open gripper success" def gripper_close(self) -> typing.Tuple[bool, str]: + """ + Closes the gripper + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -452,8 +486,15 @@ def gripper_close(self) -> typing.Tuple[bool, str]: return True, "Closed gripper successfully" def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: - # takes an angle between 0 (closed) and 90 (fully opened) and opens the - # gripper at this angle + """ + Takes an angle between 0 (closed) and 90 (fully opened) and opens the gripper at this angle + + Args: + gripper_ang: Angle to which the gripper should be opened + + Returns: + Boolean success, string message + """ if gripper_ang > 90 or gripper_ang < 0: return False, "Gripper angle must be between 0 and 90" try: @@ -480,6 +521,15 @@ def gripper_angle_open(self, gripper_ang: float) -> typing.Tuple[bool, str]: return True, "Opened gripper successfully" def hand_pose(self, data) -> typing.Tuple[bool, str]: + """ + Set the pose of the hand + + Args: + data: + + Returns: + Boolean success, string message + """ try: success, msg = self.ensure_arm_power_and_stand() if not success: @@ -516,7 +566,7 @@ def hand_pose(self, data) -> typing.Tuple[bool, str]: ) arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( - root_frame_name=data.frame, + root_frame_name=frame, pose_trajectory_in_task=hand_trajectory, force_remain_near_current_joint_configuration=True, ) @@ -553,7 +603,7 @@ def block_until_manipulation_completes( manipulation_client: ManipulationApiClient, cmd_id: int, timeout_sec: float = None, - ): + ) -> bool: """ Helper that blocks until the arm achieves a finishing state for the specific manipulation command.