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)
class EstopNoGui(): """Provides a software estop without a GUI. To use this estop, create an instance of the EstopNoGui class and use the stop() and allow() functions programmatically. """ 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 __enter__(self): pass def __exit__(self, exc_type, exc_val, exc_tb): """Cleanly shut down estop on exit.""" self.estop_keep_alive.end_periodic_check_in() def stop(self): self.estop_keep_alive.stop() def allow(self): self.estop_keep_alive.allow() def settle_then_cut(self): self.estop_keep_alive.settle_then_cut()
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 __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 enable_movement(self): if self.__lease is None: logging.info("Acquiring lease") self.__lease = self.__lease_client.take() self.__lease_keep_alive = LeaseKeepAlive(self.__lease_client) if self.__estop_endpoint is None: logging.info("Creating estop endpoint") self.__estop_endpoint = estop.EstopEndpoint( client=self.__estop_client, name='mt-node-payload', estop_timeout=9) self.__estop_endpoint.force_simple_setup() self.__estop_keepalive = EstopKeepAlive(self.__estop_endpoint) try: logging.info("Powering motors") power.power_on(self.__power_client) except BatteryMissingError: logging.error("Battery missing")
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 EstopGui(QtWidgets.QMainWindow): """The GUI for the estop Button. Provides software estop.""" disable_signal = QtCore.pyqtSignal() checkin_status_signal = QtCore.pyqtSignal('QString') got_status_signal = QtCore.pyqtSignal('QString') 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 do_status_rpc(self): """Make an rpc call to get the robot estop status.""" try: status = self.estop_keep_alive.client.get_status() #pylint: disable=broad-except except Exception as exc: markup = 'Exception while getting status!' traceback.print_exc() else: markup = status_response_to_markup( status, my_id=self.estop_keep_alive.endpoint.unique_id) self.got_status_signal.emit(markup) def status(self): """Asynchronously request and print the endpoint status.""" if self.status_extant: self.logger.info('Ignoring duplicate request for status') return self.status_extant = True self.logger.info('Getting estop system status') thread = threading.Thread(target=self.do_status_rpc) thread.start() def _check_keep_alive_status(self): """Monitor estop keep alive status and display status in GUI via Qt signals.""" while not self.quitting: # Wait for queue to be populated. After timeout, check if GUI is still running. try: status, msg = self.estop_keep_alive.status_queue.get( timeout=1) # blocking except queue.Empty: continue if status == EstopKeepAlive.KeepAliveStatus.OK: self.checkin_status_signal.emit('OK! {:%H:%M:%S}'.format( datetime.now())) elif status == EstopKeepAlive.KeepAliveStatus.ERROR: self.checkin_status_signal.emit(msg) elif status == EstopKeepAlive.KeepAliveStatus.DISABLED: self.disable_signal.emit() else: raise Exception( "Unknown estop keep alive status seen: {}.".format(status)) def disable_buttons(self): """Disable the estop buttons.""" self.stop_button.setEnabled(False) self.release_button.setEnabled(False) self.stop_button.setText('(disabled)') self.release_button.setText('(disabled)') def set_status_label(self, status_msg): self.status_label.setText(status_msg) def _launch_estop_status_dialog(self, markup): self.status_extant = False d = QtWidgets.QMessageBox() d.setWindowTitle('SW Estop Status') d.setText(markup) d.exec_() def quit(self): """Shutdown estop keep-alive and all GUI threads.""" self.estop_keep_alive.shutdown() self.quitting = True
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
class Spot: app_name = "twitch_client" __robot = None __sdk = None __lease_client = None __power_client = None __estop_client = None __state_client = None __command_client = None __estop_endpoint = None __estop_keepalive = None __lease = None __lease_keep_alive = None movement_helper = None image_helper = None def __init__(self, config): logging.info( f"Creating new robot instance for {config.name} {config.host}") self.hostname = config.host self.client_name = config.name self.guid = config.guid self.secret = config.secret def connect(self, cb=None, retry=False): self.__sdk = create_standard_sdk(self.app_name) try: self.__robot = self.__sdk.create_robot(self.hostname) logging.info(f"Authenticating payload with guid {self.guid}") self.__robot.authenticate_from_payload_credentials( guid=self.guid, secret=self.secret) logging.info("Starting time sync") self.__robot.start_time_sync() self.__preflight() if cb is not None: cb() except RpcError: logging.error( f"Could not connect with robot using {self.hostname}") if retry: logging.info(f"Retrying using {self.hostname}") self.connect(cb, retry) except InvalidPayloadCredentialsError: logging.error(f"Invalid guid '{self.guid}' or secret") except Exception as exc: logging.error(exc) def __preflight(self): logging.info("Ensuring clients") self.__lease_client = self.__robot.ensure_client( lease.LeaseClient.default_service_name) self.__power_client = self.__robot.ensure_client( power.PowerClient.default_service_name) self.__state_client = self.__robot.ensure_client( robot_state.RobotStateClient.default_service_name) self.__estop_client = self.__robot.ensure_client( estop.EstopClient.default_service_name) self.__command_client = self.__robot.ensure_client( robot_command.RobotCommandClient.default_service_name) self.__image_client = self.__robot.ensure_client( image.ImageClient.default_service_name) logging.info("Initializing movement helper") self.movement_helper = MovementHelper(self.__command_client) logging.info("Initializing image helper") self.image_helper = ImageViewer(self.__image_client) def get_battery_level(self): return self.__state_client.get_robot_state( ).battery_states[0].charge_percentage.value def enable_movement(self): if self.__lease is None: logging.info("Acquiring lease") self.__lease = self.__lease_client.take() self.__lease_keep_alive = LeaseKeepAlive(self.__lease_client) if self.__estop_endpoint is None: logging.info("Creating estop endpoint") self.__estop_endpoint = estop.EstopEndpoint( client=self.__estop_client, name='mt-node-payload', estop_timeout=9) self.__estop_endpoint.force_simple_setup() self.__estop_keepalive = EstopKeepAlive(self.__estop_endpoint) try: logging.info("Powering motors") power.power_on(self.__power_client) except BatteryMissingError: logging.error("Battery missing") def disable_movement(self): logging.info("Depowering motors") power.power_off(self.__power_client) if self.__lease is not None: logging.info("Releasing lease") self.__lease_keep_alive.shutdown() self.__lease_client.return_lease(self.__lease) self.__lease = None if self.__estop_endpoint is not None: logging.info("Releasing estop") self.__estop_keepalive.stop() self.__estop_keepalive = None self.__estop_endpoint.stop() self.__estop_endpoint = None
def resetEStop(self): """Get keepalive for eStop""" self._estop_endpoint.force_simple_setup( ) # Set this endpoint as the robot's sole estop. self._estop_keepalive = EstopKeepAlive(self._estop_endpoint)
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