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 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 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 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