def __init__(self, client, timeout_sec, name=None, unique_id=None):
QtWidgets.QMainWindow.__init__(self)
self.logger = logging.getLogger("Estop GUI")
self.disable_signal.connect(self.disable_buttons)
self.checkin_status_signal.connect(self.set_status_label)
self.got_status_signal.connect(self._launch_estop_status_dialog)
self.status_extant = False
self.quitting = False # Used to tell threads to shutdown
# Force server to set up a single endpoint system
ep = EstopEndpoint(client, name, timeout_sec)
ep.force_simple_setup()
# Begin periodic check-in between keep-alive and robot
self.estop_keep_alive = EstopKeepAlive(ep)
# Configure UI.
self.setCentralWidget(QtWidgets.QWidget())
self.center_layout = QtWidgets.QVBoxLayout(self.centralWidget())
self.center_layout.setAlignment(QtCore.Qt.AlignTop)
self.center_layout.setSpacing(1)
self.center_layout.setContentsMargins(1, 1, 1, 1)
self.stop_button = QtWidgets.QPushButton(self)
self.stop_button.setText('STOP')
self.stop_button.clicked.connect(self.estop_keep_alive.stop)
self.stop_button.setStyleSheet(STOP_BUTTON_STYLESHEET)
self.stop_button.setSizePolicy(QtWidgets.QSizePolicy.Expanding,
QtWidgets.QSizePolicy.Expanding)
self.center_layout.addWidget(self.stop_button)
self.status_label = QtWidgets.QLabel('Starting...')
self.status_label.setAlignment(QtCore.Qt.AlignCenter)
self.status_label.setStyleSheet(ERROR_LABEL_STYLESHEET)
self.center_layout.addWidget(self.status_label)
self.release_button = QtWidgets.QPushButton(self)
self.release_button.setText('Release')
self.release_button.clicked.connect(self.estop_keep_alive.allow)
self.release_button.setStyleSheet(RELEASE_BUTTON_STYLESHEET)
self.center_layout.addWidget(self.release_button)
self.status_button = QtWidgets.QPushButton(self)
self.status_button.setText('Status')
self.status_button.clicked.connect(self.status)
self.center_layout.addWidget(self.status_button)
self.setWindowTitle("E-Stop ({} {}sec)".format("TEST ADDRESS",
timeout_sec))
# Begin monitoring the keep-alive status
thread = threading.Thread(target=self._check_keep_alive_status)
thread.start()
def __init__(self, client, timeout_sec, name=None):
# Force server to set up a single endpoint system
ep = EstopEndpoint(client, name, timeout_sec)
ep.force_simple_setup()
# Begin periodic check-in between keep-alive and robot
self.estop_keep_alive = EstopKeepAlive(ep)
# Release the estop
self.estop_keep_alive.allow()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--timeout',
type=float,
help="Timeout of the estop (seconds)",
default=1)
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args()
if options.verbose:
log_level = logging.DEBUG
else:
log_level = logging.INFO
logging.basicConfig(
level=log_level,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')
sdk = bosdyn.client.create_standard_sdk('EstopClientWithSdk')
sdk.load_app_token(options.app_token)
robot = sdk.create_robot(options.hostname)
robot.authenticate(options.username, options.password)
estop_client = robot.ensure_client(EstopClient.default_service_name)
estop_endpoint = EstopEndpoint(
client=estop_client,
name=bosdyn.client.sdk.generate_client_name('estop-tutorial:'),
estop_timeout=options.timeout)
estop_endpoint.force_simple_setup()
call_and_check(estop_client,
estop_endpoint,
StopLevel.ESTOP_LEVEL_NONE,
sync=True)
call_and_check(estop_client,
estop_endpoint,
StopLevel.ESTOP_LEVEL_CUT,
sync=True)
call_and_check(estop_client,
estop_endpoint,
StopLevel.ESTOP_LEVEL_NONE,
sync=True)
call_and_check(estop_client,
estop_endpoint,
StopLevel.ESTOP_LEVEL_CUT,
sync=True)
call_and_check(estop_client,
estop_endpoint,
StopLevel.ESTOP_LEVEL_NONE,
sync=False)
call_and_check(estop_client,
estop_endpoint,
StopLevel.ESTOP_LEVEL_CUT,
sync=False)
call_and_check(estop_client,
estop_endpoint,
StopLevel.ESTOP_LEVEL_NONE,
sync=False)
call_and_check(estop_client,
estop_endpoint,
StopLevel.ESTOP_LEVEL_CUT,
sync=False)
current_config = estop_client.get_config()
estop_client.deregister(current_config.unique_id, estop_endpoint)
class RecorderInterface(object):
"""A curses interface for recording robot missions."""
def __init__(self, robot, download_filepath):
self._robot = robot
# Flag indicating whether mission is currently being recorded
self._recording = False
# Filepath for the location to put the downloaded graph and snapshots.
self._download_filepath = download_filepath
# List of visited waypoints in current mission
self._waypoint_path = []
# Current waypoint id
self._waypoint_id = 'NONE'
# Create clients -- do not use the for communication yet.
self._lease_client = robot.ensure_client(
LeaseClient.default_service_name)
try:
self._estop_client = self._robot.ensure_client(
EstopClient.default_service_name)
self._estop_endpoint = EstopEndpoint(self._estop_client,
'GNClient', 9.0)
except:
# Not the estop.
self._estop_client = None
self._estop_endpoint = None
self._power_client = robot.ensure_client(
PowerClient.default_service_name)
self._robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
self._world_object_client = robot.ensure_client(
WorldObjectClient.default_service_name)
# Setup the recording service client.
self._recording_client = self._robot.ensure_client(
GraphNavRecordingServiceClient.default_service_name)
# Setup the graph nav service client.
self._graph_nav_client = robot.ensure_client(
GraphNavClient.default_service_name)
self._robot_state_task = AsyncRobotState(self._robot_state_client)
self._async_tasks = AsyncTasks([self._robot_state_task])
self._lock = threading.Lock()
self._command_dictionary = {
ord('\t'): self._quit_program,
ord('T'): self._toggle_time_sync,
ord(' '): self._toggle_estop,
ord('r'): self._self_right,
ord('P'): self._toggle_power,
ord('v'): self._sit,
ord('f'): self._stand,
ord('w'): self._move_forward,
ord('s'): self._move_backward,
ord('a'): self._strafe_left,
ord('d'): self._strafe_right,
ord('q'): self._turn_left,
ord('e'): self._turn_right,
ord('m'): self._start_recording,
ord('l'): self._relocalize,
ord('g'): self._generate_mission,
}
self._locked_messages = ['', '',
''] # string: displayed message for user
self._estop_keepalive = None
self._exit_check = None
# Stuff that is set in start()
self._robot_id = None
self._lease = None
def start(self):
"""Begin communication with the robot."""
self._lease = self._lease_client.acquire()
self._robot_id = self._robot.get_id()
if self._estop_endpoint is not None:
self._estop_endpoint.force_simple_setup(
) # Set this endpoint as the robot's sole estop.
# Clear existing graph nav map
self._graph_nav_client.clear_graph()
def shutdown(self):
"""Release control of robot as gracefully as posssible."""
LOGGER.info("Shutting down WasdInterface.")
if self._estop_keepalive:
# This stops the check-in thread but does not stop the robot.
self._estop_keepalive.shutdown()
if self._lease:
_grpc_or_log("returning lease",
lambda: self._lease_client.return_lease(self._lease))
self._lease = None
def __del__(self):
self.shutdown()
def flush_and_estop_buffer(self, stdscr):
"""Manually flush the curses input buffer but trigger any estop requests (space)"""
key = ''
while key != -1:
key = stdscr.getch()
if key == ord(' '):
self._toggle_estop()
def add_message(self, msg_text):
"""Display the given message string to the user in the curses interface."""
with self._lock:
self._locked_messages = [msg_text] + self._locked_messages[:-1]
def message(self, idx):
"""Grab one of the 3 last messages added."""
with self._lock:
return self._locked_messages[idx]
@property
def robot_state(self):
"""Get latest robot state proto."""
return self._robot_state_task.proto
def drive(self, stdscr):
"""User interface to control the robot via the passed-in curses screen interface object."""
with LeaseKeepAlive(self._lease_client) as lease_keep_alive, \
ExitCheck() as self._exit_check:
curses_handler = CursesHandler(self)
curses_handler.setLevel(logging.INFO)
LOGGER.addHandler(curses_handler)
stdscr.nodelay(True) # Don't block for user input.
stdscr.resize(26, 96)
stdscr.refresh()
# for debug
curses.echo()
try:
while not self._exit_check.kill_now:
self._async_tasks.update()
self._drive_draw(stdscr, lease_keep_alive)
try:
cmd = stdscr.getch()
# Do not queue up commands on client
self.flush_and_estop_buffer(stdscr)
self._drive_cmd(cmd)
time.sleep(COMMAND_INPUT_RATE)
except Exception:
# On robot command fault, sit down safely before killing the program.
self._safe_power_off()
time.sleep(2.0)
self.shutdown()
raise
finally:
LOGGER.removeHandler(curses_handler)
def _count_visible_fiducials(self):
"""Return number of fiducials visible to robot."""
request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG]
fiducial_objects = self._world_object_client.list_world_objects(
object_type=request_fiducials).world_objects
return len(fiducial_objects)
def _drive_draw(self, stdscr, lease_keep_alive):
"""Draw the interface screen at each update."""
stdscr.clear() # clear screen
stdscr.resize(26, 96)
stdscr.addstr(
0, 0, '{:20s} {}'.format(self._robot_id.nickname,
self._robot_id.serial_number))
stdscr.addstr(1, 0, self._lease_str(lease_keep_alive))
stdscr.addstr(2, 0, self._battery_str())
stdscr.addstr(3, 0, self._estop_str())
stdscr.addstr(4, 0, self._power_state_str())
stdscr.addstr(5, 0, self._time_sync_str())
stdscr.addstr(6, 0, self._waypoint_str())
stdscr.addstr(7, 0, self._fiducial_str())
for i in range(3):
stdscr.addstr(9 + i, 2, self.message(i))
stdscr.addstr(13, 0,
"Commands: [TAB]: quit ")
stdscr.addstr(14, 0,
" [T]: Time-sync, [SPACE]: Estop, [P]: Power")
stdscr.addstr(15, 0,
" [v]: Sit, [f]: Stand, [r]: Self-right ")
stdscr.addstr(16, 0,
" [wasd]: Directional strafing ")
stdscr.addstr(17, 0,
" [qe]: Turning ")
stdscr.addstr(18, 0,
" [m]: Start recording mission ")
stdscr.addstr(19, 0,
" [l]: Add fiducial localization to mission ")
stdscr.addstr(20, 0,
" [g]: Stop recording and generate mission ")
stdscr.refresh()
def _drive_cmd(self, key):
"""Run user commands at each update."""
try:
cmd_function = self._command_dictionary[key]
cmd_function()
except KeyError:
if key and key != -1 and key < 256:
self.add_message("Unrecognized keyboard command: '{}'".format(
chr(key)))
def _try_grpc(self, desc, thunk):
try:
return thunk()
except (ResponseError, RpcError) as err:
self.add_message("Failed {}: {}".format(desc, err))
return None
def _quit_program(self):
self._robot.power_off()
self.shutdown()
if self._exit_check is not None:
self._exit_check.request_exit()
def _toggle_time_sync(self):
if self._robot.time_sync.stopped:
self._robot.time_sync.start()
else:
self._robot.time_sync.stop()
def _toggle_estop(self):
"""toggle estop on/off. Initial state is ON"""
if self._estop_client is not None and self._estop_endpoint is not None:
if not self._estop_keepalive:
self._estop_keepalive = EstopKeepAlive(self._estop_endpoint)
else:
self._try_grpc("stopping estop", self._estop_keepalive.stop)
self._estop_keepalive.shutdown()
self._estop_keepalive = None
def _start_robot_command(self, desc, command_proto, end_time_secs=None):
def _start_command():
self._robot_command_client.robot_command(
lease=None, command=command_proto, end_time_secs=end_time_secs)
self._try_grpc(desc, _start_command)
def _self_right(self):
self._start_robot_command('self_right',
RobotCommandBuilder.selfright_command())
def _sit(self):
self._start_robot_command('sit', RobotCommandBuilder.sit_command())
def _stand(self):
self._start_robot_command('stand', RobotCommandBuilder.stand_command())
def _move_forward(self):
self._velocity_cmd_helper('move_forward', v_x=VELOCITY_BASE_SPEED)
def _move_backward(self):
self._velocity_cmd_helper('move_backward', v_x=-VELOCITY_BASE_SPEED)
def _strafe_left(self):
self._velocity_cmd_helper('strafe_left', v_y=VELOCITY_BASE_SPEED)
def _strafe_right(self):
self._velocity_cmd_helper('strafe_right', v_y=-VELOCITY_BASE_SPEED)
def _turn_left(self):
self._velocity_cmd_helper('turn_left', v_rot=VELOCITY_BASE_ANGULAR)
def _turn_right(self):
self._velocity_cmd_helper('turn_right', v_rot=-VELOCITY_BASE_ANGULAR)
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0):
self._start_robot_command(
desc,
RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y,
v_rot=v_rot),
end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def _return_to_origin(self):
self._start_robot_command(
'fwd_and_rotate',
RobotCommandBuilder.trajectory_command(
goal_x=0.0,
goal_y=0.0,
goal_heading=0.0,
frame_name=ODOM_FRAME_NAME,
params=None,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT),
end_time_secs=time.time() + 20)
def _toggle_power(self):
power_state = self._power_state()
if power_state is None:
self.add_message(
'Could not toggle power because power state is unknown')
return
if power_state == robot_state_proto.PowerState.STATE_OFF:
self._try_grpc("powering-on", self._request_power_on)
else:
self._try_grpc("powering-off", self._safe_power_off)
def _request_power_on(self):
bosdyn.client.power.power_on(self._power_client)
def _safe_power_off(self):
self._start_robot_command('safe_power_off',
RobotCommandBuilder.safe_power_off_command())
def _power_state(self):
state = self.robot_state
if not state:
return None
return state.power_state.motor_power_state
def _lease_str(self, lease_keep_alive):
alive = 'RUNNING' if lease_keep_alive.is_alive() else 'STOPPED'
lease = '{}:{}'.format(self._lease.lease_proto.resource,
self._lease.lease_proto.sequence)
return 'Lease {} THREAD:{}'.format(lease, alive)
def _power_state_str(self):
power_state = self._power_state()
if power_state is None:
return ''
state_str = robot_state_proto.PowerState.MotorPowerState.Name(
power_state)
return 'Power: {}'.format(state_str[6:]) # get rid of STATE_ prefix
def _estop_str(self):
if not self._estop_client:
thread_status = 'NOT ESTOP'
else:
thread_status = 'RUNNING' if self._estop_keepalive else 'STOPPED'
estop_status = '??'
state = self.robot_state
if state:
for estop_state in state.estop_states:
if estop_state.type == estop_state.TYPE_SOFTWARE:
estop_status = estop_state.State.Name(
estop_state.state)[6:] # s/STATE_//
break
return 'Estop {} (thread: {})'.format(estop_status, thread_status)
def _time_sync_str(self):
if not self._robot.time_sync:
return 'Time sync: (none)'
if self._robot.time_sync.stopped:
status = 'STOPPED'
exception = self._robot.time_sync.thread_exception
if exception:
status = '{} Exception: {}'.format(status, exception)
else:
status = 'RUNNING'
try:
skew = self._robot.time_sync.get_robot_clock_skew()
if skew:
skew_str = 'offset={}'.format(duration_str(skew))
else:
skew_str = "(Skew undetermined)"
except (TimeSyncError, RpcError) as err:
skew_str = '({})'.format(err)
return 'Time sync: {} {}'.format(status, skew_str)
def _waypoint_str(self):
state = self._graph_nav_client.get_localization_state()
try:
self._waypoint_id = state.localization.waypoint_id
if self._waypoint_id == '':
self._waypoint_id = 'NONE'
except:
self._waypoint_id = 'ERROR'
if self._recording and self._waypoint_id != 'NONE' and self._waypoint_id != 'ERROR':
if (self._waypoint_path
== []) or (self._waypoint_id != self._waypoint_path[-1]):
self._waypoint_path += [self._waypoint_id]
return 'Current waypoint: {} [ RECORDING ]'.format(
self._waypoint_id)
return 'Current waypoint: {}'.format(self._waypoint_id)
def _fiducial_str(self):
return 'Visible fiducials: {}'.format(
str(self._count_visible_fiducials()))
def _battery_str(self):
if not self.robot_state:
return ''
battery_state = self.robot_state.battery_states[0]
status = battery_state.Status.Name(battery_state.status)
status = status[7:] # get rid of STATUS_ prefix
if battery_state.charge_percentage.value:
bar_len = int(battery_state.charge_percentage.value) // 10
bat_bar = '|{}{}|'.format('=' * bar_len, ' ' * (10 - bar_len))
else:
bat_bar = ''
time_left = ''
if battery_state.estimated_runtime:
time_left = ' ({})'.format(
secs_to_hms(battery_state.estimated_runtime.seconds))
return 'Battery: {}{}{}'.format(status, bat_bar, time_left)
def _fiducial_visible(self):
"""Return True if robot can see fiducial."""
request_fiducials = [world_object_pb2.WORLD_OBJECT_APRILTAG]
fiducial_objects = self._world_object_client.list_world_objects(
object_type=request_fiducials).world_objects
self.add_message("Fiducial objects: " + str(fiducial_objects))
if len(fiducial_objects) > 0:
return True
return False
def _start_recording(self):
"""Start recording a map."""
if self._count_visible_fiducials() == 0:
self.add_message(
"ERROR: Can't start recording -- No fiducials in view.")
return
self._waypoint_path = []
status = self._recording_client.start_recording()
if status != recording_pb2.StartRecordingResponse.STATUS_OK:
self.add_message("Start recording failed.")
else:
self.add_message("Successfully started recording a map.")
self._recording = True
def _stop_recording(self):
"""Stop or pause recording a map."""
if not self._recording:
return True
self._recording = False
try:
status = self._recording_client.stop_recording()
if status != recording_pb2.StopRecordingResponse.STATUS_OK:
self.add_message("Stop recording failed.")
return False
self.add_message("Successfully stopped recording a map.")
return True
except NotLocalizedToEndError:
self.add_message(
"ERROR: Move to final waypoint before stopping recording.")
return False
def _relocalize(self):
"""Insert localization node into mission."""
if not self._recording:
print('Not recording mission.')
return False
self.add_message("Adding fiducial localization to mission.")
self._waypoint_path += ['LOCALIZE']
return True
def _generate_mission(self):
"""Save graph map and mission file."""
# Check whether mission has been recorded
if not self._recording:
self.add_message("ERROR: No mission recorded.")
return
# Stop recording mission
if not self._stop_recording():
self.add_message("ERROR: Error while stopping recording.")
return
# Save graph map
os.mkdir(self._download_filepath)
self._download_full_graph()
# Generate mission
mission = self._make_mission()
# Save mission file
os.mkdir(self._download_filepath + "/missions")
mission_filepath = self._download_filepath + "/missions/autogenerated"
write_mission(mission, mission_filepath)
# Quit program
self._quit_program()
def _download_full_graph(self):
"""Download the graph and snapshots from the robot."""
graph = self._graph_nav_client.download_graph()
if graph is None:
self.add_message("Failed to download the graph.")
return
self._write_full_graph(graph)
self.add_message(
"Graph downloaded with {} waypoints and {} edges".format(
len(graph.waypoints), len(graph.edges)))
# Download the waypoint and edge snapshots.
self._download_and_write_waypoint_snapshots(graph.waypoints)
self._download_and_write_edge_snapshots(graph.edges)
def _write_full_graph(self, graph):
"""Download the graph from robot to the specified, local filepath location."""
graph_bytes = graph.SerializeToString()
write_bytes(self._download_filepath, '/graph', graph_bytes)
def _download_and_write_waypoint_snapshots(self, waypoints):
"""Download the waypoint snapshots from robot to the specified, local filepath location."""
num_waypoint_snapshots_downloaded = 0
for waypoint in waypoints:
try:
waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot(
waypoint.snapshot_id)
except Exception:
# Failure in downloading waypoint snapshot. Continue to next snapshot.
self.add_message("Failed to download waypoint snapshot: " +
waypoint.snapshot_id)
continue
write_bytes(self._download_filepath + '/waypoint_snapshots',
'/' + waypoint.snapshot_id,
waypoint_snapshot.SerializeToString())
num_waypoint_snapshots_downloaded += 1
self.add_message(
"Downloaded {} of the total {} waypoint snapshots.".format(
num_waypoint_snapshots_downloaded, len(waypoints)))
def _download_and_write_edge_snapshots(self, edges):
"""Download the edge snapshots from robot to the specified, local filepath location."""
num_edge_snapshots_downloaded = 0
for edge in edges:
try:
edge_snapshot = self._graph_nav_client.download_edge_snapshot(
edge.snapshot_id)
except Exception:
# Failure in downloading edge snapshot. Continue to next snapshot.
self.add_message("Failed to download edge snapshot: " +
edge.snapshot_id)
continue
write_bytes(self._download_filepath + '/edge_snapshots',
'/' + edge.snapshot_id,
edge_snapshot.SerializeToString())
num_edge_snapshots_downloaded += 1
self.add_message(
"Downloaded {} of the total {} edge snapshots.".format(
num_edge_snapshots_downloaded, len(edges)))
def _make_mission(self):
""" Create a mission that visits each waypoint on stored path."""
# Create a Sequence that visits all the waypoints.
sequence = nodes_pb2.Sequence()
sequence.children.add().CopyFrom(self._make_localize_node())
for waypoint_id in self._waypoint_path:
if waypoint_id == 'LOCALIZE':
sequence.children.add().CopyFrom(self._make_localize_node())
else:
sequence.children.add().CopyFrom(
self._make_goto_node(waypoint_id))
# Return a Node with the Sequence.
ret = nodes_pb2.Node()
ret.name = "visit %d waypoints" % len(self._waypoint_path)
ret.impl.Pack(sequence)
return ret
def _make_goto_node(self, waypoint_id):
""" Create a leaf node that will go to the waypoint. """
ret = nodes_pb2.Node()
ret.name = "goto %s" % waypoint_id
impl = nodes_pb2.BosdynNavigateTo()
impl.destination_waypoint_id = waypoint_id
ret.impl.Pack(impl)
return ret
def _make_localize_node(self):
"""Make localization node."""
loc = nodes_pb2.Node()
loc.name = "localize robot"
impl = nodes_pb2.BosdynGraphNavLocalize()
loc.impl.Pack(impl)
return loc
class WasdInterface(object):
"""A curses interface for driving the robot."""
def __init__(self, robot):
self._robot = robot
# Create clients -- do not use the for communication yet.
self._lease_client = robot.ensure_client(
LeaseClient.default_service_name)
self._estop_client = robot.ensure_client(
EstopClient.default_service_name)
self._estop_endpoint = EstopEndpoint(self._estop_client, 'wasd', 9.0)
self._power_client = robot.ensure_client(
PowerClient.default_service_name)
self._robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
self._robot_metrics_task = AsyncMetrics(self._robot_state_client)
self._robot_state_task = AsyncRobotState(self._robot_state_client)
self._image_task = AsyncImageCapture(robot)
self._async_tasks = AsyncTasks([
self._robot_metrics_task, self._robot_state_task, self._image_task
])
self._lock = threading.Lock()
self._command_dictionary = {
ord('\t'): self._quit_program,
ord('T'): self._toggle_time_sync,
ord(' '): self._toggle_estop,
ord('r'): self._self_right,
ord('P'): self._toggle_power,
ord('v'): self._sit,
ord('f'): self._stand,
ord('w'): self._move_forward,
ord('s'): self._move_backward,
ord('a'): self._strafe_left,
ord('d'): self._strafe_right,
ord('q'): self._turn_left,
ord('e'): self._turn_right,
ord('I'): self._image_task.take_image,
ord('O'): self._image_task.toggle_video_mode,
}
self._locked_messages = ['', '',
''] # string: displayed message for user
self._estop_keepalive = None
self._exit_check = None
# Stuff that is set in start()
self._robot_id = None
self._lease = None
def start(self):
"""Begin communication with the robot."""
self._robot_id = self._robot.get_id()
self._lease = self._lease_client.acquire()
self._estop_endpoint.force_simple_setup(
) # Set this endpoint as the robot's sole estop.
def shutdown(self):
"""Release control of robot as gracefully as posssible."""
LOGGER.info("Shutting down WasdInterface.")
if self._robot.time_sync:
self._robot.time_sync.stop()
if self._estop_keepalive:
_grpc_or_log("stopping estop", self._estop_keepalive.stop)
self._estop_keepalive = None
if self._lease:
_grpc_or_log("returning lease",
lambda: self._lease_client.return_lease(self._lease))
self._lease = None
def __del__(self):
self.shutdown()
def flush_and_estop_buffer(self, stdscr):
"""Manually flush the curses input buffer but trigger any estop requests (space)"""
key = ''
while key != -1:
key = stdscr.getch()
if key == ord(' '):
self._toggle_estop()
def add_message(self, msg_text):
"""Display the given message string to the user in the curses interface."""
with self._lock:
self._locked_messages = [msg_text] + self._locked_messages[:-1]
def message(self, idx):
"""Grab one of the 3 last messages added."""
with self._lock:
return self._locked_messages[idx]
@property
def robot_state(self):
"""Get latest robot state proto."""
return self._robot_state_task.proto
@property
def robot_metrics(self):
"""Get latest robot metrics proto."""
return self._robot_metrics_task.proto
def drive(self, stdscr):
"""User interface to control the robot via the passed-in curses screen interface object."""
with ExitCheck(exit_delay_secs=2.0) as self._exit_check, \
LeaseKeepAlive(self._lease_client,
keep_running_cb=lambda: self._exit_check.keep_alive) \
as lease_keep_alive:
curses_handler = CursesHandler(self)
curses_handler.setLevel(logging.INFO)
LOGGER.addHandler(curses_handler)
stdscr.nodelay(True) # Don't block for user input.
# for debug
curses.echo()
try:
while self._exit_check.keep_alive:
self._async_tasks.update()
self._drive_draw(stdscr, lease_keep_alive)
if not self._exit_check.exit_requested:
try:
cmd = stdscr.getch()
# Do not queue up commands on client
self.flush_and_estop_buffer(stdscr)
self._drive_cmd(cmd)
except Exception:
# On robot command fault, sit down safely before killing the program.
self._safe_power_off()
time.sleep(2.0)
raise
time.sleep(COMMAND_INPUT_RATE)
finally:
LOGGER.removeHandler(curses_handler)
def _drive_draw(self, stdscr, lease_keep_alive):
"""Draw the interface screen at each update."""
stdscr.clear() # clear screen
stdscr.addstr(
0, 0, '{:20s} {}'.format(self._robot_id.nickname,
self._robot_id.serial_number))
stdscr.addstr(1, 0, self._lease_str(lease_keep_alive))
stdscr.addstr(2, 0, self._battery_str())
stdscr.addstr(3, 0, self._estop_str())
stdscr.addstr(4, 0, self._power_state_str())
stdscr.addstr(5, 0, self._time_sync_str())
for i in range(3):
stdscr.addstr(7 + i, 2, self.message(i))
self._show_metrics(stdscr)
stdscr.addstr(10, 0, "Commands: [TAB]: quit")
stdscr.addstr(11, 0,
" [T]: Time-sync, [SPACE]: Estop, [P]: Power")
stdscr.addstr(12, 0, " [I]: Take image, [O]: Video mode")
stdscr.addstr(13, 0, " [v]: Sit, [f]: Stand, [r]: Self-right")
stdscr.addstr(14, 0, " [wasd]: Directional strafing")
stdscr.addstr(15, 0, " [qe]: Turning")
# print as many lines of the image as will fit on the curses screen
if self._image_task.ascii_image != None:
max_y, _max_x = stdscr.getmaxyx()
for y_i, img_line in enumerate(self._image_task.ascii_image):
if y_i + 16 >= max_y:
break
stdscr.addstr('\n' + img_line)
stdscr.refresh()
def _drive_cmd(self, key):
"""Run user commands at each update."""
try:
cmd_function = self._command_dictionary[key]
cmd_function()
except KeyError:
if key and key != -1 and key < 256:
self.add_message("Unrecognized keyboard command: '{}'".format(
chr(key)))
def _try_grpc(self, desc, thunk):
try:
return thunk()
except (ResponseError, RpcError) as err:
self.add_message("Failed {}: {}".format(desc, err))
return None
def _quit_program(self):
self._sit()
self._exit_check.request_exit()
def _toggle_time_sync(self):
if self._robot.time_sync.stopped:
self._robot.time_sync.start()
else:
self._robot.time_sync.stop()
def _toggle_estop(self):
"""toggle estop on/off. Initial state is ON"""
if not self._estop_keepalive:
self._estop_keepalive = EstopKeepAlive(self._estop_endpoint)
else:
self._try_grpc("stopping estop", self._estop_keepalive.stop)
self._estop_keepalive.shutdown()
self._estop_keepalive = None
def _start_robot_command(self, desc, command_proto, end_time_secs=None):
def _start_command():
self._robot_command_client.robot_command(
lease=None, command=command_proto, end_time_secs=end_time_secs)
self._try_grpc(desc, _start_command)
def _self_right(self):
self._start_robot_command('self_right',
RobotCommandBuilder.selfright_command())
def _sit(self):
self._start_robot_command('sit', RobotCommandBuilder.sit_command())
def _stand(self):
self._start_robot_command('stand', RobotCommandBuilder.stand_command())
def _move_forward(self):
self._velocity_cmd_helper('move_forward', v_x=VELOCITY_BASE_SPEED)
def _move_backward(self):
self._velocity_cmd_helper('move_backward', v_x=-VELOCITY_BASE_SPEED)
def _strafe_left(self):
self._velocity_cmd_helper('strafe_left', v_y=VELOCITY_BASE_SPEED)
def _strafe_right(self):
self._velocity_cmd_helper('strafe_right', v_y=-VELOCITY_BASE_SPEED)
def _turn_left(self):
self._velocity_cmd_helper('turn_left', v_rot=VELOCITY_BASE_ANGULAR)
def _turn_right(self):
self._velocity_cmd_helper('turn_right', v_rot=-VELOCITY_BASE_ANGULAR)
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0):
self._start_robot_command(
desc,
RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y,
v_rot=v_rot),
end_time_secs=time.time() + VELOCITY_CMD_DURATION)
# This command makes the robot move very fast and the 'origin' is very unpredictable.
def _return_to_origin(self):
self._start_robot_command(
'fwd_and_rotate',
RobotCommandBuilder.trajectory_command(
goal_x=0.0,
goal_y=0.0,
goal_heading=0.0,
frame=geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_KO),
params=None,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT),
end_time_secs=time.time() + 20)
def _take_ascii_image(self):
source_name = "frontright_fisheye_image"
image_response = self._image_client.get_image_from_sources(
[source_name])
image = Image.open(io.BytesIO(image_response[0].shot.image.data))
ascii_image = self._ascii_converter.convert_to_ascii(image,
new_width=70)
self._last_image_ascii = ascii_image
def _toggle_ascii_video(self):
if self._video_mode:
self._video_mode = False
else:
self._video_mode = True
# def _clear_behavior_faults(self):
# cmd = robot_state_proto.GetRobotState()
# self._start_robot_command(cmd)
# robot_command_pb2.clear_behavior_fault(behavior_fault_id = , lease = self._lease)
def _toggle_power(self):
power_state = self._power_state()
if power_state is None:
self.add_message(
'Could not toggle power because power state is unknown')
return
if power_state == robot_state_proto.PowerState.STATE_OFF:
self._try_grpc("powering-on", self._request_power_on)
else:
self._try_grpc("powering-off", self._safe_power_off)
def _request_power_on(self):
bosdyn.client.power.power_on(self._power_client)
def _safe_power_off(self):
self._start_robot_command('safe_power_off',
RobotCommandBuilder.safe_power_off_command())
def _show_metrics(self, stdscr):
metrics = self.robot_metrics
if not metrics:
return
for idx, metric in enumerate(metrics.metrics):
stdscr.addstr(2 + idx, 50, format_metric(metric))
def _power_state(self):
state = self.robot_state
if not state:
return None
return state.power_state.motor_power_state
def _lease_str(self, lease_keep_alive):
alive = 'RUNNING' if lease_keep_alive.is_alive() else 'STOPPED'
lease = '{}:{}'.format(self._lease.lease_proto.resource,
self._lease.lease_proto.sequence)
return 'Lease {} THREAD:{}'.format(lease, alive)
def _power_state_str(self):
power_state = self._power_state()
if power_state is None:
return ''
state_str = robot_state_proto.PowerState.MotorPowerState.Name(
power_state)
return 'Power: {}'.format(state_str[6:]) # get rid of STATE_ prefix
def _estop_str(self):
thread_status = 'RUNNING' if self._estop_keepalive else 'STOPPED'
estop_status = '??'
state = self.robot_state
if state:
for estop_state in state.estop_states:
if estop_state.type == estop_state.TYPE_SOFTWARE:
estop_status = estop_state.State.Name(
estop_state.state)[6:] # s/STATE_//
break
return 'Estop {} (thread: {})'.format(estop_status, thread_status)
def _time_sync_str(self):
if not self._robot.time_sync:
return 'Time sync: (none)'
if self._robot.time_sync.stopped:
status = 'STOPPED'
exception = self._robot.time_sync.thread_exception
if exception:
status = '{} Exception: {}'.format(status, exception)
else:
status = 'RUNNING'
try:
skew = self._robot.time_sync.get_robot_clock_skew()
if skew:
skew_str = 'offset={}'.format(duration_str(skew))
else:
skew_str = "(Skew undetermined)"
except (TimeSyncError, RpcError) as err:
skew_str = '({})'.format(err)
return 'Time sync: {} {}'.format(status, skew_str)
def _battery_str(self):
if not self.robot_state:
return ''
battery_state = self.robot_state.battery_states[0]
status = battery_state.Status.Name(battery_state.status)
status = status[7:] # get rid of STATUS_ prefix
if battery_state.charge_percentage.value:
bar_len = int(battery_state.charge_percentage.value) // 10
bat_bar = '|{}{}|'.format('=' * bar_len, ' ' * (10 - bar_len))
else:
bat_bar = ''
time_left = ''
if battery_state.estimated_runtime:
time_left = ' ({})'.format(
secs_to_hms(battery_state.estimated_runtime.seconds))
return 'Battery: {}{}{}'.format(status, bat_bar, time_left)
class SpotWrapper():
"""Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically"""
def __init__(self,
username,
password,
hostname,
logger,
rates={},
callbacks={}):
self._username = username
self._password = password
self._hostname = hostname
self._logger = logger
self._rates = rates
self._callbacks = callbacks
self._keep_alive = True
self._valid = True
self._mobility_params = RobotCommandBuilder.mobility_params()
self._is_standing = False
self._is_sitting = True
self._is_moving = False
self._last_stand_command = None
self._last_sit_command = None
self._last_motion_command = None
self._last_motion_command_time = None
self._front_image_requests = []
for source in front_image_sources:
self._front_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
self._side_image_requests = []
for source in side_image_sources:
self._side_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
self._rear_image_requests = []
for source in rear_image_sources:
self._rear_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
try:
self._sdk = create_standard_sdk('ros_spot')
except Exception as e:
self._logger.error("Error creating SDK object: %s", e)
self._valid = False
return
self._robot = self._sdk.create_robot(self._hostname)
try:
self._robot.authenticate(self._username, self._password)
self._robot.start_time_sync()
except RpcError as err:
self._logger.error("Failed to communicate with robot: %s", err)
self._valid = False
return
if self._robot:
# Clients
try:
self._robot_state_client = self._robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
self._graph_nav_client = self._robot.ensure_client(
GraphNavClient.default_service_name)
self._power_client = self._robot.ensure_client(
PowerClient.default_service_name)
self._lease_client = self._robot.ensure_client(
LeaseClient.default_service_name)
self._lease_wallet = self._lease_client.lease_wallet
self._image_client = self._robot.ensure_client(
ImageClient.default_service_name)
self._estop_client = self._robot.ensure_client(
EstopClient.default_service_name)
except Exception as e:
self._logger.error("Unable to create client service: %s", e)
self._valid = False
return
# Store the most recent knowledge of the state of the robot based on rpc calls.
self._current_graph = None
self._current_edges = dict(
) #maps to_waypoint to list(from_waypoint)
self._current_waypoint_snapshots = dict(
) # maps id to waypoint snapshot
self._current_edge_snapshots = dict() # maps id to edge snapshot
self._current_annotation_name_to_wp_id = dict()
# Async Tasks
self._async_task_list = []
self._robot_state_task = AsyncRobotState(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("robot_state", 0.0)),
self._callbacks.get("robot_state", lambda: None))
self._robot_metrics_task = AsyncMetrics(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("metrics", 0.0)),
self._callbacks.get("metrics", lambda: None))
self._lease_task = AsyncLease(
self._lease_client, self._logger,
max(0.0, self._rates.get("lease", 0.0)),
self._callbacks.get("lease", lambda: None))
self._front_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("front_image", 0.0)),
self._callbacks.get("front_image", lambda: None),
self._front_image_requests)
self._side_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("side_image", 0.0)),
self._callbacks.get("side_image", lambda: None),
self._side_image_requests)
self._rear_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("rear_image", 0.0)),
self._callbacks.get("rear_image", lambda: None),
self._rear_image_requests)
self._idle_task = AsyncIdle(self._robot_command_client,
self._logger, 10.0, self)
self._estop_endpoint = None
self._async_tasks = AsyncTasks([
self._robot_state_task, self._robot_metrics_task,
self._lease_task, self._front_image_task,
self._side_image_task, self._rear_image_task, self._idle_task
])
self._robot_id = None
self._lease = None
@property
def is_valid(self):
"""Return boolean indicating if the wrapper initialized successfully"""
return self._valid
@property
def id(self):
"""Return robot's ID"""
return self._robot_id
@property
def robot_state(self):
"""Return latest proto from the _robot_state_task"""
return self._robot_state_task.proto
@property
def metrics(self):
"""Return latest proto from the _robot_metrics_task"""
return self._robot_metrics_task.proto
@property
def lease(self):
"""Return latest proto from the _lease_task"""
return self._lease_task.proto
@property
def front_images(self):
"""Return latest proto from the _front_image_task"""
return self._front_image_task.proto
@property
def side_images(self):
"""Return latest proto from the _side_image_task"""
return self._side_image_task.proto
@property
def rear_images(self):
"""Return latest proto from the _rear_image_task"""
return self._rear_image_task.proto
@property
def is_standing(self):
"""Return boolean of standing state"""
return self._is_standing
@property
def is_sitting(self):
"""Return boolean of standing state"""
return self._is_sitting
@property
def is_moving(self):
"""Return boolean of walking state"""
return self._is_moving
@property
def time_skew(self):
"""Return the time skew between local and spot time"""
return self._robot.time_sync.endpoint.clock_skew
def robotToLocalTime(self, timestamp):
"""Takes a timestamp and an estimated skew and return seconds and nano seconds
Args:
timestamp: google.protobuf.Timestamp
Returns:
google.protobuf.Timestamp
"""
rtime = Timestamp()
rtime.seconds = timestamp.seconds - self.time_skew.seconds
rtime.nanos = timestamp.nanos - self.time_skew.nanos
if rtime.nanos < 0:
rtime.nanos = rtime.nanos + 1000000000
rtime.seconds = rtime.seconds - 1
# Workaround for timestamps being incomplete
if rtime.seconds < 0:
rtime.seconds = 0
rtime.nanos = 0
return rtime
def claim(self):
"""Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot."""
try:
self._robot_id = self._robot.get_id()
self.getLease()
self.resetEStop()
return True, "Success"
except (ResponseError, RpcError) as err:
self._logger.error("Failed to initialize robot communication: %s",
err)
return False, str(err)
def updateTasks(self):
"""Loop through all periodic tasks and update their data if needed."""
self._async_tasks.update()
def resetEStop(self):
"""Get keepalive for eStop"""
self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0)
self._estop_endpoint.force_simple_setup(
) # Set this endpoint as the robot's sole estop.
self._estop_keepalive = EstopKeepAlive(self._estop_endpoint)
def assertEStop(self, severe=True):
"""Forces the robot into eStop state.
Args:
severe: Default True - If true, will cut motor power immediately. If false, will try to settle the robot on the ground first
"""
try:
if severe:
self._estop_endpoint.stop()
else:
self._estop_endpoint.settle_then_cut()
return True, "Success"
except:
return False, "Error"
def releaseEStop(self):
"""Stop eStop keepalive"""
if self._estop_keepalive:
self._estop_keepalive.stop()
self._estop_keepalive = None
self._estop_endpoint = None
def getLease(self):
"""Get a lease for the robot and keep the lease alive automatically."""
self._lease = self._lease_client.acquire()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
def releaseLease(self):
"""Return the lease on the body."""
if self._lease:
self._lease_client.return_lease(self._lease)
self._lease = None
def release(self):
"""Return the lease on the body and the eStop handle."""
try:
self.releaseLease()
self.releaseEStop()
return True, "Success"
except Exception as e:
return False, str(e)
def disconnect(self):
"""Release control of robot as gracefully as posssible."""
if self._robot.time_sync:
self._robot.time_sync.stop()
self.releaseLease()
self.releaseEStop()
def _robot_command(self, command_proto, end_time_secs=None):
"""Generic blocking function for sending commands to robots.
Args:
command_proto: robot_command_pb2 object to send to the robot. Usually made with RobotCommandBuilder
end_time_secs: (optional) Time-to-live for the command in seconds
"""
try:
id = self._robot_command_client.robot_command(
lease=None, command=command_proto, end_time_secs=end_time_secs)
return True, "Success", id
except Exception as e:
return False, str(e), None
def stop(self):
"""Stop the robot's motion."""
response = self._robot_command(RobotCommandBuilder.stop_command())
return response[0], response[1]
def self_right(self):
"""Have the robot self-right itself."""
response = self._robot_command(RobotCommandBuilder.selfright_command())
return response[0], response[1]
def sit(self):
"""Stop the robot's motion and sit down if able."""
response = self._robot_command(RobotCommandBuilder.sit_command())
self._last_sit_command = response[2]
return response[0], response[1]
def stand(self, monitor_command=True):
"""If the e-stop is enabled, and the motor power is enabled, stand the robot up."""
response = self._robot_command(
RobotCommandBuilder.synchro_stand_command(
params=self._mobility_params))
if monitor_command:
self._last_stand_command = response[2]
return response[0], response[1]
def safe_power_off(self):
"""Stop the robot's motion and sit if possible. Once sitting, disable motor power."""
response = self._robot_command(
RobotCommandBuilder.safe_power_off_command())
return response[0], response[1]
def power_on(self):
"""Enble the motor power if e-stop is enabled."""
try:
power.power_on(self._power_client)
return True, "Success"
except:
return False, "Error"
def set_mobility_params(self, mobility_params):
"""Set Params for mobility and movement
Args:
mobility_params: spot.MobilityParams, params for spot mobility commands.
"""
self._mobility_params = mobility_params
def get_mobility_params(self):
"""Get mobility params
"""
return self._mobility_params
def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.1):
"""Send a velocity motion command to the robot.
Args:
v_x: Velocity in the X direction in meters
v_y: Velocity in the Y direction in meters
v_rot: Angular velocity around the Z axis in radians
cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate).
"""
end_time = time.time() + cmd_duration
self._robot_command(RobotCommandBuilder.synchro_velocity_command(
v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params),
end_time_secs=end_time)
self._last_motion_command_time = end_time
def list_graph(self, upload_path):
"""List waypoint ids of garph_nav
Args:
upload_path : Path to the root directory of the map.
"""
ids, eds = self._list_graph_waypoint_and_edge_ids()
# skip waypoint_ for v2.2.1, skip waypiont for < v2.2
return [
v for k, v in sorted(
ids.items(),
key=lambda id: int(id[0].replace('waypoint_', '')))
]
def navigate_to(self,
upload_path,
navigate_to,
initial_localization_fiducial=True,
initial_localization_waypoint=None):
""" navigate with graph nav.
Args:
upload_path : Path to the root directory of the map.
navigate_to : Waypont id string for where to goal
initial_localization_fiducial : Tells the initializer whether to use fiducials
initial_localization_waypoint : Waypoint id string of current robot position (optional)
"""
# Filepath for uploading a saved graph's and snapshots too.
if upload_path[-1] == "/":
upload_filepath = upload_path[:-1]
else:
upload_filepath = upload_path
# Boolean indicating the robot's power state.
power_state = self._robot_state_client.get_robot_state().power_state
self._started_powered_on = (
power_state.motor_power_state == power_state.STATE_ON)
self._powered_on = self._started_powered_on
# FIX ME somehow,,,, if the robot is stand, need to sit the robot before starting garph nav
if self.is_standing and not self.is_moving:
self.sit()
# TODO verify estop / claim / power_on
self._clear_graph()
self._upload_graph_and_snapshots(upload_filepath)
if initial_localization_fiducial:
self._set_initial_localization_fiducial()
if initial_localization_waypoint:
self._set_initial_localization_waypoint(
[initial_localization_waypoint])
self._list_graph_waypoint_and_edge_ids()
self._get_localization_state()
resp = self._navigate_to([navigate_to])
return resp
## copy from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py
def _get_localization_state(self, *args):
"""Get the current localization and state of the robot."""
state = self._graph_nav_client.get_localization_state()
print('Got localization: \n%s' % str(state.localization))
odom_tform_body = get_odom_tform_body(
state.robot_kinematics.transforms_snapshot)
print('Got robot state in kinematic odometry frame: \n%s' %
str(odom_tform_body))
def _set_initial_localization_fiducial(self, *args):
"""Trigger localization when near a fiducial."""
robot_state = self._robot_state_client.get_robot_state()
current_odom_tform_body = get_odom_tform_body(
robot_state.kinematic_state.transforms_snapshot).to_proto()
# Create an empty instance for initial localization since we are asking it to localize
# based on the nearest fiducial.
localization = nav_pb2.Localization()
self._graph_nav_client.set_localization(
initial_guess_localization=localization,
ko_tform_body=current_odom_tform_body)
def _set_initial_localization_waypoint(self, *args):
"""Trigger localization to a waypoint."""
# Take the first argument as the localization waypoint.
if len(args) < 1:
# If no waypoint id is given as input, then return without initializing.
print("No waypoint specified to initialize to.")
return
destination_waypoint = graph_nav_util.find_unique_waypoint_id(
args[0][0], self._current_graph,
self._current_annotation_name_to_wp_id)
if not destination_waypoint:
# Failed to find the unique waypoint id.
return
robot_state = self._robot_state_client.get_robot_state()
current_odom_tform_body = get_odom_tform_body(
robot_state.kinematic_state.transforms_snapshot).to_proto()
# Create an initial localization to the specified waypoint as the identity.
localization = nav_pb2.Localization()
localization.waypoint_id = destination_waypoint
localization.waypoint_tform_body.rotation.w = 1.0
self._graph_nav_client.set_localization(
initial_guess_localization=localization,
# It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m).
max_distance=0.2,
max_yaw=20.0 * math.pi / 180.0,
fiducial_init=graph_nav_pb2.SetLocalizationRequest.
FIDUCIAL_INIT_NO_FIDUCIAL,
ko_tform_body=current_odom_tform_body)
def _list_graph_waypoint_and_edge_ids(self, *args):
"""List the waypoint ids and edge ids of the graph currently on the robot."""
# Download current graph
graph = self._graph_nav_client.download_graph()
if graph is None:
print("Empty graph.")
return
self._current_graph = graph
localization_id = self._graph_nav_client.get_localization_state(
).localization.waypoint_id
# Update and print waypoints and edges
self._current_annotation_name_to_wp_id, self._current_edges = graph_nav_util.update_waypoints_and_edges(
graph, localization_id)
return self._current_annotation_name_to_wp_id, self._current_edges
def _upload_graph_and_snapshots(self, upload_filepath):
"""Upload the graph and snapshots to the robot."""
print("Loading the graph from disk into local storage...")
with open(upload_filepath + "/graph", "rb") as graph_file:
# Load the graph from disk.
data = graph_file.read()
self._current_graph = map_pb2.Graph()
self._current_graph.ParseFromString(data)
print("Loaded graph has {} waypoints and {} edges".format(
len(self._current_graph.waypoints),
len(self._current_graph.edges)))
for waypoint in self._current_graph.waypoints:
# Load the waypoint snapshots from disk.
with open(
upload_filepath +
"/waypoint_snapshots/{}".format(waypoint.snapshot_id),
"rb") as snapshot_file:
waypoint_snapshot = map_pb2.WaypointSnapshot()
waypoint_snapshot.ParseFromString(snapshot_file.read())
self._current_waypoint_snapshots[
waypoint_snapshot.id] = waypoint_snapshot
for edge in self._current_graph.edges:
# Load the edge snapshots from disk.
with open(
upload_filepath +
"/edge_snapshots/{}".format(edge.snapshot_id),
"rb") as snapshot_file:
edge_snapshot = map_pb2.EdgeSnapshot()
edge_snapshot.ParseFromString(snapshot_file.read())
self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot
# Upload the graph to the robot.
print("Uploading the graph and snapshots to the robot...")
self._graph_nav_client.upload_graph(lease=self._lease.lease_proto,
graph=self._current_graph)
# Upload the snapshots to the robot.
for waypoint_snapshot in self._current_waypoint_snapshots.values():
self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot)
print("Uploaded {}".format(waypoint_snapshot.id))
for edge_snapshot in self._current_edge_snapshots.values():
self._graph_nav_client.upload_edge_snapshot(edge_snapshot)
print("Uploaded {}".format(edge_snapshot.id))
# The upload is complete! Check that the robot is localized to the graph,
# and it if is not, prompt the user to localize the robot before attempting
# any navigation commands.
localization_state = self._graph_nav_client.get_localization_state()
if not localization_state.localization.waypoint_id:
# The robot is not localized to the newly uploaded graph.
print("\n")
print("Upload complete! The robot is currently not localized to the map; please localize", \
"the robot using commands (2) or (3) before attempting a navigation command.")
def _navigate_to(self, *args):
"""Navigate to a specific waypoint."""
# Take the first argument as the destination waypoint.
if len(args) < 1:
# If no waypoint id is given as input, then return without requesting navigation.
print("No waypoint provided as a destination for navigate to.")
return
self._lease = self._lease_wallet.get_lease()
destination_waypoint = graph_nav_util.find_unique_waypoint_id(
args[0][0], self._current_graph,
self._current_annotation_name_to_wp_id)
if not destination_waypoint:
# Failed to find the appropriate unique waypoint id for the navigation command.
return
if not self.toggle_power(should_power_on=True):
print(
"Failed to power on the robot, and cannot complete navigate to request."
)
return
# Stop the lease keepalive and create a new sublease for graph nav.
self._lease = self._lease_wallet.advance()
sublease = self._lease.create_sublease()
self._lease_keepalive.shutdown()
# Navigate to the destination waypoint.
is_finished = False
nav_to_cmd_id = -1
while not is_finished:
# Issue the navigation command about twice a second such that it is easy to terminate the
# navigation command (with estop or killing the program).
nav_to_cmd_id = self._graph_nav_client.navigate_to(
destination_waypoint, 1.0, leases=[sublease.lease_proto])
time.sleep(
.5) # Sleep for half a second to allow for command execution.
# Poll the robot for feedback to determine if the navigation command is complete. Then sit
# the robot down once it is finished.
is_finished = self._check_success(nav_to_cmd_id)
self._lease = self._lease_wallet.advance()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
# Update the lease and power off the robot if appropriate.
if self._powered_on and not self._started_powered_on:
# Sit the robot down + power off after the navigation command is complete.
self.toggle_power(should_power_on=False)
status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id)
if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL:
return True, "Successfully completed the navigation commands!"
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST:
return False, "Robot got lost when navigating the route, the robot will now sit down."
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK:
return False, "Robot got stuck when navigating the route, the robot will now sit down."
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED:
return False, "Robot is impaired."
else:
return False, "Navigation command is not complete yet."
def _navigate_route(self, *args):
"""Navigate through a specific route of waypoints."""
if len(args) < 1:
# If no waypoint ids are given as input, then return without requesting navigation.
print("No waypoints provided for navigate route.")
return
waypoint_ids = args[0]
for i in range(len(waypoint_ids)):
waypoint_ids[i] = graph_nav_util.find_unique_waypoint_id(
waypoint_ids[i], self._current_graph,
self._current_annotation_name_to_wp_id)
if not waypoint_ids[i]:
# Failed to find the unique waypoint id.
return
edge_ids_list = []
all_edges_found = True
# Attempt to find edges in the current graph that match the ordered waypoint pairs.
# These are necessary to create a valid route.
for i in range(len(waypoint_ids) - 1):
start_wp = waypoint_ids[i]
end_wp = waypoint_ids[i + 1]
edge_id = self._match_edge(self._current_edges, start_wp, end_wp)
if edge_id is not None:
edge_ids_list.append(edge_id)
else:
all_edges_found = False
print("Failed to find an edge between waypoints: ", start_wp,
" and ", end_wp)
print(
"List the graph's waypoints and edges to ensure pairs of waypoints has an edge."
)
break
self._lease = self._lease_wallet.get_lease()
if all_edges_found:
if not self.toggle_power(should_power_on=True):
print(
"Failed to power on the robot, and cannot complete navigate route request."
)
return
# Stop the lease keepalive and create a new sublease for graph nav.
self._lease = self._lease_wallet.advance()
sublease = self._lease.create_sublease()
self._lease_keepalive.shutdown()
# Navigate a specific route.
route = self._graph_nav_client.build_route(waypoint_ids,
edge_ids_list)
is_finished = False
while not is_finished:
# Issue the route command about twice a second such that it is easy to terminate the
# navigation command (with estop or killing the program).
nav_route_command_id = self._graph_nav_client.navigate_route(
route, cmd_duration=1.0, leases=[sublease.lease_proto])
time.sleep(
.5
) # Sleep for half a second to allow for command execution.
# Poll the robot for feedback to determine if the route is complete. Then sit
# the robot down once it is finished.
is_finished = self._check_success(nav_route_command_id)
self._lease = self._lease_wallet.advance()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
# Update the lease and power off the robot if appropriate.
if self._powered_on and not self._started_powered_on:
# Sit the robot down + power off after the navigation command is complete.
self.toggle_power(should_power_on=False)
def _clear_graph(self, *args):
"""Clear the state of the map on the robot, removing all waypoints and edges."""
return self._graph_nav_client.clear_graph(
lease=self._lease.lease_proto)
def toggle_power(self, should_power_on):
"""Power the robot on/off dependent on the current power state."""
is_powered_on = self.check_is_powered_on()
if not is_powered_on and should_power_on:
# Power on the robot up before navigating when it is in a powered-off state.
power_on(self._power_client)
motors_on = False
while not motors_on:
future = self._robot_state_client.get_robot_state_async()
state_response = future.result(
timeout=10
) # 10 second timeout for waiting for the state response.
if state_response.power_state.motor_power_state == robot_state_pb2.PowerState.STATE_ON:
motors_on = True
else:
# Motors are not yet fully powered on.
time.sleep(.25)
elif is_powered_on and not should_power_on:
# Safe power off (robot will sit then power down) when it is in a
# powered-on state.
safe_power_off(self._robot_command_client,
self._robot_state_client)
else:
# Return the current power state without change.
return is_powered_on
# Update the locally stored power state.
self.check_is_powered_on()
return self._powered_on
def check_is_powered_on(self):
"""Determine if the robot is powered on or off."""
power_state = self._robot_state_client.get_robot_state().power_state
self._powered_on = (
power_state.motor_power_state == power_state.STATE_ON)
return self._powered_on
def _check_success(self, command_id=-1):
"""Use a navigation command id to get feedback from the robot and sit when command succeeds."""
if command_id == -1:
# No command, so we have not status to check.
return False
status = self._graph_nav_client.navigation_feedback(command_id)
if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL:
# Successfully completed the navigation commands!
return True
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST:
print(
"Robot got lost when navigating the route, the robot will now sit down."
)
return True
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK:
print(
"Robot got stuck when navigating the route, the robot will now sit down."
)
return True
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED:
print("Robot is impaired.")
return True
else:
# Navigation command is not complete yet.
return False
def _match_edge(self, current_edges, waypoint1, waypoint2):
"""Find an edge in the graph that is between two waypoint ids."""
# Return the correct edge id as soon as it's found.
for edge_to_id in current_edges:
for edge_from_id in current_edges[edge_to_id]:
if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id):
# This edge matches the pair of waypoints! Add it the edge list and continue.
return map_pb2.Edge.Id(from_waypoint=waypoint2,
to_waypoint=waypoint1)
elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id):
# This edge matches the pair of waypoints! Add it the edge list and continue.
return map_pb2.Edge.Id(from_waypoint=waypoint1,
to_waypoint=waypoint2)
return None
class SpotWrapper():
"""Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically"""
def __init__(self,
username,
password,
hostname,
logger,
rates={},
callbacks={}):
self._username = username
self._password = password
self._hostname = hostname
self._logger = logger
self._rates = rates
self._callbacks = callbacks
self._keep_alive = True
self._valid = True
self._mobility_params = RobotCommandBuilder.mobility_params()
self._is_standing = False
self._is_sitting = True
self._is_moving = False
self._last_stand_command = None
self._last_sit_command = None
self._last_motion_command = None
self._last_motion_command_time = None
self._front_image_requests = []
for source in front_image_sources:
self._front_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
self._side_image_requests = []
for source in side_image_sources:
self._side_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
self._rear_image_requests = []
for source in rear_image_sources:
self._rear_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
try:
self._sdk = create_standard_sdk('ros_spot')
except Exception as e:
self._logger.error("Error creating SDK object: %s", e)
self._valid = False
return
self._robot = self._sdk.create_robot(self._hostname)
try:
self._robot.authenticate(self._username, self._password)
self._robot.start_time_sync()
except RpcError as err:
self._logger.error("Failed to communicate with robot: %s", err)
self._valid = False
return
if self._robot:
# Clients
try:
self._robot_state_client = self._robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
self._power_client = self._robot.ensure_client(
PowerClient.default_service_name)
self._lease_client = self._robot.ensure_client(
LeaseClient.default_service_name)
self._image_client = self._robot.ensure_client(
ImageClient.default_service_name)
self._estop_client = self._robot.ensure_client(
EstopClient.default_service_name)
except Exception as e:
self._logger.error("Unable to create client service: %s", e)
self._valid = False
return
# Async Tasks
self._async_task_list = []
self._robot_state_task = AsyncRobotState(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("robot_state", 0.0)),
self._callbacks.get("robot_state", lambda: None))
self._robot_metrics_task = AsyncMetrics(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("metrics", 0.0)),
self._callbacks.get("metrics", lambda: None))
self._lease_task = AsyncLease(
self._lease_client, self._logger,
max(0.0, self._rates.get("lease", 0.0)),
self._callbacks.get("lease", lambda: None))
self._front_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("front_image", 0.0)),
self._callbacks.get("front_image", lambda: None),
self._front_image_requests)
self._side_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("side_image", 0.0)),
self._callbacks.get("side_image", lambda: None),
self._side_image_requests)
self._rear_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("rear_image", 0.0)),
self._callbacks.get("rear_image", lambda: None),
self._rear_image_requests)
self._idle_task = AsyncIdle(self._robot_command_client,
self._logger, 10.0, self)
self._estop_endpoint = None
self._async_tasks = AsyncTasks([
self._robot_state_task, self._robot_metrics_task,
self._lease_task, self._front_image_task,
self._side_image_task, self._rear_image_task, self._idle_task
])
self._robot_id = None
self._lease = None
@property
def is_valid(self):
"""Return boolean indicating if the wrapper initialized successfully"""
return self._valid
@property
def id(self):
"""Return robot's ID"""
return self._robot_id
@property
def robot_state(self):
"""Return latest proto from the _robot_state_task"""
return self._robot_state_task.proto
@property
def metrics(self):
"""Return latest proto from the _robot_metrics_task"""
return self._robot_metrics_task.proto
@property
def lease(self):
"""Return latest proto from the _lease_task"""
return self._lease_task.proto
@property
def front_images(self):
"""Return latest proto from the _front_image_task"""
return self._front_image_task.proto
@property
def side_images(self):
"""Return latest proto from the _side_image_task"""
return self._side_image_task.proto
@property
def rear_images(self):
"""Return latest proto from the _rear_image_task"""
return self._rear_image_task.proto
@property
def is_standing(self):
"""Return boolean of standing state"""
return self._is_standing
@property
def is_sitting(self):
"""Return boolean of standing state"""
return self._is_sitting
@property
def is_moving(self):
"""Return boolean of walking state"""
return self._is_moving
@property
def time_skew(self):
"""Return the time skew between local and spot time"""
return self._robot.time_sync.endpoint.clock_skew
def robotToLocalTime(self, timestamp):
"""Takes a timestamp and an estimated skew and return seconds and nano seconds
Args:
timestamp: google.protobuf.Timestamp
Returns:
google.protobuf.Timestamp
"""
rtime = Timestamp()
rtime.seconds = timestamp.seconds - self.time_skew.seconds
rtime.nanos = timestamp.nanos - self.time_skew.nanos
if rtime.nanos < 0:
rtime.nanos = rtime.nanos + 1000000000
rtime.seconds = rtime.seconds - 1
# Workaround for timestamps being incomplete
if rtime.seconds < 0:
rtime.seconds = 0
rtime.nanos = 0
return rtime
def claim(self):
"""Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot."""
try:
self._robot_id = self._robot.get_id()
self.getLease()
self.resetEStop()
return True, "Success"
except (ResponseError, RpcError) as err:
self._logger.error("Failed to initialize robot communication: %s",
err)
return False, str(err)
def updateTasks(self):
"""Loop through all periodic tasks and update their data if needed."""
self._async_tasks.update()
def resetEStop(self):
"""Get keepalive for eStop"""
self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0)
self._estop_endpoint.force_simple_setup(
) # Set this endpoint as the robot's sole estop.
self._estop_keepalive = EstopKeepAlive(self._estop_endpoint)
def assertEStop(self, severe=True):
"""Forces the robot into eStop state.
Args:
severe: Default True - If true, will cut motor power immediately. If false, will try to settle the robot on the ground first
"""
try:
if severe:
self._estop_endpoint.stop()
else:
self._estop_endpoint.settle_then_cut()
return True, "Success"
except:
return False, "Error"
def releaseEStop(self):
"""Stop eStop keepalive"""
if self._estop_keepalive:
self._estop_keepalive.stop()
self._estop_keepalive = None
self._estop_endpoint = None
def getLease(self):
"""Get a lease for the robot and keep the lease alive automatically."""
self._lease = self._lease_client.acquire()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
def releaseLease(self):
"""Return the lease on the body."""
if self._lease:
self._lease_client.return_lease(self._lease)
self._lease = None
def release(self):
"""Return the lease on the body and the eStop handle."""
try:
self.releaseLease()
self.releaseEStop()
return True, "Success"
except Exception as e:
return False, str(e)
def disconnect(self):
"""Release control of robot as gracefully as posssible."""
if self._robot.time_sync:
self._robot.time_sync.stop()
self.releaseLease()
self.releaseEStop()
def _robot_command(self, command_proto, end_time_secs=None):
"""Generic blocking function for sending commands to robots.
Args:
command_proto: robot_command_pb2 object to send to the robot. Usually made with RobotCommandBuilder
end_time_secs: (optional) Time-to-live for the command in seconds
"""
try:
id = self._robot_command_client.robot_command(
lease=None, command=command_proto, end_time_secs=end_time_secs)
return True, "Success", id
except Exception as e:
return False, str(e), None
def stop(self):
"""Stop the robot's motion."""
response = self._robot_command(RobotCommandBuilder.stop_command())
return response[0], response[1]
def self_right(self):
"""Have the robot self-right itself."""
response = self._robot_command(RobotCommandBuilder.selfright_command())
return response[0], response[1]
def sit(self):
"""Stop the robot's motion and sit down if able."""
response = self._robot_command(RobotCommandBuilder.sit_command())
self._last_sit_command = response[2]
return response[0], response[1]
def stand(self, monitor_command=True):
"""If the e-stop is enabled, and the motor power is enabled, stand the robot up."""
response = self._robot_command(
RobotCommandBuilder.stand_command(params=self._mobility_params))
if monitor_command:
self._last_stand_command = response[2]
return response[0], response[1]
def safe_power_off(self):
"""Stop the robot's motion and sit if possible. Once sitting, disable motor power."""
response = self._robot_command(
RobotCommandBuilder.safe_power_off_command())
return response[0], response[1]
def power_on(self):
"""Enble the motor power if e-stop is enabled."""
try:
power.power_on(self._power_client)
return True, "Success"
except:
return False, "Error"
def set_mobility_params(self,
body_height=0,
footprint_R_body=EulerZXY(),
locomotion_hint=1,
stair_hint=False,
external_force_params=None):
"""Define body, locomotion, and stair parameters.
Args:
body_height: Body height in meters
footprint_R_body: (EulerZXY) – The orientation of the body frame with respect to the footprint frame (gravity aligned framed with yaw computed from the stance feet)
locomotion_hint: Locomotion hint
stair_hint: Boolean to define stair motion
"""
self._mobility_params = RobotCommandBuilder.mobility_params(
body_height, footprint_R_body, locomotion_hint, stair_hint,
external_force_params)
def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.1):
"""Send a velocity motion command to the robot.
Args:
v_x: Velocity in the X direction in meters
v_y: Velocity in the Y direction in meters
v_rot: Angular velocity around the Z axis in radians
cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate).
"""
end_time = time.time() + cmd_duration
self._robot_command(RobotCommandBuilder.velocity_command(
v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params),
end_time_secs=end_time)
self._last_motion_command_time = end_time
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
class FiducialFollow(object):
""" Detect and follow a fiducial with Spot"""
def __init__(self, robot):
#Robot instance variable
self._robot = robot
self._lease_client = robot.ensure_client(
LeaseClient.default_service_name)
self._estop_client = robot.ensure_client(
EstopClient.default_service_name)
self._estop_endpoint = EstopEndpoint(self._estop_client,
'fiducial_follow', 9.0)
self._power_client = robot.ensure_client(
PowerClient.default_service_name)
self._image_client = robot.ensure_client(
ImageClient.default_service_name)
self._robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
#Fiducial dimensions (width, height) in millimeters
self._fiducial_width_mm = 154.1
self._fiducial_height_mm = 154.1
#Detection parameters for the fiducial library
self.blur = 0.0 #gaussian blur (useful for noisy images)
#Stopping Distance (x,y) offset from the tag and angle offset from desired angle
self._dist_x_desired = .05
self._dist_y_desired = .05
#Maximum speeds
self._max_x_vel = 1.0
self._max_y_vel = 1.0
self._max_ang_vel = 1.0
#Indicators for movement and image displays
self._display_images = True # Display live from the robot
self._standup = True # Stand up the robot
self._movement_on = True # Let the robot walk towards the fiducial
self._limit_speed = False # Limit the robot's walking speed
self._avoid_obstacles = False # Disable obstacle avoidance
self._debug = False # Printouts for debugging purposes
#Robot's id, lease and estop information
self._robot_id = None
self._lease = None
self._lease_keepalive = None
self._estop_keepalive = None
# Epsilon distance between robot and april tag
self._x_eps = .05
self._y_eps = .05
self._angle_eps = .075
# Indicator for if motor power is on
self._powered_on = False
# Counter for the number of iterations completed
self._attempts = 0
# Maximum amount of iterations before powering off the motors
self._max_attempts = 100000
# Camera intrinsics for the current camera source being analyzed
self._intrinsics = None
# Transform from the robot's camera frame to the baselink frame
self._camera_T_body = None
# Transform from the robot's baselink to the world frame
self._body_T_world = None
# Latest detected fiducial's position in the world
self._current_tag_world_pose = np.array([])
# Heading angle based on the camera source which detected the fiducial
self._angle_desired = None
# Indicator if a fiducial has been detected this iteration
self._tag_not_located = True
# Dictionary mapping camera source to it's latest image taken
self._image = dict()
# List of all possible camera sources
self._source_names = [
src.name for src in self._image_client.list_image_sources()
if "image" in src.name
]
# Dictionary mapping camera source to previously computed extrinsics
self._cam_to_ext_guess = self.populate_source_dict()
# Camera source which a bounding box was last detected in
self._previous_source = None
@property
def robot_state(self):
"""Get latest robot state proto."""
return self._robot_state_client.get_robot_state()
@property
def display_images(self):
"""Return if images should be displayed as video."""
return self._display_images
@property
def image(self):
"""Return the current image associated with each source name."""
return self._image
@property
def image_sources_list(self):
"""Return the list of camera sources."""
return self._source_names
def populate_source_dict(self):
"""Fills dictionary of the most recently computed camera extrinsics with the camera source.
The initial boolean indicates if the extrinsics guess should be used."""
camera_to_ext_guess = dict()
for src in self._source_names:
#Dictionary values: use_extrinsics_guess flag, (rvec, tvec) tuple
camera_to_ext_guess[src] = (False, (None, None))
return camera_to_ext_guess
def start(self):
"""Claim lease of robot and start the fiducial follower."""
self._lease = self._lease_client.acquire()
self._estop_endpoint.force_simple_setup()
self._robot.time_sync.wait_for_sync()
with bosdyn.client.lease.LeaseKeepAlive(self._lease_client) as self._lease_keepalive, \
bosdyn.client.estop.EstopKeepAlive(self._estop_endpoint) as self._estop_keepalive:
#stand the robot up
if self._standup:
self.power_on()
self.stand()
#delay grabbing image until spot is standing (or close enough to upright)
time.sleep(.35)
stop_spot = self.check_num_attempts()
self.populate_source_dict()
while not stop_spot:
#grab a raw image from spot's camera
bboxes, source_name = self.image_to_bounding_box()
if bboxes:
self._previous_source = source_name
(tvec, _, source_name) = self.get_position_lib(
bboxes, self._intrinsics, source_name)
else:
self._tag_not_located = True
print("No bounding boxes found")
#Go to the tag and stop within a certain distance
if not self._tag_not_located:
self.go_to_tag(tvec, source_name)
self._attempts += 1 #increment attempts at finding bounding boxes
#stop iterating through bbox detection+movement when close to tag
stop_spot = self.check_num_attempts()
if self._powered_on:
self.power_off()
def power_on(self):
"""Power on the robot."""
self._robot.power_on()
self._powered_on = True
print("Powered On " + str(self._robot.is_powered_on()))
def power_off(self):
"""Power off the robot."""
safe_power_off_cmd = RobotCommandBuilder.safe_power_off_command()
self._robot_command_client.robot_command(lease=None,
command=safe_power_off_cmd)
time.sleep(2.5)
print("Powered Off " + str(not self._robot.is_powered_on()))
def image_to_bounding_box(self):
"""Determine which camera source has a fiducial.
Return the bounding box of the first detected fiducial."""
#Iterate through all five camera sources to check for a fiducial
for i in range(len(self._source_names) + 1):
#Get the image from the source camera
if i == 0:
if self._previous_source != None:
source_name = self._previous_source
else:
continue
elif self._source_names[i - 1] == self._previous_source:
continue
else:
source_name = self._source_names[i - 1]
img_req = build_image_request(source_name,
quality_percent=100,
image_format=bosdyn_image.FORMAT_RAW)
image_response = self._image_client.get_image([img_req])
#transformation between camera frame to body frame
self._camera_T_body = image_response[
0].shot.sample.frame.base_tform_offset
#transformation between body to world
self._body_T_world = image_response[0].ko_tform_body
#Camera intrinsics for the given source camera
self._intrinsics = image_response[0].source.pinhole.intrinsics
width = image_response[0].shot.image.cols
height = image_response[0].shot.image.rows
# detect given fiducial in image and return the bounding box of it
bboxes = self.find_image_tag_lib(image_response[0].shot.image,
(width, height), source_name)
if bboxes:
return bboxes, source_name
else:
self._tag_not_located = True
print("Failed to find bounding box for " + str(source_name))
return [], None
def find_image_tag_lib(self, image, dim, source_name):
"""Detect the fiducial within a single image and return its bounding box."""
image_grey = np.array(
Image.frombytes('P', (int(dim[0]), int(dim[1])),
data=image.data,
decoder_name='raw'))
#Rotate each image such that it is upright
image_grey = self.rotate_image(image_grey, source_name)
#Make the image greyscale to use bounding box detections
detector = apriltag(family="tag36h11", blur=self.blur)
detections = detector.detect(image_grey)
bboxes = []
for i in range(len(detections)):
bbox = detections[i]['lb-rb-rt-lt']
cv2.polylines(image_grey, [np.int32(bbox)], True, (0, 0, 0), 2)
bboxes.append(bbox)
self._image[source_name] = image_grey
if self._debug and bboxes and not self._display_images:
cv2.imshow('found', image_grey)
cv2.waitKey()
cv2.destroyAllWindows()
return bboxes
def bbox_to_img_obj_pts(self, bbox):
"""Determine the object points and image points for the bounding box.
The origin in object coordinates = top left corner of the fiducial.
Order both points sets following: (TL,TR, BL, BR)"""
obj_pts = np.array(
[[0, 0], [self._fiducial_width_mm, 0],
[0, self._fiducial_height_mm],
[self._fiducial_width_mm,
self._fiducial_height_mm]]).reshape(bbox.shape[0], 1, 2)
#insert a 1 as the third coordinate (xyz)
obj_points = np.insert(obj_pts, 2, 0,
axis=2).reshape(obj_pts.shape[0], 3, 1)
#['lb-rb-rt-lt']
img_pts = np.array([[bbox[3][0], bbox[3][1]], [bbox[2][0], bbox[2][1]],
[bbox[0][0], bbox[0][1]],
[bbox[1][0],
bbox[1][1]]]).reshape(bbox.shape[0], 2, 1)
return obj_points, img_pts
def get_position_lib(self, bbox, ints, source_name):
"""Compute transformation of 2d pixel coordinates to 3d camera coordinates."""
camera = self.make_cam_mat(ints)
best_bbox = (None, None, source_name)
closest_dist = 1000
for i in range(len(bbox)):
obj_points, img_points = self.bbox_to_img_obj_pts(bbox[i])
if self._cam_to_ext_guess[source_name][0]:
# initialize the position estimate with the previous extrinsics solution
# then iteratively solve for new position
old_rvec, old_tvec = self._cam_to_ext_guess[source_name][1]
_, rvec, tvec = cv2.solvePnP(obj_points, img_points, camera,
np.zeros(
(5, 1)), old_rvec, old_tvec,
True, cv2.SOLVEPNP_ITERATIVE)
else:
# Determine current extrinsic solution for the tag
_, rvec, tvec = cv2.solvePnP(obj_points, img_points, camera,
np.zeros((5, 1)))
#Save extrinsics results to help speed up next attempts to locate bounding box
self._cam_to_ext_guess[source_name] = (True, (rvec, tvec))
if self._debug:
self.back_prop_error(obj_points, img_points, rvec, tvec,
camera)
cv2.drawFrameAxes(self._image[source_name], camera,
np.zeros((5, 1)), rvec, tvec, 100)
if not self._display_images:
cv2.imshow('frame', self._image[source_name])
cv2.waitKey()
cv2.destroyAllWindows()
dist = (float(tvec[0][0])**2 + float(tvec[1][0])**2 +
float(tvec[2][0])**2)**(.5) / 1000.0
if dist < closest_dist:
closest_dist = dist
best_bbox = (tvec, rvec, source_name)
# Flag indicating if the best april tag been found/located
self._tag_not_located = best_bbox[0] is None and best_bbox[1] is None
return best_bbox
def offset_tag_pose(self, tag_position):
"""Offset the location of the fiducial to keep a buffer of how close the robot gets.
Converted in body frame so that world location is accurate."""
if tag_position[0] < 0:
offset_x = tag_position[0] + self._dist_x_desired
else:
offset_x = tag_position[0] - self._dist_x_desired
if tag_position[1] < 0:
offset_y = tag_position[1] + self._dist_y_desired
else:
offset_y = tag_position[1] - self._dist_y_desired
return np.array([offset_x, offset_y, tag_position[2]])
def go_to_tag(self, tvec, source_name):
"""Transform the fiducial position to the world frame (kinematic odometry frame)
Command the robot to move to this position."""
#Transform the tag position from camera coordinates to world coordinates
tag_pose_in_camera = np.array([
float(tvec[0][0]) / 1000.0,
float(tvec[1][0]) / 1000.0,
float(tvec[2][0]) / 1000.0
])
tag_pose_in_body = self.transform_to_frame(self._camera_T_body,
tag_pose_in_camera)
tag_pose_body_offset = self.offset_tag_pose(tag_pose_in_body)
self._current_tag_world_pose = self.transform_to_frame(
self._body_T_world, tag_pose_body_offset)
#Get the robot's current position in the world
robot_state = self.robot_state.kinematic_state.ko_tform_body
robot_angle = self.quat_to_euler(
(robot_state.rotation.x, robot_state.rotation.y,
robot_state.rotation.z, robot_state.rotation.w))[2]
#Compute the heading angle to turn the robot to face the tag
self._angle_desired = self.get_desired_angle(robot_angle,
robot_state.position)
if self._debug:
print("Camera: " + str(source_name))
print("Tag pose in camera", tag_pose_in_camera)
print("Tag pose in body", tag_pose_in_body)
print("Tag pose in body offsetted", tag_pose_body_offset)
print("Tag pose in ko", self._current_tag_world_pose)
print("Robot Pose in ko", robot_state.position)
print("Robot heading Angle", robot_angle)
print("Desired heading angle", self._angle_desired)
#Command the robot to go to the tag in kinematic odometry frame
frame_name = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_KO)
mobility_params = self.set_mobility_params()
tag_cmd = RobotCommandBuilder.trajectory_command(
goal_x=self._current_tag_world_pose[0],
goal_y=self._current_tag_world_pose[1],
goal_heading=self._angle_desired,
frame=frame_name,
params=mobility_params,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO)
end_time = 5.0
if self._movement_on:
#Issue the command to the robot
self._robot_command_client.robot_command(
lease=None,
command=tag_cmd,
end_time_secs=time.time() + end_time)
# #Feedback to check and wait until the robot is in the desired position or timeout
start_time = time.time()
current_time = time.time()
while (not self.final_state()
and current_time - start_time < end_time):
time.sleep(.25)
current_time = time.time()
return
def final_state(self):
"""Check if the current robot state is within range of the fiducial position."""
robot_state = self.robot_state.kinematic_state.ko_tform_body
robot_pose = robot_state.position
robot_angle = self.quat_to_euler(
(robot_state.rotation.x, robot_state.rotation.y,
robot_state.rotation.z, robot_state.rotation.w))[2]
if self._current_tag_world_pose.size != 0:
x_dist = abs(self._current_tag_world_pose[0] - robot_pose.x)
y_dist = abs(self._current_tag_world_pose[1] - robot_pose.y)
angle = abs(self._angle_desired - robot_angle)
if ((x_dist < self._x_eps) and (y_dist < self._y_eps)
and (angle < self._angle_eps)):
return True
return False
def check_num_attempts(self):
"""Check if the number of attempts is within the bounds."""
return self._attempts >= self._max_attempts
def stand(self):
"""Stand the robot."""
blocking_stand(self._robot_command_client)
def get_desired_angle(self, robot_angle, robot_state):
"""Compute heading using the offset angle of a vector to the apriltag"""
vec_to_tag = np.array([
self._current_tag_world_pose[0] - robot_state.x,
self._current_tag_world_pose[1] - robot_state.y
])
vec_of_robot = np.array([math.cos(robot_angle), math.sin(robot_angle)])
rotate_ang = self.angle_bn_vectors(vec_to_tag, vec_of_robot)
side_sign = np.cross(vec_to_tag, vec_of_robot)
if side_sign > 0:
#angle is to the right, so make it negative when transforming the current robot angle
rotate = -1.0 * rotate_ang
else:
rotate = rotate_ang
desired_angle = self.wrap_angle_back(robot_angle + rotate)
return desired_angle
def transform_to_frame(self, frame, tag_position):
""" Transform the tag position into the inputted coordinate frame"""
rot_mat = self.quat_to_rotmat((frame.rotation.x, frame.rotation.y,
frame.rotation.z, frame.rotation.w))
frame_pose = np.array(
[frame.position.x, frame.position.y, frame.position.z])
tf_tag_pose = frame_pose + np.matmul(rot_mat, tag_position)
return tf_tag_pose
def set_mobility_params(self):
"""Set robot mobility params to disable obstacle avoidance."""
obstacles = spot_command_pb2.ObstacleParams(
disable_vision_body_obstacle_avoidance=True,
disable_vision_foot_obstacle_avoidance=True,
disable_vision_foot_constraint_avoidance=True,
obstacle_avoidance_padding=.001)
body_control = self.set_default_body_control()
if self._limit_speed:
speed_limit = SE2VelocityLimit(max_vel=SE2Velocity(
linear=Vec2(x=self._max_x_vel, y=self._max_y_vel),
angular=self._max_ang_vel))
if not self._avoid_obstacles:
mobility_params = spot_command_pb2.MobilityParams(
obstacle_params=obstacles,
vel_limit=speed_limit,
body_control=body_control,
locomotion_hint=spot_command_pb2.HINT_AUTO)
else:
mobility_params = spot_command_pb2.MobilityParams(
vel_limit=speed_limit,
body_control=body_control,
locomotion_hint=spot_command_pb2.HINT_AUTO)
elif not self._avoid_obstacles:
mobility_params = spot_command_pb2.MobilityParams(
obstacle_params=obstacles,
body_control=body_control,
locomotion_hint=spot_command_pb2.HINT_AUTO)
else:
#When set to none, RobotCommandBuilder populates with good default values
mobility_params = None
return mobility_params
@staticmethod
def set_default_body_control():
"""Set default body control params to current body position"""
footprint_R_body = geometry.EulerZXY()
position = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
rotation = footprint_R_body.to_quaternion()
pose = geometry_pb2.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY)
traj = trajectory_pb2.SE3Trajectory(points=[point], frame=frame)
return spot_command_pb2.BodyControlParams(
base_offset_rt_footprint=traj)
@staticmethod
def rotate_image(image, source_name):
"""Rotate the image so that it is always displayed upright."""
if source_name == "frontleft_fisheye_image":
image = cv2.rotate(image, rotateCode=0)
elif source_name == "right_fisheye_image":
image = cv2.rotate(image, rotateCode=1)
elif source_name == "frontright_fisheye_image":
image = cv2.rotate(image, rotateCode=0)
return image
@staticmethod
def back_prop_error(obj_points, img_points, rvec, tvec, ints):
"""Determine the error [in pixel space] of the points projected
back to image frame using the new transformation."""
projected = cv2.projectPoints(obj_points, rvec, tvec, ints,
np.zeros((5, 1)))[0]
err = []
for i in range(len(obj_points)):
dx = img_points[i][0][0] - projected[i][0][0]
dy = img_points[i][1][0] - projected[i][0][1]
error = (dx**2 + dy**2)**(.5)
err.append(error)
avg_error = np.sum(np.array(err)) / len(err)
print("Average Error in pixel space", avg_error)
return avg_error
@staticmethod
def wrap_angle_back(desired_angle):
"""Wrap an angle value (radians) around at +/- pi to match the robot's frame."""
if desired_angle > math.pi:
desired_angle = -math.pi + desired_angle % math.pi
elif desired_angle < -math.pi:
desired_angle = desired_angle % math.pi
return desired_angle
@staticmethod
def angle_bn_vectors(vec1, vec2):
"""Compute the angle between two vectors."""
vec1 /= np.linalg.norm(vec1)
vec2 /= np.linalg.norm(vec2)
dot_pdt = vec1[0] * vec2[0] + vec1[1] * vec2[1]
return math.acos(dot_pdt)
@staticmethod
def make_cam_mat(ints):
"""Transform the ImageResponse proto intrinsics into a camera matrix."""
camera_matrix = np.array(
[[ints.focal_length.x, ints.skew.x, ints.principal_point.x],
[ints.skew.y, ints.focal_length.y, ints.principal_point.y],
[0, 0, 1]])
return camera_matrix
@staticmethod
def quat_to_rotmat(q):
"""Convert a quaternion into a rotation matrix."""
rm00 = 1 - 2 * q[1]**2 - 2 * q[2]**2
rm01 = 2 * q[0] * q[1] - 2 * q[2] * q[3]
rm02 = 2 * q[0] * q[2] + 2 * q[1] * q[3]
rm10 = 2 * q[0] * q[1] + 2 * q[2] * q[3]
rm11 = 1 - 2 * q[0]**2 - 2 * q[2]**2
rm12 = 2 * q[1] * q[2] - 2 * q[0] * q[3]
rm20 = 2 * q[0] * q[2] - 2 * q[1] * q[3]
rm21 = 2 * q[1] * q[2] + 2 * q[0] * q[3]
rm22 = 1 - 2 * q[0]**2 - 2 * q[1]**2
return np.array([[rm00, rm01, rm02], [rm10, rm11, rm12],
[rm20, rm21, rm22]])
@staticmethod
def rotmat_to_quat(mat):
"""Convert a rotation matrix into a quaternion."""
w = math.sqrt(1 + mat[0][0] + mat[1][1] + mat[2][2]) / 2.0
x = (mat[2][1] - mat[1][2]) / (4.0 * w)
y = (mat[0][2] - mat[2][0]) / (4.0 * w)
z = (mat[1][0] - mat[0][1]) / (4.0 * w)
return (x, y, z, w)
@staticmethod
def quat_to_euler(q):
"""Convert a quaternion to xyz Euler angles."""
roll = math.atan2(2 * q[3] * q[0] + q[1] * q[2],
1 - 2 * q[0]**2 + 2 * q[1]**2)
pitch = math.atan2(2 * q[1] * q[3] - 2 * q[0] * q[2],
1 - 2 * q[1]**2 - 2 * q[2]**2)
yaw = math.atan2(2 * q[2] * q[3] + 2 * q[0] * q[1],
1 - 2 * q[1]**2 - 2 * q[2]**2)
return roll, pitch, yaw
def stop(self):
"""Clean shutdown for the Fiducial Follower."""
#Power off(safely) the motors if they were turned on
if self._powered_on:
self.power_off()
#Stop the estop keepalive and the lease keepalive
if self._estop_keepalive is not None:
self._estop_keepalive.stop()
self._estop_keepalive.shutdown()
if self._lease_keepalive is not None:
if self._lease_keepalive.is_alive():
self._lease_keepalive.shutdown()
if self._lease:
try:
self._lease_client.return_lease(self._lease)
except (ResponseError, RpcError) as err:
LOGGER.error("Failed %s: %s", "Return lease", err)
#Stop time sync
self._robot.time_sync.stop()
