def resetEStop(self): """Get keepalive for eStop""" self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 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)
def __init__(self, robot): self._velocity_speed = VELOCITY_BASE_SPEED self._velocity_angular = VELOCITY_BASE_ANGULAR self.height = 0.5 self.roll = 0.0 self.pitch = 0.0 self.yaw = 0.0 self._robot = robot # Create clients -- do not use the for communication yet. self._lease_client = robot.ensure_client(LeaseClient.default_service_name) self._estop_client = robot.ensure_client(EstopClient.default_service_name) self._estop_endpoint = EstopEndpoint(self._estop_client, 'wasd', 9.0) self._power_client = robot.ensure_client(PowerClient.default_service_name) self._robot_state_client = robot.ensure_client(RobotStateClient.default_service_name) self._robot_command_client = robot.ensure_client(RobotCommandClient.default_service_name) self._robot_metrics_task = AsyncMetrics(self._robot_state_client) self._robot_state_task = AsyncRobotState(self._robot_state_client) self._image_task = AsyncImageCapture(robot) self._async_tasks = AsyncTasks( [self._robot_metrics_task, self._robot_state_task, self._image_task]) self._lock = threading.Lock() self._command_dictionary = { ord('\t'): self._quit_program, ord('T'): self._toggle_time_sync, ord('\\'): self._toggle_estop, ord('r'): self._self_right, ord('P'): self._toggle_power, ord('1'): self._sit, ord('2'): self._stand, ord('w'): self._move_forward, ord('s'): self._move_backward, ord('a'): self._strafe_left, ord('d'): self._strafe_right, ord('q'): self._turn_left, ord('e'): self._turn_right, ord('.'): self._speed_up, ord(','): self._slow_down, ord('>'): self._speed_up, ord('<'): self._slow_down, ord('z'): self._lean_left, ord('c'): self._lean_right, ord('f'): self._twist_left, ord('h'): self._twist_right, ord('t'): self._tilt_down, ord('g'): self._tilt_up, ord('3'): self._stand_lower, ord('4'): self._stand_higher, ord('I'): self._image_task.take_image, ord('O'): self._image_task.toggle_video_mode, } self._locked_messages = ['', '', ''] # string: displayed message for user self._estop_keepalive = None self._exit_check = None # Stuff that is set in start() self._robot_id = None self._lease = None
def __init__(self, robot): self._robot = robot # Create clients -- do not use the for communication yet. self._lease_client = robot.ensure_client( LeaseClient.default_service_name) try: self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) self._estop_endpoint = EstopEndpoint(self._estop_client, 'GNClient', 9.0) except: # Not the estop. self._estop_client = None self._estop_endpoint = None self._power_client = robot.ensure_client( PowerClient.default_service_name) self._robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) self._robot_state_task = AsyncRobotState(self._robot_state_client) self._image_task = AsyncImageCapture(robot) self._async_tasks = AsyncTasks( [self._robot_state_task, self._image_task]) self._lock = threading.Lock() self._command_dictionary = { 27: self._stop, # ESC key ord('\t'): self._quit_program, ord('T'): self._toggle_time_sync, ord(' '): self._toggle_estop, ord('r'): self._self_right, ord('P'): self._toggle_power, ord('v'): self._sit, ord('b'): self._battery_change_pose, ord('f'): self._stand, ord('w'): self._move_forward, ord('s'): self._move_backward, ord('a'): self._strafe_left, ord('d'): self._strafe_right, ord('q'): self._turn_left, ord('e'): self._turn_right, ord('I'): self._image_task.take_image, ord('O'): self._image_task.toggle_video_mode, ord('u'): self._unstow, ord('j'): self._stow, ord('l'): self._toggle_lease } self._locked_messages = ['', '', ''] # string: displayed message for user self._estop_keepalive = None self._exit_check = None # Stuff that is set in start() self._robot_id = None self._lease = None self._lease_keepalive = None
def __init__(self, client, timeout_sec, name=None, unique_id=None): QtWidgets.QMainWindow.__init__(self) self.logger = logging.getLogger("Estop GUI") self.disable_signal.connect(self.disable_buttons) self.checkin_status_signal.connect(self.set_status_label) self.got_status_signal.connect(self._launch_estop_status_dialog) self.status_extant = False self.quitting = False # Used to tell threads to shutdown # Force server to set up a single endpoint system ep = EstopEndpoint(client, name, timeout_sec) ep.force_simple_setup() # Begin periodic check-in between keep-alive and robot self.estop_keep_alive = EstopKeepAlive(ep) # Configure UI. self.setCentralWidget(QtWidgets.QWidget()) self.center_layout = QtWidgets.QVBoxLayout(self.centralWidget()) self.center_layout.setAlignment(QtCore.Qt.AlignTop) self.center_layout.setSpacing(1) self.center_layout.setContentsMargins(1, 1, 1, 1) self.stop_button = QtWidgets.QPushButton(self) self.stop_button.setText('STOP') self.stop_button.clicked.connect(self.estop_keep_alive.stop) self.stop_button.setStyleSheet(STOP_BUTTON_STYLESHEET) self.stop_button.setSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) self.center_layout.addWidget(self.stop_button) self.status_label = QtWidgets.QLabel('Starting...') self.status_label.setAlignment(QtCore.Qt.AlignCenter) self.status_label.setStyleSheet(ERROR_LABEL_STYLESHEET) self.center_layout.addWidget(self.status_label) self.release_button = QtWidgets.QPushButton(self) self.release_button.setText('Release') self.release_button.clicked.connect(self.estop_keep_alive.allow) self.release_button.setStyleSheet(RELEASE_BUTTON_STYLESHEET) self.center_layout.addWidget(self.release_button) self.status_button = QtWidgets.QPushButton(self) self.status_button.setText('Status') self.status_button.clicked.connect(self.status) self.center_layout.addWidget(self.status_button) self.setWindowTitle("E-Stop ({} {}sec)".format("TEST ADDRESS", timeout_sec)) # Begin monitoring the keep-alive status thread = threading.Thread(target=self._check_keep_alive_status) thread.start()
def __init__(self, client, timeout_sec, name=None): # Force server to set up a single endpoint system ep = EstopEndpoint(client, name, timeout_sec) ep.force_simple_setup() # Begin periodic check-in between keep-alive and robot self.estop_keep_alive = EstopKeepAlive(ep) # Release the estop self.estop_keep_alive.allow()
def main(): parser = argparse.ArgumentParser() parser.add_argument('--timeout', type=float, help="Timeout of the estop (seconds)", default=1) bosdyn.client.util.add_common_arguments(parser) options = parser.parse_args() if options.verbose: log_level = logging.DEBUG else: log_level = logging.INFO logging.basicConfig( level=log_level, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s') sdk = bosdyn.client.create_standard_sdk('EstopClientWithSdk') sdk.load_app_token(options.app_token) robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) estop_client = robot.ensure_client(EstopClient.default_service_name) estop_endpoint = EstopEndpoint( client=estop_client, name=bosdyn.client.sdk.generate_client_name('estop-tutorial:'), estop_timeout=options.timeout) estop_endpoint.force_simple_setup() call_and_check(estop_client, estop_endpoint, StopLevel.ESTOP_LEVEL_NONE, sync=True) call_and_check(estop_client, estop_endpoint, StopLevel.ESTOP_LEVEL_CUT, sync=True) call_and_check(estop_client, estop_endpoint, StopLevel.ESTOP_LEVEL_NONE, sync=True) call_and_check(estop_client, estop_endpoint, StopLevel.ESTOP_LEVEL_CUT, sync=True) call_and_check(estop_client, estop_endpoint, StopLevel.ESTOP_LEVEL_NONE, sync=False) call_and_check(estop_client, estop_endpoint, StopLevel.ESTOP_LEVEL_CUT, sync=False) call_and_check(estop_client, estop_endpoint, StopLevel.ESTOP_LEVEL_NONE, sync=False) call_and_check(estop_client, estop_endpoint, StopLevel.ESTOP_LEVEL_CUT, sync=False) current_config = estop_client.get_config() estop_client.deregister(current_config.unique_id, estop_endpoint)
def __init__(self, robot, download_filepath): self._robot = robot # Flag indicating whether mission is currently being recorded self._recording = False # Filepath for the location to put the downloaded graph and snapshots. self._download_filepath = download_filepath # List of visited waypoints in current mission self._waypoint_path = [] # Current waypoint id self._waypoint_id = 'NONE' # Create clients -- do not use the for communication yet. self._lease_client = robot.ensure_client( LeaseClient.default_service_name) try: self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) self._estop_endpoint = EstopEndpoint(self._estop_client, 'GNClient', 9.0) except: # Not the estop. self._estop_client = None self._estop_endpoint = None self._power_client = robot.ensure_client( PowerClient.default_service_name) self._robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) self._world_object_client = robot.ensure_client( WorldObjectClient.default_service_name) # Setup the recording service client. self._recording_client = self._robot.ensure_client( GraphNavRecordingServiceClient.default_service_name) # Setup the graph nav service client. self._graph_nav_client = robot.ensure_client( GraphNavClient.default_service_name) self._robot_state_task = AsyncRobotState(self._robot_state_client) self._async_tasks = AsyncTasks([self._robot_state_task]) self._lock = threading.Lock() self._command_dictionary = { ord('\t'): self._quit_program, ord('T'): self._toggle_time_sync, ord(' '): self._toggle_estop, ord('r'): self._self_right, ord('P'): self._toggle_power, ord('v'): self._sit, ord('f'): self._stand, ord('w'): self._move_forward, ord('s'): self._move_backward, ord('a'): self._strafe_left, ord('d'): self._strafe_right, ord('q'): self._turn_left, ord('e'): self._turn_right, ord('m'): self._start_recording, ord('l'): self._relocalize, ord('g'): self._generate_mission, } self._locked_messages = ['', '', ''] # string: displayed message for user self._estop_keepalive = None self._exit_check = None # Stuff that is set in start() self._robot_id = None self._lease = None
class RecorderInterface(object): """A curses interface for recording robot missions.""" def __init__(self, robot, download_filepath): self._robot = robot # Flag indicating whether mission is currently being recorded self._recording = False # Filepath for the location to put the downloaded graph and snapshots. self._download_filepath = download_filepath # List of visited waypoints in current mission self._waypoint_path = [] # Current waypoint id self._waypoint_id = 'NONE' # Create clients -- do not use the for communication yet. self._lease_client = robot.ensure_client( LeaseClient.default_service_name) try: self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) self._estop_endpoint = EstopEndpoint(self._estop_client, 'GNClient', 9.0) except: # Not the estop. self._estop_client = None self._estop_endpoint = None self._power_client = robot.ensure_client( PowerClient.default_service_name) self._robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) self._world_object_client = robot.ensure_client( WorldObjectClient.default_service_name) # Setup the recording service client. self._recording_client = self._robot.ensure_client( GraphNavRecordingServiceClient.default_service_name) # Setup the graph nav service client. self._graph_nav_client = robot.ensure_client( GraphNavClient.default_service_name) self._robot_state_task = AsyncRobotState(self._robot_state_client) self._async_tasks = AsyncTasks([self._robot_state_task]) self._lock = threading.Lock() self._command_dictionary = { ord('\t'): self._quit_program, ord('T'): self._toggle_time_sync, ord(' '): self._toggle_estop, ord('r'): self._self_right, ord('P'): self._toggle_power, ord('v'): self._sit, ord('f'): self._stand, ord('w'): self._move_forward, ord('s'): self._move_backward, ord('a'): self._strafe_left, ord('d'): self._strafe_right, ord('q'): self._turn_left, ord('e'): self._turn_right, ord('m'): self._start_recording, ord('l'): self._relocalize, ord('g'): self._generate_mission, } self._locked_messages = ['', '', ''] # string: displayed message for user self._estop_keepalive = None self._exit_check = None # Stuff that is set in start() self._robot_id = None self._lease = None def start(self): """Begin communication with the robot.""" self._lease = self._lease_client.acquire() self._robot_id = self._robot.get_id() if self._estop_endpoint is not None: self._estop_endpoint.force_simple_setup( ) # Set this endpoint as the robot's sole estop. # Clear existing graph nav map self._graph_nav_client.clear_graph() def shutdown(self): """Release control of robot as gracefully as posssible.""" LOGGER.info("Shutting down WasdInterface.") if self._estop_keepalive: # This stops the check-in thread but does not stop the robot. self._estop_keepalive.shutdown() if self._lease: _grpc_or_log("returning lease", lambda: self._lease_client.return_lease(self._lease)) self._lease = None def __del__(self): self.shutdown() def flush_and_estop_buffer(self, stdscr): """Manually flush the curses input buffer but trigger any estop requests (space)""" key = '' while key != -1: key = stdscr.getch() if key == ord(' '): self._toggle_estop() def add_message(self, msg_text): """Display the given message string to the user in the curses interface.""" with self._lock: self._locked_messages = [msg_text] + self._locked_messages[:-1] def message(self, idx): """Grab one of the 3 last messages added.""" with self._lock: return self._locked_messages[idx] @property def robot_state(self): """Get latest robot state proto.""" return self._robot_state_task.proto def drive(self, stdscr): """User interface to control the robot via the passed-in curses screen interface object.""" with LeaseKeepAlive(self._lease_client) as lease_keep_alive, \ ExitCheck() as self._exit_check: curses_handler = CursesHandler(self) curses_handler.setLevel(logging.INFO) LOGGER.addHandler(curses_handler) stdscr.nodelay(True) # Don't block for user input. stdscr.resize(26, 96) stdscr.refresh() # for debug curses.echo() try: while not self._exit_check.kill_now: self._async_tasks.update() self._drive_draw(stdscr, lease_keep_alive) try: cmd = stdscr.getch() # Do not queue up commands on client self.flush_and_estop_buffer(stdscr) self._drive_cmd(cmd) time.sleep(COMMAND_INPUT_RATE) except Exception: # On robot command fault, sit down safely before killing the program. self._safe_power_off() time.sleep(2.0) self.shutdown() raise finally: LOGGER.removeHandler(curses_handler) def _count_visible_fiducials(self): """Return number of fiducials visible to robot.""" request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG] fiducial_objects = self._world_object_client.list_world_objects( object_type=request_fiducials).world_objects return len(fiducial_objects) def _drive_draw(self, stdscr, lease_keep_alive): """Draw the interface screen at each update.""" stdscr.clear() # clear screen stdscr.resize(26, 96) stdscr.addstr( 0, 0, '{:20s} {}'.format(self._robot_id.nickname, self._robot_id.serial_number)) stdscr.addstr(1, 0, self._lease_str(lease_keep_alive)) stdscr.addstr(2, 0, self._battery_str()) stdscr.addstr(3, 0, self._estop_str()) stdscr.addstr(4, 0, self._power_state_str()) stdscr.addstr(5, 0, self._time_sync_str()) stdscr.addstr(6, 0, self._waypoint_str()) stdscr.addstr(7, 0, self._fiducial_str()) for i in range(3): stdscr.addstr(9 + i, 2, self.message(i)) stdscr.addstr(13, 0, "Commands: [TAB]: quit ") stdscr.addstr(14, 0, " [T]: Time-sync, [SPACE]: Estop, [P]: Power") stdscr.addstr(15, 0, " [v]: Sit, [f]: Stand, [r]: Self-right ") stdscr.addstr(16, 0, " [wasd]: Directional strafing ") stdscr.addstr(17, 0, " [qe]: Turning ") stdscr.addstr(18, 0, " [m]: Start recording mission ") stdscr.addstr(19, 0, " [l]: Add fiducial localization to mission ") stdscr.addstr(20, 0, " [g]: Stop recording and generate mission ") stdscr.refresh() def _drive_cmd(self, key): """Run user commands at each update.""" try: cmd_function = self._command_dictionary[key] cmd_function() except KeyError: if key and key != -1 and key < 256: self.add_message("Unrecognized keyboard command: '{}'".format( chr(key))) def _try_grpc(self, desc, thunk): try: return thunk() except (ResponseError, RpcError) as err: self.add_message("Failed {}: {}".format(desc, err)) return None def _quit_program(self): self._robot.power_off() self.shutdown() if self._exit_check is not None: self._exit_check.request_exit() def _toggle_time_sync(self): if self._robot.time_sync.stopped: self._robot.time_sync.start() else: self._robot.time_sync.stop() def _toggle_estop(self): """toggle estop on/off. Initial state is ON""" if self._estop_client is not None and self._estop_endpoint is not None: if not self._estop_keepalive: self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) else: self._try_grpc("stopping estop", self._estop_keepalive.stop) self._estop_keepalive.shutdown() self._estop_keepalive = None def _start_robot_command(self, desc, command_proto, end_time_secs=None): def _start_command(): self._robot_command_client.robot_command( lease=None, command=command_proto, end_time_secs=end_time_secs) self._try_grpc(desc, _start_command) def _self_right(self): self._start_robot_command('self_right', RobotCommandBuilder.selfright_command()) def _sit(self): self._start_robot_command('sit', RobotCommandBuilder.sit_command()) def _stand(self): self._start_robot_command('stand', RobotCommandBuilder.stand_command()) def _move_forward(self): self._velocity_cmd_helper('move_forward', v_x=VELOCITY_BASE_SPEED) def _move_backward(self): self._velocity_cmd_helper('move_backward', v_x=-VELOCITY_BASE_SPEED) def _strafe_left(self): self._velocity_cmd_helper('strafe_left', v_y=VELOCITY_BASE_SPEED) def _strafe_right(self): self._velocity_cmd_helper('strafe_right', v_y=-VELOCITY_BASE_SPEED) def _turn_left(self): self._velocity_cmd_helper('turn_left', v_rot=VELOCITY_BASE_ANGULAR) def _turn_right(self): self._velocity_cmd_helper('turn_right', v_rot=-VELOCITY_BASE_ANGULAR) def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0): self._start_robot_command( desc, RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot), end_time_secs=time.time() + VELOCITY_CMD_DURATION) def _return_to_origin(self): self._start_robot_command( 'fwd_and_rotate', RobotCommandBuilder.trajectory_command( goal_x=0.0, goal_y=0.0, goal_heading=0.0, frame_name=ODOM_FRAME_NAME, params=None, body_height=0.0, locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT), end_time_secs=time.time() + 20) def _toggle_power(self): power_state = self._power_state() if power_state is None: self.add_message( 'Could not toggle power because power state is unknown') return if power_state == robot_state_proto.PowerState.STATE_OFF: self._try_grpc("powering-on", self._request_power_on) else: self._try_grpc("powering-off", self._safe_power_off) def _request_power_on(self): bosdyn.client.power.power_on(self._power_client) def _safe_power_off(self): self._start_robot_command('safe_power_off', RobotCommandBuilder.safe_power_off_command()) def _power_state(self): state = self.robot_state if not state: return None return state.power_state.motor_power_state def _lease_str(self, lease_keep_alive): alive = 'RUNNING' if lease_keep_alive.is_alive() else 'STOPPED' lease = '{}:{}'.format(self._lease.lease_proto.resource, self._lease.lease_proto.sequence) return 'Lease {} THREAD:{}'.format(lease, alive) def _power_state_str(self): power_state = self._power_state() if power_state is None: return '' state_str = robot_state_proto.PowerState.MotorPowerState.Name( power_state) return 'Power: {}'.format(state_str[6:]) # get rid of STATE_ prefix def _estop_str(self): if not self._estop_client: thread_status = 'NOT ESTOP' else: thread_status = 'RUNNING' if self._estop_keepalive else 'STOPPED' estop_status = '??' state = self.robot_state if state: for estop_state in state.estop_states: if estop_state.type == estop_state.TYPE_SOFTWARE: estop_status = estop_state.State.Name( estop_state.state)[6:] # s/STATE_// break return 'Estop {} (thread: {})'.format(estop_status, thread_status) def _time_sync_str(self): if not self._robot.time_sync: return 'Time sync: (none)' if self._robot.time_sync.stopped: status = 'STOPPED' exception = self._robot.time_sync.thread_exception if exception: status = '{} Exception: {}'.format(status, exception) else: status = 'RUNNING' try: skew = self._robot.time_sync.get_robot_clock_skew() if skew: skew_str = 'offset={}'.format(duration_str(skew)) else: skew_str = "(Skew undetermined)" except (TimeSyncError, RpcError) as err: skew_str = '({})'.format(err) return 'Time sync: {} {}'.format(status, skew_str) def _waypoint_str(self): state = self._graph_nav_client.get_localization_state() try: self._waypoint_id = state.localization.waypoint_id if self._waypoint_id == '': self._waypoint_id = 'NONE' except: self._waypoint_id = 'ERROR' if self._recording and self._waypoint_id != 'NONE' and self._waypoint_id != 'ERROR': if (self._waypoint_path == []) or (self._waypoint_id != self._waypoint_path[-1]): self._waypoint_path += [self._waypoint_id] return 'Current waypoint: {} [ RECORDING ]'.format( self._waypoint_id) return 'Current waypoint: {}'.format(self._waypoint_id) def _fiducial_str(self): return 'Visible fiducials: {}'.format( str(self._count_visible_fiducials())) def _battery_str(self): if not self.robot_state: return '' battery_state = self.robot_state.battery_states[0] status = battery_state.Status.Name(battery_state.status) status = status[7:] # get rid of STATUS_ prefix if battery_state.charge_percentage.value: bar_len = int(battery_state.charge_percentage.value) // 10 bat_bar = '|{}{}|'.format('=' * bar_len, ' ' * (10 - bar_len)) else: bat_bar = '' time_left = '' if battery_state.estimated_runtime: time_left = ' ({})'.format( secs_to_hms(battery_state.estimated_runtime.seconds)) return 'Battery: {}{}{}'.format(status, bat_bar, time_left) def _fiducial_visible(self): """Return True if robot can see fiducial.""" request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG] fiducial_objects = self._world_object_client.list_world_objects( object_type=request_fiducials).world_objects self.add_message("Fiducial objects: " + str(fiducial_objects)) if len(fiducial_objects) > 0: return True return False def _start_recording(self): """Start recording a map.""" if self._count_visible_fiducials() == 0: self.add_message( "ERROR: Can't start recording -- No fiducials in view.") return self._waypoint_path = [] status = self._recording_client.start_recording() if status != recording_pb2.StartRecordingResponse.STATUS_OK: self.add_message("Start recording failed.") else: self.add_message("Successfully started recording a map.") self._recording = True def _stop_recording(self): """Stop or pause recording a map.""" if not self._recording: return True self._recording = False try: status = self._recording_client.stop_recording() if status != recording_pb2.StopRecordingResponse.STATUS_OK: self.add_message("Stop recording failed.") return False self.add_message("Successfully stopped recording a map.") return True except NotLocalizedToEndError: self.add_message( "ERROR: Move to final waypoint before stopping recording.") return False def _relocalize(self): """Insert localization node into mission.""" if not self._recording: print('Not recording mission.') return False self.add_message("Adding fiducial localization to mission.") self._waypoint_path += ['LOCALIZE'] return True def _generate_mission(self): """Save graph map and mission file.""" # Check whether mission has been recorded if not self._recording: self.add_message("ERROR: No mission recorded.") return # Stop recording mission if not self._stop_recording(): self.add_message("ERROR: Error while stopping recording.") return # Save graph map os.mkdir(self._download_filepath) self._download_full_graph() # Generate mission mission = self._make_mission() # Save mission file os.mkdir(self._download_filepath + "/missions") mission_filepath = self._download_filepath + "/missions/autogenerated" write_mission(mission, mission_filepath) # Quit program self._quit_program() def _download_full_graph(self): """Download the graph and snapshots from the robot.""" graph = self._graph_nav_client.download_graph() if graph is None: self.add_message("Failed to download the graph.") return self._write_full_graph(graph) self.add_message( "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() 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: 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.add_message("Failed to download waypoint snapshot: " + waypoint.snapshot_id) continue write_bytes(self._download_filepath + '/waypoint_snapshots', '/' + waypoint.snapshot_id, waypoint_snapshot.SerializeToString()) num_waypoint_snapshots_downloaded += 1 self.add_message( "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 for edge in edges: 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.add_message("Failed to download edge snapshot: " + edge.snapshot_id) continue write_bytes(self._download_filepath + '/edge_snapshots', '/' + edge.snapshot_id, edge_snapshot.SerializeToString()) num_edge_snapshots_downloaded += 1 self.add_message( "Downloaded {} of the total {} edge snapshots.".format( num_edge_snapshots_downloaded, len(edges))) def _make_mission(self): """ Create a mission that visits each waypoint on stored path.""" # Create a Sequence that visits all the waypoints. sequence = nodes_pb2.Sequence() sequence.children.add().CopyFrom(self._make_localize_node()) for waypoint_id in self._waypoint_path: if waypoint_id == 'LOCALIZE': sequence.children.add().CopyFrom(self._make_localize_node()) else: sequence.children.add().CopyFrom( self._make_goto_node(waypoint_id)) # Return a Node with the Sequence. ret = nodes_pb2.Node() ret.name = "visit %d waypoints" % len(self._waypoint_path) ret.impl.Pack(sequence) return ret def _make_goto_node(self, waypoint_id): """ Create a leaf node that will go to the waypoint. """ ret = nodes_pb2.Node() ret.name = "goto %s" % waypoint_id impl = nodes_pb2.BosdynNavigateTo() impl.destination_waypoint_id = waypoint_id ret.impl.Pack(impl) return ret def _make_localize_node(self): """Make localization node.""" loc = nodes_pb2.Node() loc.name = "localize robot" impl = nodes_pb2.BosdynGraphNavLocalize() loc.impl.Pack(impl) return loc
class WasdInterface(object): """A curses interface for driving the robot.""" def __init__(self, robot): self._robot = robot # Create clients -- do not use the for communication yet. self._lease_client = robot.ensure_client( LeaseClient.default_service_name) self._estop_client = robot.ensure_client( EstopClient.default_service_name) self._estop_endpoint = EstopEndpoint(self._estop_client, 'wasd', 9.0) self._power_client = robot.ensure_client( PowerClient.default_service_name) self._robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) self._robot_metrics_task = AsyncMetrics(self._robot_state_client) self._robot_state_task = AsyncRobotState(self._robot_state_client) self._image_task = AsyncImageCapture(robot) self._async_tasks = AsyncTasks([ self._robot_metrics_task, self._robot_state_task, self._image_task ]) self._lock = threading.Lock() self._command_dictionary = { ord('\t'): self._quit_program, ord('T'): self._toggle_time_sync, ord(' '): self._toggle_estop, ord('r'): self._self_right, ord('P'): self._toggle_power, ord('v'): self._sit, ord('f'): self._stand, ord('w'): self._move_forward, ord('s'): self._move_backward, ord('a'): self._strafe_left, ord('d'): self._strafe_right, ord('q'): self._turn_left, ord('e'): self._turn_right, ord('I'): self._image_task.take_image, ord('O'): self._image_task.toggle_video_mode, } self._locked_messages = ['', '', ''] # string: displayed message for user self._estop_keepalive = None self._exit_check = None # Stuff that is set in start() self._robot_id = None self._lease = None def start(self): """Begin communication with the robot.""" self._robot_id = self._robot.get_id() self._lease = self._lease_client.acquire() self._estop_endpoint.force_simple_setup( ) # Set this endpoint as the robot's sole estop. def shutdown(self): """Release control of robot as gracefully as posssible.""" LOGGER.info("Shutting down WasdInterface.") if self._robot.time_sync: self._robot.time_sync.stop() if self._estop_keepalive: _grpc_or_log("stopping estop", self._estop_keepalive.stop) self._estop_keepalive = None if self._lease: _grpc_or_log("returning lease", lambda: self._lease_client.return_lease(self._lease)) self._lease = None def __del__(self): self.shutdown() def flush_and_estop_buffer(self, stdscr): """Manually flush the curses input buffer but trigger any estop requests (space)""" key = '' while key != -1: key = stdscr.getch() if key == ord(' '): self._toggle_estop() def add_message(self, msg_text): """Display the given message string to the user in the curses interface.""" with self._lock: self._locked_messages = [msg_text] + self._locked_messages[:-1] def message(self, idx): """Grab one of the 3 last messages added.""" with self._lock: return self._locked_messages[idx] @property def robot_state(self): """Get latest robot state proto.""" return self._robot_state_task.proto @property def robot_metrics(self): """Get latest robot metrics proto.""" return self._robot_metrics_task.proto def drive(self, stdscr): """User interface to control the robot via the passed-in curses screen interface object.""" with ExitCheck(exit_delay_secs=2.0) as self._exit_check, \ LeaseKeepAlive(self._lease_client, keep_running_cb=lambda: self._exit_check.keep_alive) \ as lease_keep_alive: curses_handler = CursesHandler(self) curses_handler.setLevel(logging.INFO) LOGGER.addHandler(curses_handler) stdscr.nodelay(True) # Don't block for user input. # for debug curses.echo() try: while self._exit_check.keep_alive: self._async_tasks.update() self._drive_draw(stdscr, lease_keep_alive) if not self._exit_check.exit_requested: try: cmd = stdscr.getch() # Do not queue up commands on client self.flush_and_estop_buffer(stdscr) self._drive_cmd(cmd) except Exception: # On robot command fault, sit down safely before killing the program. self._safe_power_off() time.sleep(2.0) raise time.sleep(COMMAND_INPUT_RATE) finally: LOGGER.removeHandler(curses_handler) def _drive_draw(self, stdscr, lease_keep_alive): """Draw the interface screen at each update.""" stdscr.clear() # clear screen stdscr.addstr( 0, 0, '{:20s} {}'.format(self._robot_id.nickname, self._robot_id.serial_number)) stdscr.addstr(1, 0, self._lease_str(lease_keep_alive)) stdscr.addstr(2, 0, self._battery_str()) stdscr.addstr(3, 0, self._estop_str()) stdscr.addstr(4, 0, self._power_state_str()) stdscr.addstr(5, 0, self._time_sync_str()) for i in range(3): stdscr.addstr(7 + i, 2, self.message(i)) self._show_metrics(stdscr) stdscr.addstr(10, 0, "Commands: [TAB]: quit") stdscr.addstr(11, 0, " [T]: Time-sync, [SPACE]: Estop, [P]: Power") stdscr.addstr(12, 0, " [I]: Take image, [O]: Video mode") stdscr.addstr(13, 0, " [v]: Sit, [f]: Stand, [r]: Self-right") stdscr.addstr(14, 0, " [wasd]: Directional strafing") stdscr.addstr(15, 0, " [qe]: Turning") # print as many lines of the image as will fit on the curses screen if self._image_task.ascii_image != None: max_y, _max_x = stdscr.getmaxyx() for y_i, img_line in enumerate(self._image_task.ascii_image): if y_i + 16 >= max_y: break stdscr.addstr('\n' + img_line) stdscr.refresh() def _drive_cmd(self, key): """Run user commands at each update.""" try: cmd_function = self._command_dictionary[key] cmd_function() except KeyError: if key and key != -1 and key < 256: self.add_message("Unrecognized keyboard command: '{}'".format( chr(key))) def _try_grpc(self, desc, thunk): try: return thunk() except (ResponseError, RpcError) as err: self.add_message("Failed {}: {}".format(desc, err)) return None def _quit_program(self): self._sit() self._exit_check.request_exit() def _toggle_time_sync(self): if self._robot.time_sync.stopped: self._robot.time_sync.start() else: self._robot.time_sync.stop() def _toggle_estop(self): """toggle estop on/off. Initial state is ON""" if not self._estop_keepalive: self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) else: self._try_grpc("stopping estop", self._estop_keepalive.stop) self._estop_keepalive.shutdown() self._estop_keepalive = None def _start_robot_command(self, desc, command_proto, end_time_secs=None): def _start_command(): self._robot_command_client.robot_command( lease=None, command=command_proto, end_time_secs=end_time_secs) self._try_grpc(desc, _start_command) def _self_right(self): self._start_robot_command('self_right', RobotCommandBuilder.selfright_command()) def _sit(self): self._start_robot_command('sit', RobotCommandBuilder.sit_command()) def _stand(self): self._start_robot_command('stand', RobotCommandBuilder.stand_command()) def _move_forward(self): self._velocity_cmd_helper('move_forward', v_x=VELOCITY_BASE_SPEED) def _move_backward(self): self._velocity_cmd_helper('move_backward', v_x=-VELOCITY_BASE_SPEED) def _strafe_left(self): self._velocity_cmd_helper('strafe_left', v_y=VELOCITY_BASE_SPEED) def _strafe_right(self): self._velocity_cmd_helper('strafe_right', v_y=-VELOCITY_BASE_SPEED) def _turn_left(self): self._velocity_cmd_helper('turn_left', v_rot=VELOCITY_BASE_ANGULAR) def _turn_right(self): self._velocity_cmd_helper('turn_right', v_rot=-VELOCITY_BASE_ANGULAR) def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0): self._start_robot_command( desc, RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot), end_time_secs=time.time() + VELOCITY_CMD_DURATION) # This command makes the robot move very fast and the 'origin' is very unpredictable. def _return_to_origin(self): self._start_robot_command( 'fwd_and_rotate', RobotCommandBuilder.trajectory_command( goal_x=0.0, goal_y=0.0, goal_heading=0.0, frame=geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_KO), params=None, body_height=0.0, locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT), end_time_secs=time.time() + 20) def _take_ascii_image(self): source_name = "frontright_fisheye_image" image_response = self._image_client.get_image_from_sources( [source_name]) image = Image.open(io.BytesIO(image_response[0].shot.image.data)) ascii_image = self._ascii_converter.convert_to_ascii(image, new_width=70) self._last_image_ascii = ascii_image def _toggle_ascii_video(self): if self._video_mode: self._video_mode = False else: self._video_mode = True # def _clear_behavior_faults(self): # cmd = robot_state_proto.GetRobotState() # self._start_robot_command(cmd) # robot_command_pb2.clear_behavior_fault(behavior_fault_id = , lease = self._lease) def _toggle_power(self): power_state = self._power_state() if power_state is None: self.add_message( 'Could not toggle power because power state is unknown') return if power_state == robot_state_proto.PowerState.STATE_OFF: self._try_grpc("powering-on", self._request_power_on) else: self._try_grpc("powering-off", self._safe_power_off) def _request_power_on(self): bosdyn.client.power.power_on(self._power_client) def _safe_power_off(self): self._start_robot_command('safe_power_off', RobotCommandBuilder.safe_power_off_command()) def _show_metrics(self, stdscr): metrics = self.robot_metrics if not metrics: return for idx, metric in enumerate(metrics.metrics): stdscr.addstr(2 + idx, 50, format_metric(metric)) def _power_state(self): state = self.robot_state if not state: return None return state.power_state.motor_power_state def _lease_str(self, lease_keep_alive): alive = 'RUNNING' if lease_keep_alive.is_alive() else 'STOPPED' lease = '{}:{}'.format(self._lease.lease_proto.resource, self._lease.lease_proto.sequence) return 'Lease {} THREAD:{}'.format(lease, alive) def _power_state_str(self): power_state = self._power_state() if power_state is None: return '' state_str = robot_state_proto.PowerState.MotorPowerState.Name( power_state) return 'Power: {}'.format(state_str[6:]) # get rid of STATE_ prefix def _estop_str(self): thread_status = 'RUNNING' if self._estop_keepalive else 'STOPPED' estop_status = '??' state = self.robot_state if state: for estop_state in state.estop_states: if estop_state.type == estop_state.TYPE_SOFTWARE: estop_status = estop_state.State.Name( estop_state.state)[6:] # s/STATE_// break return 'Estop {} (thread: {})'.format(estop_status, thread_status) def _time_sync_str(self): if not self._robot.time_sync: return 'Time sync: (none)' if self._robot.time_sync.stopped: status = 'STOPPED' exception = self._robot.time_sync.thread_exception if exception: status = '{} Exception: {}'.format(status, exception) else: status = 'RUNNING' try: skew = self._robot.time_sync.get_robot_clock_skew() if skew: skew_str = 'offset={}'.format(duration_str(skew)) else: skew_str = "(Skew undetermined)" except (TimeSyncError, RpcError) as err: skew_str = '({})'.format(err) return 'Time sync: {} {}'.format(status, skew_str) def _battery_str(self): if not self.robot_state: return '' battery_state = self.robot_state.battery_states[0] status = battery_state.Status.Name(battery_state.status) status = status[7:] # get rid of STATUS_ prefix if battery_state.charge_percentage.value: bar_len = int(battery_state.charge_percentage.value) // 10 bat_bar = '|{}{}|'.format('=' * bar_len, ' ' * (10 - bar_len)) else: bat_bar = '' time_left = '' if battery_state.estimated_runtime: time_left = ' ({})'.format( secs_to_hms(battery_state.estimated_runtime.seconds)) return 'Battery: {}{}{}'.format(status, bat_bar, time_left)
class SpotWrapper(): """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically""" def __init__(self, username, password, hostname, logger, rates={}, callbacks={}): self._username = username self._password = password self._hostname = hostname self._logger = logger self._rates = rates self._callbacks = callbacks self._keep_alive = True self._valid = True self._mobility_params = RobotCommandBuilder.mobility_params() self._is_standing = False self._is_sitting = True self._is_moving = False self._last_stand_command = None self._last_sit_command = None self._last_motion_command = None self._last_motion_command_time = 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.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.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.FORMAT_RAW)) try: self._sdk = create_standard_sdk('ros_spot') except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False return self._robot = self._sdk.create_robot(self._hostname) try: self._robot.authenticate(self._username, self._password) self._robot.start_time_sync() except RpcError as err: self._logger.error("Failed to communicate with robot: %s", err) self._valid = False return if self._robot: # Clients try: self._robot_state_client = self._robot.ensure_client( RobotStateClient.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) except Exception as e: self._logger.error("Unable to create client service: %s", e) self._valid = False return # 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", lambda: 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", lambda: None)) self._lease_task = AsyncLease( self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", lambda: 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", lambda: 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", lambda: 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", lambda: None), self._rear_image_requests) self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) self._estop_endpoint = None self._async_tasks = AsyncTasks([ self._robot_state_task, self._robot_metrics_task, self._lease_task, self._front_image_task, self._side_image_task, self._rear_image_task, self._idle_task ]) self._robot_id = None self._lease = None @property def is_valid(self): """Return boolean indicating if the wrapper initialized successfully""" return self._valid @property def id(self): """Return robot's ID""" return self._robot_id @property def robot_state(self): """Return latest proto from the _robot_state_task""" return self._robot_state_task.proto @property def metrics(self): """Return latest proto from the _robot_metrics_task""" return self._robot_metrics_task.proto @property def lease(self): """Return latest proto from the _lease_task""" return self._lease_task.proto @property def front_images(self): """Return latest proto from the _front_image_task""" return self._front_image_task.proto @property def side_images(self): """Return latest proto from the _side_image_task""" return self._side_image_task.proto @property def rear_images(self): """Return latest proto from the _rear_image_task""" return self._rear_image_task.proto @property def is_standing(self): """Return boolean of standing state""" return self._is_standing @property def is_sitting(self): """Return boolean of standing state""" return self._is_sitting @property def is_moving(self): """Return boolean of walking state""" return self._is_moving @property def time_skew(self): """Return the time skew between local and spot time""" return self._robot.time_sync.endpoint.clock_skew def robotToLocalTime(self, timestamp): """Takes a timestamp and an estimated skew and return seconds and nano seconds Args: timestamp: google.protobuf.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 def claim(self): """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" try: self._robot_id = self._robot.get_id() self.getLease() self.resetEStop() return True, "Success" except (ResponseError, RpcError) as err: self._logger.error("Failed to initialize robot communication: %s", err) return False, str(err) def updateTasks(self): """Loop through all periodic tasks and update their data if needed.""" self._async_tasks.update() def resetEStop(self): """Get keepalive for eStop""" self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0) self._estop_endpoint.force_simple_setup( ) # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) def assertEStop(self, severe=True): """Forces the robot into eStop state. Args: severe: Default True - If true, will cut motor power immediately. If false, will try to settle the robot on the ground first """ try: if severe: self._estop_endpoint.stop() else: self._estop_endpoint.settle_then_cut() return True, "Success" except: return False, "Error" def releaseEStop(self): """Stop eStop keepalive""" if self._estop_keepalive: self._estop_keepalive.stop() self._estop_keepalive = None self._estop_endpoint = None def getLease(self): """Get a lease for the robot and keep the lease alive automatically.""" self._lease = self._lease_client.acquire() self._lease_keepalive = LeaseKeepAlive(self._lease_client) def releaseLease(self): """Return the lease on the body.""" if self._lease: self._lease_client.return_lease(self._lease) self._lease = None def release(self): """Return the lease on the body and the eStop handle.""" try: self.releaseLease() self.releaseEStop() return True, "Success" except Exception as e: return False, str(e) def disconnect(self): """Release control of robot as gracefully as posssible.""" if self._robot.time_sync: self._robot.time_sync.stop() self.releaseLease() self.releaseEStop() def _robot_command(self, command_proto, end_time_secs=None): """Generic blocking function for sending commands to robots. Args: command_proto: robot_command_pb2 object to send to the robot. Usually made with RobotCommandBuilder end_time_secs: (optional) Time-to-live for the command in seconds """ try: id = self._robot_command_client.robot_command( lease=None, command=command_proto, end_time_secs=end_time_secs) return True, "Success", id except Exception as e: return False, str(e), None def stop(self): """Stop the robot's motion.""" response = self._robot_command(RobotCommandBuilder.stop_command()) return response[0], response[1] def self_right(self): """Have the robot self-right itself.""" response = self._robot_command(RobotCommandBuilder.selfright_command()) return response[0], response[1] def sit(self): """Stop the robot's motion and sit down if able.""" response = self._robot_command(RobotCommandBuilder.sit_command()) self._last_sit_command = response[2] return response[0], response[1] def 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)) if monitor_command: self._last_stand_command = response[2] return response[0], response[1] def safe_power_off(self): """Stop the robot's motion and sit if possible. Once sitting, disable motor power.""" response = self._robot_command( RobotCommandBuilder.safe_power_off_command()) return response[0], response[1] def power_on(self): """Enble the motor power if e-stop is enabled.""" try: power.power_on(self._power_client) return True, "Success" except: return False, "Error" def set_mobility_params(self, mobility_params): """Set Params for mobility and movement Args: mobility_params: spot.MobilityParams, params for spot mobility commands. """ self._mobility_params = mobility_params def get_mobility_params(self): """Get mobility params """ return self._mobility_params def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.1): """Send a velocity motion command to the robot. Args: v_x: Velocity in the X direction in meters v_y: Velocity in the Y direction in meters v_rot: Angular velocity around the Z axis in radians cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate). """ end_time = time.time() + cmd_duration self._robot_command(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) self._last_motion_command_time = end_time def list_graph(self, upload_path): """List waypoint ids of garph_nav Args: upload_path : Path to the root directory of the map. """ ids, eds = self._list_graph_waypoint_and_edge_ids() # skip waypoint_ for v2.2.1, skip waypiont for < v2.2 return [ v for k, v in sorted( ids.items(), key=lambda id: int(id[0].replace('waypoint_', ''))) ] def 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 ## 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() print('Got localization: \n%s' % str(state.localization)) odom_tform_body = get_odom_tform_body( state.robot_kinematics.transforms_snapshot) print('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. print("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) 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: print("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) 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.""" print("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) print("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. print("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) print("Uploaded {}".format(waypoint_snapshot.id)) for edge_snapshot in self._current_edge_snapshots.values(): self._graph_nav_client.upload_edge_snapshot(edge_snapshot) print("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. print("\n") print("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.") 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. print("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) 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): print( "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( .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." 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. print("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) 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 print("Failed to find an edge between waypoints: ", start_wp, " and ", end_wp) print( "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): print( "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( .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) 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(.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: print( "Robot got lost when navigating the route, the robot will now sit down." ) return True elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: print( "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: print("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
class SpotWrapper(): """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically""" def __init__(self, username, password, hostname, logger, rates={}, callbacks={}): self._username = username self._password = password self._hostname = hostname self._logger = logger self._rates = rates self._callbacks = callbacks self._keep_alive = True self._valid = True self._mobility_params = RobotCommandBuilder.mobility_params() self._is_standing = False self._is_sitting = True self._is_moving = False self._last_stand_command = None self._last_sit_command = None self._last_motion_command = None self._last_motion_command_time = 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.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.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.FORMAT_RAW)) try: self._sdk = create_standard_sdk('ros_spot') except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False return self._robot = self._sdk.create_robot(self._hostname) try: self._robot.authenticate(self._username, self._password) self._robot.start_time_sync() except RpcError as err: self._logger.error("Failed to communicate with robot: %s", err) self._valid = False return if self._robot: # Clients try: self._robot_state_client = self._robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = self._robot.ensure_client( RobotCommandClient.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._image_client = self._robot.ensure_client( ImageClient.default_service_name) self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) except Exception as e: self._logger.error("Unable to create client service: %s", e) self._valid = False return # 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", lambda: 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", lambda: None)) self._lease_task = AsyncLease( self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", lambda: 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", lambda: 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", lambda: 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", lambda: None), self._rear_image_requests) self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) self._estop_endpoint = None self._async_tasks = AsyncTasks([ self._robot_state_task, self._robot_metrics_task, self._lease_task, self._front_image_task, self._side_image_task, self._rear_image_task, self._idle_task ]) self._robot_id = None self._lease = None @property def is_valid(self): """Return boolean indicating if the wrapper initialized successfully""" return self._valid @property def id(self): """Return robot's ID""" return self._robot_id @property def robot_state(self): """Return latest proto from the _robot_state_task""" return self._robot_state_task.proto @property def metrics(self): """Return latest proto from the _robot_metrics_task""" return self._robot_metrics_task.proto @property def lease(self): """Return latest proto from the _lease_task""" return self._lease_task.proto @property def front_images(self): """Return latest proto from the _front_image_task""" return self._front_image_task.proto @property def side_images(self): """Return latest proto from the _side_image_task""" return self._side_image_task.proto @property def rear_images(self): """Return latest proto from the _rear_image_task""" return self._rear_image_task.proto @property def is_standing(self): """Return boolean of standing state""" return self._is_standing @property def is_sitting(self): """Return boolean of standing state""" return self._is_sitting @property def is_moving(self): """Return boolean of walking state""" return self._is_moving @property def time_skew(self): """Return the time skew between local and spot time""" return self._robot.time_sync.endpoint.clock_skew def robotToLocalTime(self, timestamp): """Takes a timestamp and an estimated skew and return seconds and nano seconds Args: timestamp: google.protobuf.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 def claim(self): """Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot.""" try: self._robot_id = self._robot.get_id() self.getLease() self.resetEStop() return True, "Success" except (ResponseError, RpcError) as err: self._logger.error("Failed to initialize robot communication: %s", err) return False, str(err) def updateTasks(self): """Loop through all periodic tasks and update their data if needed.""" self._async_tasks.update() def resetEStop(self): """Get keepalive for eStop""" self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0) self._estop_endpoint.force_simple_setup( ) # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) def assertEStop(self, severe=True): """Forces the robot into eStop state. Args: severe: Default True - If true, will cut motor power immediately. If false, will try to settle the robot on the ground first """ try: if severe: self._estop_endpoint.stop() else: self._estop_endpoint.settle_then_cut() return True, "Success" except: return False, "Error" def releaseEStop(self): """Stop eStop keepalive""" if self._estop_keepalive: self._estop_keepalive.stop() self._estop_keepalive = None self._estop_endpoint = None def getLease(self): """Get a lease for the robot and keep the lease alive automatically.""" self._lease = self._lease_client.acquire() self._lease_keepalive = LeaseKeepAlive(self._lease_client) def releaseLease(self): """Return the lease on the body.""" if self._lease: self._lease_client.return_lease(self._lease) self._lease = None def release(self): """Return the lease on the body and the eStop handle.""" try: self.releaseLease() self.releaseEStop() return True, "Success" except Exception as e: return False, str(e) def disconnect(self): """Release control of robot as gracefully as posssible.""" if self._robot.time_sync: self._robot.time_sync.stop() self.releaseLease() self.releaseEStop() def _robot_command(self, command_proto, end_time_secs=None): """Generic blocking function for sending commands to robots. Args: command_proto: robot_command_pb2 object to send to the robot. Usually made with RobotCommandBuilder end_time_secs: (optional) Time-to-live for the command in seconds """ try: id = self._robot_command_client.robot_command( lease=None, command=command_proto, end_time_secs=end_time_secs) return True, "Success", id except Exception as e: return False, str(e), None def stop(self): """Stop the robot's motion.""" response = self._robot_command(RobotCommandBuilder.stop_command()) return response[0], response[1] def self_right(self): """Have the robot self-right itself.""" response = self._robot_command(RobotCommandBuilder.selfright_command()) return response[0], response[1] def sit(self): """Stop the robot's motion and sit down if able.""" response = self._robot_command(RobotCommandBuilder.sit_command()) self._last_sit_command = response[2] return response[0], response[1] def 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.stand_command(params=self._mobility_params)) if monitor_command: self._last_stand_command = response[2] return response[0], response[1] def safe_power_off(self): """Stop the robot's motion and sit if possible. Once sitting, disable motor power.""" response = self._robot_command( RobotCommandBuilder.safe_power_off_command()) return response[0], response[1] def power_on(self): """Enble the motor power if e-stop is enabled.""" try: power.power_on(self._power_client) return True, "Success" except: return False, "Error" def set_mobility_params(self, body_height=0, footprint_R_body=EulerZXY(), locomotion_hint=1, stair_hint=False, external_force_params=None): """Define body, locomotion, and stair parameters. Args: body_height: Body height in meters footprint_R_body: (EulerZXY) – The orientation of the body frame with respect to the footprint frame (gravity aligned framed with yaw computed from the stance feet) locomotion_hint: Locomotion hint stair_hint: Boolean to define stair motion """ self._mobility_params = RobotCommandBuilder.mobility_params( body_height, footprint_R_body, locomotion_hint, stair_hint, external_force_params) def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.1): """Send a velocity motion command to the robot. Args: v_x: Velocity in the X direction in meters v_y: Velocity in the Y direction in meters v_rot: Angular velocity around the Z axis in radians cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate). """ end_time = time.time() + cmd_duration self._robot_command(RobotCommandBuilder.velocity_command( v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), end_time_secs=end_time) self._last_motion_command_time = end_time
def __init__(self, username, password, token, hostname, logger, rates={}, callbacks={}): self._username = username self._password = password self._token = token self._hostname = hostname self._logger = logger self._rates = rates self._callbacks = callbacks self._keep_alive = True self._valid = True self._mobility_params = RobotCommandBuilder.mobility_params() self._is_standing = False self._is_sitting = True self._is_moving = False self._last_stand_command = None self._last_sit_command = None self._last_motion_command = None self._last_motion_command_time = 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.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.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.FORMAT_RAW)) try: self._sdk = create_standard_sdk('ros_spot') except Exception as e: self._logger.error("Error creating SDK object: %s", e) self._valid = False return try: self._sdk.load_app_token(self._token) except Exception as e: self._logger.error("Error loading developer token: %s", e) self._valid = False return self._robot = self._sdk.create_robot(self._hostname) try: self._robot.authenticate(self._username, self._password) self._robot.start_time_sync() except RpcError as err: self._logger.error("Failed to communicate with robot: %s", err) self._valid = False return if self._robot: # Clients try: self._robot_state_client = self._robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = self._robot.ensure_client( RobotCommandClient.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._image_client = self._robot.ensure_client( ImageClient.default_service_name) self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) except Exception as e: self._logger.error("Unable to create client service: %s", e) self._valid = False return # 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", lambda: 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", lambda: None)) self._lease_task = AsyncLease( self._lease_client, self._logger, max(0.0, self._rates.get("lease", 0.0)), self._callbacks.get("lease", lambda: 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", lambda: 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", lambda: 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", lambda: None), self._rear_image_requests) self._idle_task = AsyncIdle(self._robot_command_client, self._logger, 10.0, self) self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0) self._async_tasks = AsyncTasks([ self._robot_state_task, self._robot_metrics_task, self._lease_task, self._front_image_task, self._side_image_task, self._rear_image_task, self._idle_task ]) self._robot_id = None self._lease = None
class RecorderInterface(object): """A curses interface for recording robot missions.""" def __init__(self, robot, download_filepath): self._robot = robot # Flag indicating whether mission is currently being recorded self._recording = False # Flag indicating whether robot is in feature desert mode self._desert_mode = False # Filepath for the location to put the downloaded graph and snapshots. self._download_filepath = download_filepath # List of waypoint commands self._waypoint_commands = [] # Dictionary indicating which waypoints are deserts self._desert_flag = {} # Current waypoint id self._waypoint_id = 'NONE' # Create clients -- do not use the for communication yet. self._lease_client = robot.ensure_client( LeaseClient.default_service_name) try: self._estop_client = self._robot.ensure_client( EstopClient.default_service_name) self._estop_endpoint = EstopEndpoint(self._estop_client, 'GNClient', 9.0) except: # Not the estop. self._estop_client = None self._estop_endpoint = None self._map_processing_client = robot.ensure_client( MapProcessingServiceClient.default_service_name) self._power_client = robot.ensure_client( PowerClient.default_service_name) self._robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) self._world_object_client = robot.ensure_client( WorldObjectClient.default_service_name) # Setup the recording service client. self._recording_client = self._robot.ensure_client( GraphNavRecordingServiceClient.default_service_name) # Setup the graph nav service client. self._graph_nav_client = robot.ensure_client( GraphNavClient.default_service_name) # Local copy of the graph. self._graph = None self._all_graph_wps_in_order = [] self._robot_state_task = AsyncRobotState(self._robot_state_client) self._async_tasks = AsyncTasks([self._robot_state_task]) self._lock = threading.Lock() self._command_dictionary = { 27: self._stop, # ESC key ord('\t'): self._quit_program, ord('T'): self._toggle_time_sync, ord(' '): self._toggle_estop, ord('r'): self._self_right, ord('P'): self._toggle_power, ord('v'): self._sit, ord('f'): self._stand, ord('w'): self._move_forward, ord('s'): self._move_backward, ord('a'): self._strafe_left, ord('c'): self._auto_close_loops, ord('d'): self._strafe_right, ord('q'): self._turn_left, ord('e'): self._turn_right, ord('m'): self._start_recording, ord('l'): self._relocalize, ord('z'): self._enter_desert, ord('x'): self._exit_desert, ord('g'): self._generate_mission } self._locked_messages = ['', '', ''] # string: displayed message for user self._estop_keepalive = None self._exit_check = None # Stuff that is set in start() self._robot_id = None self._lease = None def start(self): """Begin communication with the robot.""" self._lease = self._lease_client.acquire() self._robot_id = self._robot.get_id() if self._estop_endpoint is not None: self._estop_endpoint.force_simple_setup( ) # Set this endpoint as the robot's sole estop. # Clear existing graph nav map self._graph_nav_client.clear_graph() def shutdown(self): """Release control of robot as gracefully as possible.""" LOGGER.info("Shutting down WasdInterface.") if self._estop_keepalive: # This stops the check-in thread but does not stop the robot. self._estop_keepalive.shutdown() if self._lease: _grpc_or_log("returning lease", lambda: self._lease_client.return_lease(self._lease)) self._lease = None def __del__(self): self.shutdown() def flush_and_estop_buffer(self, stdscr): """Manually flush the curses input buffer but trigger any estop requests (space)""" key = '' while key != -1: key = stdscr.getch() if key == ord(' '): self._toggle_estop() def add_message(self, msg_text): """Display the given message string to the user in the curses interface.""" with self._lock: self._locked_messages = [msg_text] + self._locked_messages[:-1] def message(self, idx): """Grab one of the 3 last messages added.""" with self._lock: return self._locked_messages[idx] @property def robot_state(self): """Get latest robot state proto.""" return self._robot_state_task.proto def drive(self, stdscr): """User interface to control the robot via the passed-in curses screen interface object.""" with LeaseKeepAlive(self._lease_client) as lease_keep_alive, \ ExitCheck() as self._exit_check: curses_handler = CursesHandler(self) curses_handler.setLevel(logging.INFO) LOGGER.addHandler(curses_handler) stdscr.nodelay(True) # Don't block for user input. stdscr.resize(26, 96) stdscr.refresh() # for debug curses.echo() try: while not self._exit_check.kill_now: self._async_tasks.update() self._drive_draw(stdscr, lease_keep_alive) try: cmd = stdscr.getch() # Do not queue up commands on client self.flush_and_estop_buffer(stdscr) self._drive_cmd(cmd) time.sleep(COMMAND_INPUT_RATE) except Exception: # On robot command fault, sit down safely before killing the program. self._safe_power_off() time.sleep(2.0) self.shutdown() raise finally: LOGGER.removeHandler(curses_handler) def _count_visible_fiducials(self): """Return number of fiducials visible to robot.""" request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG] fiducial_objects = self._world_object_client.list_world_objects( object_type=request_fiducials).world_objects return len(fiducial_objects) def _drive_draw(self, stdscr, lease_keep_alive): """Draw the interface screen at each update.""" stdscr.clear() # clear screen stdscr.resize(26, 96) stdscr.addstr( 0, 0, '{:20s} {}'.format(self._robot_id.nickname, self._robot_id.serial_number)) stdscr.addstr(1, 0, self._lease_str(lease_keep_alive)) stdscr.addstr(2, 0, self._battery_str()) stdscr.addstr(3, 0, self._estop_str()) stdscr.addstr(4, 0, self._power_state_str()) stdscr.addstr(5, 0, self._time_sync_str()) stdscr.addstr(6, 0, self._waypoint_str()) stdscr.addstr(7, 0, self._fiducial_str()) stdscr.addstr(8, 0, self._desert_str()) for i in range(3): stdscr.addstr(10 + i, 2, self.message(i)) stdscr.addstr(14, 0, "Commands: [TAB]: quit ") stdscr.addstr(15, 0, " [T]: Time-sync, [SPACE]: Estop, [P]: Power") stdscr.addstr(16, 0, " [v]: Sit, [f]: Stand, [r]: Self-right ") stdscr.addstr(17, 0, " [wasd]: Directional strafing ") stdscr.addstr(18, 0, " [qe]: Turning, [ESC]: Stop ") stdscr.addstr(19, 0, " [m]: Start recording mission ") stdscr.addstr(20, 0, " [l]: Add fiducial localization to mission ") stdscr.addstr(21, 0, " [z]: Enter desert mode ") stdscr.addstr(22, 0, " [x]: Exit desert mode ") stdscr.addstr(23, 0, " [g]: Stop recording and generate mission ") stdscr.refresh() def _drive_cmd(self, key): """Run user commands at each update.""" try: cmd_function = self._command_dictionary[key] cmd_function() except KeyError: if key and key != -1 and key < 256: self.add_message("Unrecognized keyboard command: '{}'".format( chr(key))) def _try_grpc(self, desc, thunk): try: return thunk() except (ResponseError, RpcError) as err: self.add_message("Failed {}: {}".format(desc, err)) return None def _quit_program(self): if self._recording: self._stop_recording() self._robot.power_off() self.shutdown() if self._exit_check is not None: self._exit_check.request_exit() def _toggle_time_sync(self): if self._robot.time_sync.stopped: self._robot.time_sync.start() else: self._robot.time_sync.stop() def _toggle_estop(self): """toggle estop on/off. Initial state is ON""" if self._estop_client is not None and self._estop_endpoint is not None: if not self._estop_keepalive: self._estop_keepalive = EstopKeepAlive(self._estop_endpoint) else: self._try_grpc("stopping estop", self._estop_keepalive.stop) self._estop_keepalive.shutdown() self._estop_keepalive = None def _start_robot_command(self, desc, command_proto, end_time_secs=None): def _start_command(): self._robot_command_client.robot_command( lease=None, command=command_proto, end_time_secs=end_time_secs) self._try_grpc(desc, _start_command) def _self_right(self): self._start_robot_command('self_right', RobotCommandBuilder.selfright_command()) def _sit(self): self._start_robot_command('sit', RobotCommandBuilder.synchro_sit_command()) def _stand(self): self._start_robot_command('stand', RobotCommandBuilder.synchro_stand_command()) def _move_forward(self): self._velocity_cmd_helper('move_forward', v_x=VELOCITY_BASE_SPEED) def _move_backward(self): self._velocity_cmd_helper('move_backward', v_x=-VELOCITY_BASE_SPEED) def _strafe_left(self): self._velocity_cmd_helper('strafe_left', v_y=VELOCITY_BASE_SPEED) def _strafe_right(self): self._velocity_cmd_helper('strafe_right', v_y=-VELOCITY_BASE_SPEED) def _turn_left(self): self._velocity_cmd_helper('turn_left', v_rot=VELOCITY_BASE_ANGULAR) def _turn_right(self): self._velocity_cmd_helper('turn_right', v_rot=-VELOCITY_BASE_ANGULAR) def _stop(self): self._start_robot_command('stop', RobotCommandBuilder.stop_command()) def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0): self._start_robot_command( desc, RobotCommandBuilder.synchro_velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot), end_time_secs=time.time() + VELOCITY_CMD_DURATION) def _return_to_origin(self): self._start_robot_command( 'fwd_and_rotate', RobotCommandBuilder.synchro_se2_trajectory_point_command( goal_x=0.0, goal_y=0.0, goal_heading=0.0, frame_name=ODOM_FRAME_NAME, params=None, body_height=0.0, locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT), end_time_secs=time.time() + 20) def _toggle_power(self): power_state = self._power_state() if power_state is None: self.add_message( 'Could not toggle power because power state is unknown') return if power_state == robot_state_proto.PowerState.STATE_OFF: self._try_grpc("powering-on", self._request_power_on) else: self._try_grpc("powering-off", self._safe_power_off) def _request_power_on(self): bosdyn.client.power.power_on(self._power_client) def _safe_power_off(self): self._start_robot_command('safe_power_off', RobotCommandBuilder.safe_power_off_command()) def _power_state(self): state = self.robot_state if not state: return None return state.power_state.motor_power_state def _lease_str(self, lease_keep_alive): alive = 'RUNNING' if lease_keep_alive.is_alive() else 'STOPPED' lease = '{}:{}'.format(self._lease.lease_proto.resource, self._lease.lease_proto.sequence) return 'Lease {} THREAD:{}'.format(lease, alive) def _power_state_str(self): power_state = self._power_state() if power_state is None: return '' state_str = robot_state_proto.PowerState.MotorPowerState.Name( power_state) return 'Power: {}'.format(state_str[6:]) # get rid of STATE_ prefix def _estop_str(self): if not self._estop_client: thread_status = 'NOT ESTOP' else: thread_status = 'RUNNING' if self._estop_keepalive else 'STOPPED' estop_status = '??' state = self.robot_state if state: for estop_state in state.estop_states: if estop_state.type == estop_state.TYPE_SOFTWARE: estop_status = estop_state.State.Name( estop_state.state)[6:] # s/STATE_// break return 'Estop {} (thread: {})'.format(estop_status, thread_status) def _time_sync_str(self): if not self._robot.time_sync: return 'Time sync: (none)' if self._robot.time_sync.stopped: status = 'STOPPED' exception = self._robot.time_sync.thread_exception if exception: status = '{} Exception: {}'.format(status, exception) else: status = 'RUNNING' try: skew = self._robot.time_sync.get_robot_clock_skew() if skew: skew_str = 'offset={}'.format(duration_str(skew)) else: skew_str = "(Skew undetermined)" except (TimeSyncError, RpcError) as err: skew_str = '({})'.format(err) return 'Time sync: {} {}'.format(status, skew_str) def _waypoint_str(self): state = self._graph_nav_client.get_localization_state() try: self._waypoint_id = state.localization.waypoint_id if self._waypoint_id == '': self._waypoint_id = 'NONE' except: self._waypoint_id = 'ERROR' if self._recording and self._waypoint_id != 'NONE' and self._waypoint_id != 'ERROR': if self._waypoint_id not in self._desert_flag: self._desert_flag[self._waypoint_id] = self._desert_mode return 'Current waypoint: {} [ RECORDING ]'.format( self._waypoint_id) return 'Current waypoint: {}'.format(self._waypoint_id) def _fiducial_str(self): return 'Visible fiducials: {}'.format( str(self._count_visible_fiducials())) def _desert_str(self): if self._desert_mode: return '[ FEATURE DESERT MODE ]' else: return '' def _battery_str(self): if not self.robot_state: return '' battery_state = self.robot_state.battery_states[0] status = battery_state.Status.Name(battery_state.status) status = status[7:] # get rid of STATUS_ prefix if battery_state.charge_percentage.value: bar_len = int(battery_state.charge_percentage.value) // 10 bat_bar = '|{}{}|'.format('=' * bar_len, ' ' * (10 - bar_len)) else: bat_bar = '' time_left = '' if battery_state.estimated_runtime: time_left = ' ({})'.format( secs_to_hms(battery_state.estimated_runtime.seconds)) return 'Battery: {}{}{}'.format(status, bat_bar, time_left) def _fiducial_visible(self): """Return True if robot can see fiducial.""" request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG] fiducial_objects = self._world_object_client.list_world_objects( object_type=request_fiducials).world_objects self.add_message("Fiducial objects: " + str(fiducial_objects)) if len(fiducial_objects) > 0: return True return False def _start_recording(self): """Start recording a map.""" if self._count_visible_fiducials() == 0: self.add_message( "ERROR: Can't start recording -- No fiducials in view.") return if self._waypoint_id is None: self.add_message("ERROR: Not localized to waypoint.") return # Tell graph nav to start recording map status = self._recording_client.start_recording() if status != recording_pb2.StartRecordingResponse.STATUS_OK: self.add_message("Start recording failed.") return self.add_message("Started recording map.") self._graph = None self._all_graph_wps_in_order = [] state = self._graph_nav_client.get_localization_state() if '' == state.localization.waypoint_id: self.add_message("No localization after start recording.") self._recording_client.stop_recording() return self._waypoint_commands = [] # Treat this sort of like RPN / postfix, where operators follow their operands. # waypoint_id LOCALIZE means "localize to waypoint_id". # INITIALIZE takes no argument. # Implicitly, we make routes between pairs of waypoint ids as they occur. # `w0 w1 LOCALIZE w2` will give a route w0->w1, whereupon we localize, # and then a route w1->w2. self._waypoint_commands.append('INITIALIZE') # Start with the first waypoint. self._waypoint_commands.append(state.localization.waypoint_id) self._recording = True def _stop_recording(self): """Stop or pause recording a map.""" if not self._recording: return True self._recording = False if self._waypoint_id != self._waypoint_commands[-1]: self._waypoint_commands += [self._waypoint_id] try: status = self._recording_client.stop_recording() if status != recording_pb2.StopRecordingResponse.STATUS_OK: self.add_message("Stop recording failed.") return False self.add_message("Successfully stopped recording a map.") return True except NotLocalizedToEndError: self.add_message( "ERROR: Move to final waypoint before stopping recording.") return False def _relocalize(self): """Insert localization node into mission.""" if not self._recording: print('Not recording mission.') return False self.add_message("Adding fiducial localization to mission.") self._waypoint_commands += [self._waypoint_id] self._waypoint_commands += ['LOCALIZE'] # Return to waypoint (in case localization shifted to another waypoint) self._waypoint_commands += [self._waypoint_id] return True def _enter_desert(self): self._desert_mode = True if self._recording: if self._waypoint_commands == [] or self._waypoint_commands[ -1] != self._waypoint_id: self._waypoint_commands += [self._waypoint_id] def _exit_desert(self): self._desert_mode = False if self._recording: if self._waypoint_commands == [] or self._waypoint_commands[ -1] != self._waypoint_id: self._waypoint_commands += [self._waypoint_id] def _generate_mission(self): """Save graph map and mission file.""" # Check whether mission has been recorded if not self._recording: self.add_message("ERROR: No mission recorded.") return # Check for empty mission if len(self._waypoint_commands) == 0: self.add_message("ERROR: No waypoints in mission.") return # Stop recording mission if not self._stop_recording(): self.add_message("ERROR: Error while stopping recording.") return # Save graph map os.mkdir(self._download_filepath) if not self._download_full_graph(): self.add_message("ERROR: Error downloading graph.") return # Generate mission mission = self._make_mission() # Save mission file os.mkdir(os.path.join(self._download_filepath, "missions")) mission_filepath = os.path.join(self._download_filepath, "missions", "autogenerated") write_mission(mission, mission_filepath) # Quit program self._quit_program() def _make_desert(self, waypoint): waypoint.annotations.scan_match_region.empty.CopyFrom( map_pb2.Waypoint.Annotations.LocalizeRegion.Empty()) def _download_full_graph(self, overwrite_desert_flag=None): """Download the graph and snapshots from the robot.""" graph = self._graph_nav_client.download_graph() if graph is None: self.add_message("Failed to download the graph.") return False if overwrite_desert_flag is not None: for wp in graph.waypoints: if wp.id in overwrite_desert_flag: # Overwrite anything that we passed in. self._desert_flag[wp.id] = overwrite_desert_flag[wp.id] elif wp.id not in self._desert_flag: self._desert_flag[wp.id] = False # Mark desert waypoints for waypoint in graph.waypoints: if self._desert_flag[waypoint.id]: self._make_desert(waypoint) # Write graph map self._write_full_graph(graph) self.add_message( "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) # Cache a list of all graph waypoints, ordered by creation time. # Because we only record one (non-branching) chain, all possible routes are contained # in ranges in this list. self._graph = graph wp_to_time = [] for wp in graph.waypoints: time = wp.annotations.creation_time.seconds + wp.annotations.creation_time.nanos / 1e9 wp_to_time.append((wp.id, time)) # Switch inner and outer grouping and grab only the waypoint names after sorting by time. self._all_graph_wps_in_order = list( zip(*sorted(wp_to_time, key=lambda x: x[1])))[0] return True def _write_full_graph(self, graph): """Download the graph from robot to the specified, local filepath location.""" graph_bytes = graph.SerializeToString() 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: 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.add_message("Failed to download waypoint snapshot: " + waypoint.snapshot_id) continue write_bytes( os.path.join(self._download_filepath, 'waypoint_snapshots'), waypoint.snapshot_id, waypoint_snapshot.SerializeToString()) num_waypoint_snapshots_downloaded += 1 self.add_message( "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.add_message("Failed to download edge snapshot: " + edge.snapshot_id) continue write_bytes( os.path.join(self._download_filepath, 'edge_snapshots'), edge.snapshot_id, edge_snapshot.SerializeToString()) num_edge_snapshots_downloaded += 1 self.add_message( "Downloaded {} of the total {} edge snapshots.".format( num_edge_snapshots_downloaded, num_to_download)) def _auto_close_loops(self): """Automatically find and close all loops in the graph.""" close_fiducial_loops = True close_odometry_loops = True response = self._map_processing_client.process_topology( params=map_processing_pb2.ProcessTopologyRequest.Params( do_fiducial_loop_closure=wrappers.BoolValue( value=close_fiducial_loops), do_odometry_loop_closure=wrappers.BoolValue( value=close_odometry_loops)), modify_map_on_server=True) self.add_message("Created {} new edge(s).".format( len(response.new_subgraph.edges))) def _make_mission(self): """ Create a mission that visits each waypoint on stored path.""" self.add_message("Mission: " + str(self._waypoint_commands)) # Create a Sequence that visits all the waypoints. sequence = nodes_pb2.Sequence() prev_waypoint_command = None first_waypoint = True for waypoint_command in self._waypoint_commands: if waypoint_command == 'LOCALIZE': if prev_waypoint_command is None: raise RuntimeError(f'No prev waypoint; LOCALIZE') sequence.children.add().CopyFrom( self._make_localize_node(prev_waypoint_command)) elif waypoint_command == 'INITIALIZE': # Initialize to any fiducial. sequence.children.add().CopyFrom(self._make_initialize_node()) else: if first_waypoint: # Go to the beginning of the mission using any path. May be a no-op. sequence.children.add().CopyFrom( self._make_goto_node(waypoint_command)) else: if prev_waypoint_command is None: raise RuntimeError( f'No prev waypoint; route to {waypoint_command}') sequence.children.add().CopyFrom( self._make_go_route_node(prev_waypoint_command, waypoint_command)) prev_waypoint_command = waypoint_command first_waypoint = False # Return a Node with the Sequence. ret = nodes_pb2.Node() ret.name = "Visit %d goals" % len(self._waypoint_commands) ret.impl.Pack(sequence) return ret def _make_goto_node(self, waypoint_id): """ Create a leaf node that will go to the waypoint. """ ret = nodes_pb2.Node() ret.name = "goto %s" % waypoint_id vel_limit = NAV_VELOCITY_LIMITS # We don't actually know which path we will plan. It could have many feature deserts. # So, let's just allow feature deserts. tolerance = graph_nav_pb2.TravelParams.TOLERANCE_IGNORE_POOR_FEATURE_QUALITY travel_params = graph_nav_pb2.TravelParams( velocity_limit=vel_limit, feature_quality_tolerance=tolerance) impl = nodes_pb2.BosdynNavigateTo(travel_params=travel_params) impl.destination_waypoint_id = waypoint_id ret.impl.Pack(impl) return ret def _make_go_route_node(self, prev_waypoint_id, waypoint_id): """ Create a leaf node that will go to the waypoint along a route. """ ret = nodes_pb2.Node() ret.name = "go route %s -> %s" % (prev_waypoint_id, waypoint_id) vel_limit = NAV_VELOCITY_LIMITS if prev_waypoint_id not in self._all_graph_wps_in_order: raise RuntimeError( f'{prev_waypoint_id} (FROM) not in {self._all_graph_wps_in_order}' ) if waypoint_id not in self._all_graph_wps_in_order: raise RuntimeError( f'{waypoint_id} (TO) not in {self._all_graph_wps_in_order}') prev_wp_idx = self._all_graph_wps_in_order.index(prev_waypoint_id) wp_idx = self._all_graph_wps_in_order.index(waypoint_id) impl = nodes_pb2.BosdynNavigateRoute() backwards = False if wp_idx >= prev_wp_idx: impl.route.waypoint_id[:] = self._all_graph_wps_in_order[ prev_wp_idx:wp_idx + 1] else: backwards = True # Switch the indices and reverse the output order. impl.route.waypoint_id[:] = reversed( self._all_graph_wps_in_order[wp_idx:prev_wp_idx + 1]) edge_ids = [e.id for e in self._graph.edges] for i in range(len(impl.route.waypoint_id) - 1): eid = map_pb2.Edge.Id() # Because we only record one (non-branching) chain, and we only create routes from # older to newer waypoints, we can just follow the time-sorted list of all waypoints. if backwards: from_i = i + 1 to_i = i else: from_i = i to_i = i + 1 eid.from_waypoint = impl.route.waypoint_id[from_i] eid.to_waypoint = impl.route.waypoint_id[to_i] impl.route.edge_id.extend([eid]) if eid not in edge_ids: raise RuntimeError( f'Was map recorded in linear chain? {eid} not in graph') if any(self._desert_flag[wp_id] for wp_id in impl.route.waypoint_id): tolerance = graph_nav_pb2.TravelParams.TOLERANCE_IGNORE_POOR_FEATURE_QUALITY else: tolerance = graph_nav_pb2.TravelParams.TOLERANCE_DEFAULT travel_params = graph_nav_pb2.TravelParams( velocity_limit=vel_limit, feature_quality_tolerance=tolerance) impl.travel_params.CopyFrom(travel_params) ret.impl.Pack(impl) return ret def _make_localize_node(self, waypoint_id): """Make localization node.""" loc = nodes_pb2.Node() loc.name = "localize robot" impl = nodes_pb2.BosdynGraphNavLocalize() impl.localization_request.fiducial_init = graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NEAREST_AT_TARGET impl.localization_request.initial_guess.waypoint_id = waypoint_id loc.impl.Pack(impl) return loc def _make_initialize_node(self): """Make initialization node.""" loc = nodes_pb2.Node() loc.name = "initialize robot" impl = nodes_pb2.BosdynGraphNavLocalize() impl.localization_request.fiducial_init = graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NEAREST loc.impl.Pack(impl) return loc
def __init__(self, robot): #Robot instance variable self._robot = robot self._lease_client = robot.ensure_client( LeaseClient.default_service_name) self._estop_client = robot.ensure_client( EstopClient.default_service_name) self._estop_endpoint = EstopEndpoint(self._estop_client, 'fiducial_follow', 9.0) self._power_client = robot.ensure_client( PowerClient.default_service_name) self._image_client = robot.ensure_client( ImageClient.default_service_name) self._robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) #Fiducial dimensions (width, height) in millimeters self._fiducial_width_mm = 154.1 self._fiducial_height_mm = 154.1 #Detection parameters for the fiducial library self.blur = 0.0 #gaussian blur (useful for noisy images) #Stopping Distance (x,y) offset from the tag and angle offset from desired angle self._dist_x_desired = .05 self._dist_y_desired = .05 #Maximum speeds self._max_x_vel = 1.0 self._max_y_vel = 1.0 self._max_ang_vel = 1.0 #Indicators for movement and image displays self._display_images = True # Display live from the robot self._standup = True # Stand up the robot self._movement_on = True # Let the robot walk towards the fiducial self._limit_speed = False # Limit the robot's walking speed self._avoid_obstacles = False # Disable obstacle avoidance self._debug = False # Printouts for debugging purposes #Robot's id, lease and estop information self._robot_id = None self._lease = None self._lease_keepalive = None self._estop_keepalive = None # Epsilon distance between robot and april tag self._x_eps = .05 self._y_eps = .05 self._angle_eps = .075 # Indicator for if motor power is on self._powered_on = False # Counter for the number of iterations completed self._attempts = 0 # Maximum amount of iterations before powering off the motors self._max_attempts = 100000 # Camera intrinsics for the current camera source being analyzed self._intrinsics = None # Transform from the robot's camera frame to the baselink frame self._camera_T_body = None # Transform from the robot's baselink to the world frame self._body_T_world = None # Latest detected fiducial's position in the world self._current_tag_world_pose = np.array([]) # Heading angle based on the camera source which detected the fiducial self._angle_desired = None # Indicator if a fiducial has been detected this iteration self._tag_not_located = True # Dictionary mapping camera source to it's latest image taken self._image = dict() # List of all possible camera sources self._source_names = [ src.name for src in self._image_client.list_image_sources() if "image" in src.name ] # Dictionary mapping camera source to previously computed extrinsics self._cam_to_ext_guess = self.populate_source_dict() # Camera source which a bounding box was last detected in self._previous_source = None
class FiducialFollow(object): """ Detect and follow a fiducial with Spot""" def __init__(self, robot): #Robot instance variable self._robot = robot self._lease_client = robot.ensure_client( LeaseClient.default_service_name) self._estop_client = robot.ensure_client( EstopClient.default_service_name) self._estop_endpoint = EstopEndpoint(self._estop_client, 'fiducial_follow', 9.0) self._power_client = robot.ensure_client( PowerClient.default_service_name) self._image_client = robot.ensure_client( ImageClient.default_service_name) self._robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) self._robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) #Fiducial dimensions (width, height) in millimeters self._fiducial_width_mm = 154.1 self._fiducial_height_mm = 154.1 #Detection parameters for the fiducial library self.blur = 0.0 #gaussian blur (useful for noisy images) #Stopping Distance (x,y) offset from the tag and angle offset from desired angle self._dist_x_desired = .05 self._dist_y_desired = .05 #Maximum speeds self._max_x_vel = 1.0 self._max_y_vel = 1.0 self._max_ang_vel = 1.0 #Indicators for movement and image displays self._display_images = True # Display live from the robot self._standup = True # Stand up the robot self._movement_on = True # Let the robot walk towards the fiducial self._limit_speed = False # Limit the robot's walking speed self._avoid_obstacles = False # Disable obstacle avoidance self._debug = False # Printouts for debugging purposes #Robot's id, lease and estop information self._robot_id = None self._lease = None self._lease_keepalive = None self._estop_keepalive = None # Epsilon distance between robot and april tag self._x_eps = .05 self._y_eps = .05 self._angle_eps = .075 # Indicator for if motor power is on self._powered_on = False # Counter for the number of iterations completed self._attempts = 0 # Maximum amount of iterations before powering off the motors self._max_attempts = 100000 # Camera intrinsics for the current camera source being analyzed self._intrinsics = None # Transform from the robot's camera frame to the baselink frame self._camera_T_body = None # Transform from the robot's baselink to the world frame self._body_T_world = None # Latest detected fiducial's position in the world self._current_tag_world_pose = np.array([]) # Heading angle based on the camera source which detected the fiducial self._angle_desired = None # Indicator if a fiducial has been detected this iteration self._tag_not_located = True # Dictionary mapping camera source to it's latest image taken self._image = dict() # List of all possible camera sources self._source_names = [ src.name for src in self._image_client.list_image_sources() if "image" in src.name ] # Dictionary mapping camera source to previously computed extrinsics self._cam_to_ext_guess = self.populate_source_dict() # Camera source which a bounding box was last detected in self._previous_source = None @property def robot_state(self): """Get latest robot state proto.""" return self._robot_state_client.get_robot_state() @property def display_images(self): """Return if images should be displayed as video.""" return self._display_images @property def image(self): """Return the current image associated with each source name.""" return self._image @property def image_sources_list(self): """Return the list of camera sources.""" return self._source_names def populate_source_dict(self): """Fills dictionary of the most recently computed camera extrinsics with the camera source. The initial boolean indicates if the extrinsics guess should be used.""" camera_to_ext_guess = dict() for src in self._source_names: #Dictionary values: use_extrinsics_guess flag, (rvec, tvec) tuple camera_to_ext_guess[src] = (False, (None, None)) return camera_to_ext_guess def start(self): """Claim lease of robot and start the fiducial follower.""" self._lease = self._lease_client.acquire() self._estop_endpoint.force_simple_setup() self._robot.time_sync.wait_for_sync() with bosdyn.client.lease.LeaseKeepAlive(self._lease_client) as self._lease_keepalive, \ bosdyn.client.estop.EstopKeepAlive(self._estop_endpoint) as self._estop_keepalive: #stand the robot up if self._standup: self.power_on() self.stand() #delay grabbing image until spot is standing (or close enough to upright) time.sleep(.35) stop_spot = self.check_num_attempts() self.populate_source_dict() while not stop_spot: #grab a raw image from spot's camera bboxes, source_name = self.image_to_bounding_box() if bboxes: self._previous_source = source_name (tvec, _, source_name) = self.get_position_lib( bboxes, self._intrinsics, source_name) else: self._tag_not_located = True print("No bounding boxes found") #Go to the tag and stop within a certain distance if not self._tag_not_located: self.go_to_tag(tvec, source_name) self._attempts += 1 #increment attempts at finding bounding boxes #stop iterating through bbox detection+movement when close to tag stop_spot = self.check_num_attempts() if self._powered_on: self.power_off() def power_on(self): """Power on the robot.""" self._robot.power_on() self._powered_on = True print("Powered On " + str(self._robot.is_powered_on())) def power_off(self): """Power off the robot.""" safe_power_off_cmd = RobotCommandBuilder.safe_power_off_command() self._robot_command_client.robot_command(lease=None, command=safe_power_off_cmd) time.sleep(2.5) print("Powered Off " + str(not self._robot.is_powered_on())) def image_to_bounding_box(self): """Determine which camera source has a fiducial. Return the bounding box of the first detected fiducial.""" #Iterate through all five camera sources to check for a fiducial for i in range(len(self._source_names) + 1): #Get the image from the source camera if i == 0: if self._previous_source != None: source_name = self._previous_source else: continue elif self._source_names[i - 1] == self._previous_source: continue else: source_name = self._source_names[i - 1] img_req = build_image_request(source_name, quality_percent=100, image_format=bosdyn_image.FORMAT_RAW) image_response = self._image_client.get_image([img_req]) #transformation between camera frame to body frame self._camera_T_body = image_response[ 0].shot.sample.frame.base_tform_offset #transformation between body to world self._body_T_world = image_response[0].ko_tform_body #Camera intrinsics for the given source camera self._intrinsics = image_response[0].source.pinhole.intrinsics width = image_response[0].shot.image.cols height = image_response[0].shot.image.rows # detect given fiducial in image and return the bounding box of it bboxes = self.find_image_tag_lib(image_response[0].shot.image, (width, height), source_name) if bboxes: return bboxes, source_name else: self._tag_not_located = True print("Failed to find bounding box for " + str(source_name)) return [], None def find_image_tag_lib(self, image, dim, source_name): """Detect the fiducial within a single image and return its bounding box.""" image_grey = np.array( Image.frombytes('P', (int(dim[0]), int(dim[1])), data=image.data, decoder_name='raw')) #Rotate each image such that it is upright image_grey = self.rotate_image(image_grey, source_name) #Make the image greyscale to use bounding box detections detector = apriltag(family="tag36h11", blur=self.blur) detections = detector.detect(image_grey) bboxes = [] for i in range(len(detections)): bbox = detections[i]['lb-rb-rt-lt'] cv2.polylines(image_grey, [np.int32(bbox)], True, (0, 0, 0), 2) bboxes.append(bbox) self._image[source_name] = image_grey if self._debug and bboxes and not self._display_images: cv2.imshow('found', image_grey) cv2.waitKey() cv2.destroyAllWindows() return bboxes def bbox_to_img_obj_pts(self, bbox): """Determine the object points and image points for the bounding box. The origin in object coordinates = top left corner of the fiducial. Order both points sets following: (TL,TR, BL, BR)""" obj_pts = np.array( [[0, 0], [self._fiducial_width_mm, 0], [0, self._fiducial_height_mm], [self._fiducial_width_mm, self._fiducial_height_mm]]).reshape(bbox.shape[0], 1, 2) #insert a 1 as the third coordinate (xyz) obj_points = np.insert(obj_pts, 2, 0, axis=2).reshape(obj_pts.shape[0], 3, 1) #['lb-rb-rt-lt'] img_pts = np.array([[bbox[3][0], bbox[3][1]], [bbox[2][0], bbox[2][1]], [bbox[0][0], bbox[0][1]], [bbox[1][0], bbox[1][1]]]).reshape(bbox.shape[0], 2, 1) return obj_points, img_pts def get_position_lib(self, bbox, ints, source_name): """Compute transformation of 2d pixel coordinates to 3d camera coordinates.""" camera = self.make_cam_mat(ints) best_bbox = (None, None, source_name) closest_dist = 1000 for i in range(len(bbox)): obj_points, img_points = self.bbox_to_img_obj_pts(bbox[i]) if self._cam_to_ext_guess[source_name][0]: # initialize the position estimate with the previous extrinsics solution # then iteratively solve for new position old_rvec, old_tvec = self._cam_to_ext_guess[source_name][1] _, rvec, tvec = cv2.solvePnP(obj_points, img_points, camera, np.zeros( (5, 1)), old_rvec, old_tvec, True, cv2.SOLVEPNP_ITERATIVE) else: # Determine current extrinsic solution for the tag _, rvec, tvec = cv2.solvePnP(obj_points, img_points, camera, np.zeros((5, 1))) #Save extrinsics results to help speed up next attempts to locate bounding box self._cam_to_ext_guess[source_name] = (True, (rvec, tvec)) if self._debug: self.back_prop_error(obj_points, img_points, rvec, tvec, camera) cv2.drawFrameAxes(self._image[source_name], camera, np.zeros((5, 1)), rvec, tvec, 100) if not self._display_images: cv2.imshow('frame', self._image[source_name]) cv2.waitKey() cv2.destroyAllWindows() dist = (float(tvec[0][0])**2 + float(tvec[1][0])**2 + float(tvec[2][0])**2)**(.5) / 1000.0 if dist < closest_dist: closest_dist = dist best_bbox = (tvec, rvec, source_name) # Flag indicating if the best april tag been found/located self._tag_not_located = best_bbox[0] is None and best_bbox[1] is None return best_bbox def offset_tag_pose(self, tag_position): """Offset the location of the fiducial to keep a buffer of how close the robot gets. Converted in body frame so that world location is accurate.""" if tag_position[0] < 0: offset_x = tag_position[0] + self._dist_x_desired else: offset_x = tag_position[0] - self._dist_x_desired if tag_position[1] < 0: offset_y = tag_position[1] + self._dist_y_desired else: offset_y = tag_position[1] - self._dist_y_desired return np.array([offset_x, offset_y, tag_position[2]]) def go_to_tag(self, tvec, source_name): """Transform the fiducial position to the world frame (kinematic odometry frame) Command the robot to move to this position.""" #Transform the tag position from camera coordinates to world coordinates tag_pose_in_camera = np.array([ float(tvec[0][0]) / 1000.0, float(tvec[1][0]) / 1000.0, float(tvec[2][0]) / 1000.0 ]) tag_pose_in_body = self.transform_to_frame(self._camera_T_body, tag_pose_in_camera) tag_pose_body_offset = self.offset_tag_pose(tag_pose_in_body) self._current_tag_world_pose = self.transform_to_frame( self._body_T_world, tag_pose_body_offset) #Get the robot's current position in the world robot_state = self.robot_state.kinematic_state.ko_tform_body robot_angle = self.quat_to_euler( (robot_state.rotation.x, robot_state.rotation.y, robot_state.rotation.z, robot_state.rotation.w))[2] #Compute the heading angle to turn the robot to face the tag self._angle_desired = self.get_desired_angle(robot_angle, robot_state.position) if self._debug: print("Camera: " + str(source_name)) print("Tag pose in camera", tag_pose_in_camera) print("Tag pose in body", tag_pose_in_body) print("Tag pose in body offsetted", tag_pose_body_offset) print("Tag pose in ko", self._current_tag_world_pose) print("Robot Pose in ko", robot_state.position) print("Robot heading Angle", robot_angle) print("Desired heading angle", self._angle_desired) #Command the robot to go to the tag in kinematic odometry frame frame_name = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_KO) mobility_params = self.set_mobility_params() tag_cmd = RobotCommandBuilder.trajectory_command( goal_x=self._current_tag_world_pose[0], goal_y=self._current_tag_world_pose[1], goal_heading=self._angle_desired, frame=frame_name, params=mobility_params, body_height=0.0, locomotion_hint=spot_command_pb2.HINT_AUTO) end_time = 5.0 if self._movement_on: #Issue the command to the robot self._robot_command_client.robot_command( lease=None, command=tag_cmd, end_time_secs=time.time() + end_time) # #Feedback to check and wait until the robot is in the desired position or timeout start_time = time.time() current_time = time.time() while (not self.final_state() and current_time - start_time < end_time): time.sleep(.25) current_time = time.time() return def final_state(self): """Check if the current robot state is within range of the fiducial position.""" robot_state = self.robot_state.kinematic_state.ko_tform_body robot_pose = robot_state.position robot_angle = self.quat_to_euler( (robot_state.rotation.x, robot_state.rotation.y, robot_state.rotation.z, robot_state.rotation.w))[2] if self._current_tag_world_pose.size != 0: x_dist = abs(self._current_tag_world_pose[0] - robot_pose.x) y_dist = abs(self._current_tag_world_pose[1] - robot_pose.y) angle = abs(self._angle_desired - robot_angle) if ((x_dist < self._x_eps) and (y_dist < self._y_eps) and (angle < self._angle_eps)): return True return False def check_num_attempts(self): """Check if the number of attempts is within the bounds.""" return self._attempts >= self._max_attempts def stand(self): """Stand the robot.""" blocking_stand(self._robot_command_client) def get_desired_angle(self, robot_angle, robot_state): """Compute heading using the offset angle of a vector to the apriltag""" vec_to_tag = np.array([ self._current_tag_world_pose[0] - robot_state.x, self._current_tag_world_pose[1] - robot_state.y ]) vec_of_robot = np.array([math.cos(robot_angle), math.sin(robot_angle)]) rotate_ang = self.angle_bn_vectors(vec_to_tag, vec_of_robot) side_sign = np.cross(vec_to_tag, vec_of_robot) if side_sign > 0: #angle is to the right, so make it negative when transforming the current robot angle rotate = -1.0 * rotate_ang else: rotate = rotate_ang desired_angle = self.wrap_angle_back(robot_angle + rotate) return desired_angle def transform_to_frame(self, frame, tag_position): """ Transform the tag position into the inputted coordinate frame""" rot_mat = self.quat_to_rotmat((frame.rotation.x, frame.rotation.y, frame.rotation.z, frame.rotation.w)) frame_pose = np.array( [frame.position.x, frame.position.y, frame.position.z]) tf_tag_pose = frame_pose + np.matmul(rot_mat, tag_position) return tf_tag_pose def set_mobility_params(self): """Set robot mobility params to disable obstacle avoidance.""" obstacles = spot_command_pb2.ObstacleParams( disable_vision_body_obstacle_avoidance=True, disable_vision_foot_obstacle_avoidance=True, disable_vision_foot_constraint_avoidance=True, obstacle_avoidance_padding=.001) body_control = self.set_default_body_control() if self._limit_speed: speed_limit = SE2VelocityLimit(max_vel=SE2Velocity( linear=Vec2(x=self._max_x_vel, y=self._max_y_vel), angular=self._max_ang_vel)) if not self._avoid_obstacles: mobility_params = spot_command_pb2.MobilityParams( obstacle_params=obstacles, vel_limit=speed_limit, body_control=body_control, locomotion_hint=spot_command_pb2.HINT_AUTO) else: mobility_params = spot_command_pb2.MobilityParams( vel_limit=speed_limit, body_control=body_control, locomotion_hint=spot_command_pb2.HINT_AUTO) elif not self._avoid_obstacles: mobility_params = spot_command_pb2.MobilityParams( obstacle_params=obstacles, body_control=body_control, locomotion_hint=spot_command_pb2.HINT_AUTO) else: #When set to none, RobotCommandBuilder populates with good default values mobility_params = None return mobility_params @staticmethod def set_default_body_control(): """Set default body control params to current body position""" footprint_R_body = geometry.EulerZXY() position = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) rotation = footprint_R_body.to_quaternion() pose = geometry_pb2.SE3Pose(position=position, rotation=rotation) point = trajectory_pb2.SE3TrajectoryPoint(pose=pose) frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY) traj = trajectory_pb2.SE3Trajectory(points=[point], frame=frame) return spot_command_pb2.BodyControlParams( base_offset_rt_footprint=traj) @staticmethod def rotate_image(image, source_name): """Rotate the image so that it is always displayed upright.""" if source_name == "frontleft_fisheye_image": image = cv2.rotate(image, rotateCode=0) elif source_name == "right_fisheye_image": image = cv2.rotate(image, rotateCode=1) elif source_name == "frontright_fisheye_image": image = cv2.rotate(image, rotateCode=0) return image @staticmethod def back_prop_error(obj_points, img_points, rvec, tvec, ints): """Determine the error [in pixel space] of the points projected back to image frame using the new transformation.""" projected = cv2.projectPoints(obj_points, rvec, tvec, ints, np.zeros((5, 1)))[0] err = [] for i in range(len(obj_points)): dx = img_points[i][0][0] - projected[i][0][0] dy = img_points[i][1][0] - projected[i][0][1] error = (dx**2 + dy**2)**(.5) err.append(error) avg_error = np.sum(np.array(err)) / len(err) print("Average Error in pixel space", avg_error) return avg_error @staticmethod def wrap_angle_back(desired_angle): """Wrap an angle value (radians) around at +/- pi to match the robot's frame.""" if desired_angle > math.pi: desired_angle = -math.pi + desired_angle % math.pi elif desired_angle < -math.pi: desired_angle = desired_angle % math.pi return desired_angle @staticmethod def angle_bn_vectors(vec1, vec2): """Compute the angle between two vectors.""" vec1 /= np.linalg.norm(vec1) vec2 /= np.linalg.norm(vec2) dot_pdt = vec1[0] * vec2[0] + vec1[1] * vec2[1] return math.acos(dot_pdt) @staticmethod def make_cam_mat(ints): """Transform the ImageResponse proto intrinsics into a camera matrix.""" camera_matrix = np.array( [[ints.focal_length.x, ints.skew.x, ints.principal_point.x], [ints.skew.y, ints.focal_length.y, ints.principal_point.y], [0, 0, 1]]) return camera_matrix @staticmethod def quat_to_rotmat(q): """Convert a quaternion into a rotation matrix.""" rm00 = 1 - 2 * q[1]**2 - 2 * q[2]**2 rm01 = 2 * q[0] * q[1] - 2 * q[2] * q[3] rm02 = 2 * q[0] * q[2] + 2 * q[1] * q[3] rm10 = 2 * q[0] * q[1] + 2 * q[2] * q[3] rm11 = 1 - 2 * q[0]**2 - 2 * q[2]**2 rm12 = 2 * q[1] * q[2] - 2 * q[0] * q[3] rm20 = 2 * q[0] * q[2] - 2 * q[1] * q[3] rm21 = 2 * q[1] * q[2] + 2 * q[0] * q[3] rm22 = 1 - 2 * q[0]**2 - 2 * q[1]**2 return np.array([[rm00, rm01, rm02], [rm10, rm11, rm12], [rm20, rm21, rm22]]) @staticmethod def rotmat_to_quat(mat): """Convert a rotation matrix into a quaternion.""" w = math.sqrt(1 + mat[0][0] + mat[1][1] + mat[2][2]) / 2.0 x = (mat[2][1] - mat[1][2]) / (4.0 * w) y = (mat[0][2] - mat[2][0]) / (4.0 * w) z = (mat[1][0] - mat[0][1]) / (4.0 * w) return (x, y, z, w) @staticmethod def quat_to_euler(q): """Convert a quaternion to xyz Euler angles.""" roll = math.atan2(2 * q[3] * q[0] + q[1] * q[2], 1 - 2 * q[0]**2 + 2 * q[1]**2) pitch = math.atan2(2 * q[1] * q[3] - 2 * q[0] * q[2], 1 - 2 * q[1]**2 - 2 * q[2]**2) yaw = math.atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], 1 - 2 * q[1]**2 - 2 * q[2]**2) return roll, pitch, yaw def stop(self): """Clean shutdown for the Fiducial Follower.""" #Power off(safely) the motors if they were turned on if self._powered_on: self.power_off() #Stop the estop keepalive and the lease keepalive if self._estop_keepalive is not None: self._estop_keepalive.stop() self._estop_keepalive.shutdown() if self._lease_keepalive is not None: if self._lease_keepalive.is_alive(): self._lease_keepalive.shutdown() if self._lease: try: self._lease_client.return_lease(self._lease) except (ResponseError, RpcError) as err: LOGGER.error("Failed %s: %s", "Return lease", err) #Stop time sync self._robot.time_sync.stop()