def test_lease_keep_alive_shutdown():
# Tests that shutdown will stop the loop.
lease_wallet = LeaseWallet()
lease_client = MockLeaseClient(lease_wallet)
keep_alive = LeaseKeepAlive(lease_client, resource='A', rpc_interval_seconds=.1)
assert keep_alive.is_alive()
time.sleep(.5)
assert keep_alive.is_alive()
keep_alive.shutdown()
assert not keep_alive.is_alive()
# A second shutdown should also work, even if it is a no-op.
keep_alive.shutdown()
assert not keep_alive.is_alive()
class GraphNavInterface(object):
"""GraphNav service command line interface."""
def __init__(self, robot, upload_path):
self._robot = robot
# Force trigger timesync.
self._robot.time_sync.wait_for_sync()
# Create the lease client with keepalive, then take the lease.
self._lease_client = self._robot.ensure_client(
LeaseClient.default_service_name)
self._lease_wallet = self._lease_client.lease_wallet
self._lease = self._lease_client.take()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
# Create robot state and command clients.
self._robot_command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
self._robot_state_client = self._robot.ensure_client(
RobotStateClient.default_service_name)
# Create the client for the Graph Nav main service.
self._graph_nav_client = self._robot.ensure_client(
GraphNavClient.default_service_name)
# Create a power client for the robot.
self._power_client = self._robot.ensure_client(
PowerClient.default_service_name)
# 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
# Number of attempts to wait before trying to re-power on.
self._max_attempts_to_wait = 50
# 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()
# Filepath for uploading a saved graph's and snapshots too.
if upload_path[-1] == "/":
self._upload_filepath = upload_path[:-1]
else:
self._upload_filepath = upload_path
self._command_dictionary = {
'1': self._get_localization_state,
'2': self._set_initial_localization_fiducial,
'3': self._set_initial_localization_waypoint,
'4': self._list_graph_waypoint_and_edge_ids,
'5': self._upload_graph_and_snapshots,
'6': self._navigate_to,
'7': self._navigate_route,
'8': self._clear_graph
}
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)
def _upload_graph_and_snapshots(self, *args):
"""Upload the graph and snapshots to the robot."""
print("Loading the graph from disk into local storage...")
with open(self._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(
self._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(
self._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
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)
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
def _on_quit(self):
"""Cleanup on quit from the command line interface."""
# Sit the robot down + power off after the navigation command is complete.
if self._powered_on and not self._started_powered_on:
self._robot_command_client.robot_command(
RobotCommandBuilder.safe_power_off_command(),
end_time_secs=time.time())
def run(self):
"""Main loop for the command line interface."""
while True:
print("""
Options:
(1) Get localization state.
(2) Initialize localization to the nearest fiducial (must be in sight of a fiducial).
(3) Initialize localization to a specific waypoint (must be exactly at the waypoint).
(4) List the waypoint ids and edge ids of the map on the robot.
(5) Upload the graph and its snapshots.
(6) Navigate to. The destination waypoint id is the second argument.
(7) Navigate route. The (in-order) waypoint ids of the route are the arguments.
(8) Clear the current graph.
(q) Exit.
""")
try:
inputs = input('>')
except NameError:
pass
req_type = str.split(inputs)[0]
if req_type == 'q':
self._on_quit()
break
if req_type not in self._command_dictionary:
print("Request not in the known command dictionary.")
continue
try:
cmd_func = self._command_dictionary[req_type]
cmd_func(str.split(inputs)[1:])
except Exception as e:
print(e)
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 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)
try:
self._estop_client = self._robot.ensure_client(
EstopClient.default_service_name)
self._estop_endpoint = EstopEndpoint(self._estop_client,
'GNClient', 9.0)
except:
# Not the estop.
self._estop_client = None
self._estop_endpoint = None
self._power_client = robot.ensure_client(
PowerClient.default_service_name)
self._robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
self._robot_state_task = AsyncRobotState(self._robot_state_client)
self._image_task = AsyncImageCapture(robot)
self._async_tasks = AsyncTasks(
[self._robot_state_task, self._image_task])
self._lock = threading.Lock()
self._command_dictionary = {
ord('\t'): self._quit_program,
ord('T'): self._toggle_time_sync,
ord(' '): self._toggle_estop,
ord('r'): self._self_right,
ord('P'): self._toggle_power,
ord('v'): self._sit,
ord('b'): self._battery_change_pose,
ord('f'): self._stand,
ord('w'): self._move_forward,
ord('s'): self._move_backward,
ord('a'): self._strafe_left,
ord('d'): self._strafe_right,
ord('q'): self._turn_left,
ord('e'): self._turn_right,
ord('I'): self._image_task.take_image,
ord('O'): self._image_task.toggle_video_mode,
ord('l'): self._toggle_lease
}
self._locked_messages = ['', '',
''] # string: displayed message for user
self._estop_keepalive = None
self._exit_check = None
# Stuff that is set in start()
self._robot_id = None
self._lease = None
self._lease_keepalive = None
def start(self):
"""Begin communication with the robot."""
self._lease = self._lease_client.acquire()
# Construct our lease keep-alive object, which begins RetainLease calls in a thread.
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
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.
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 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 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, self._lease_keepalive)
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)
raise
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.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())
for i in range(3):
stdscr.addstr(7 + i, 2, self.message(i))
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,
" [f]: Stand, [r]: Self-right ")
stdscr.addstr(14, 0,
" [v]: Sit, [b]: Battery-change ")
stdscr.addstr(15, 0,
" [wasd]: Directional strafing ")
stdscr.addstr(16, 0,
" [qe]: Turning ")
stdscr.addstr(17, 0,
" [l]: Return/Acquire lease ")
stdscr.addstr(18, 0, "")
# 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 + 17 >= max_y:
break
stdscr.addstr(y_i + 17, 0, 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, LeaseBaseError) as err:
self.add_message("Failed {}: {}".format(desc, err))
return None
def _try_grpc_async(self, desc, thunk):
def on_future_done(fut):
try:
fut.result()
except (ResponseError, RpcError, LeaseBaseError) as err:
self.add_message("Failed {}: {}".format(desc, err))
return None
future = thunk()
future.add_done_callback(on_future_done)
def _quit_program(self):
self._sit()
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.start_time_sync()
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 _toggle_lease(self):
"""toggle lease acquisition. Initial state is acquired"""
if self._lease_client is not None:
if self._lease_keepalive is None:
self._lease = self._lease_client.acquire()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
else:
self._lease_client.return_lease(self._lease)
self._lease_keepalive.shutdown()
self._lease_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 _battery_change_pose(self):
# Default HINT_RIGHT, maybe add option to choose direction?
self._start_robot_command(
'battery_change_pose',
RobotCommandBuilder.battery_change_pose_command(
dir_hint=basic_command_pb2.BatteryChangePoseCommand.Request.
HINT_RIGHT))
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 _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 _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 _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_async("powering-on", self._request_power_on)
else:
self._try_grpc("powering-off", self._safe_power_off)
def _request_power_on(self):
request = PowerServiceProto.PowerCommandRequest.REQUEST_ON
return self._power_client.power_command_async(request)
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 = '??'
lease = '??'
if lease_keep_alive is None:
alive = 'STOPPED'
lease = 'RETURNED'
else:
if self._lease:
lease = '{}:{}'.format(self._lease.lease_proto.resource,
self._lease.lease_proto.sequence)
else:
lease = '...'
if lease_keep_alive.is_alive():
alive = 'RUNNING'
else:
alive = 'STOPPED'
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 _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 main(argv):
"""Command line interface.
Args:
argv: List of command-line arguments passed to the program.
"""
parser = argparse.ArgumentParser()
parser.add_argument(
"--model-path",
required=True,
help=
"Local file path to the Tensorflow model, example pre-trained models \
can be found at \
https://github.com/tensorflow/models/blob/master/research/object_detection/g3doc/detection_model_zoo.md"
)
parser.add_argument(
"--classes",
default='/classes.json',
type=str,
help="File containing json mapping of object class IDs to class names")
parser.add_argument(
"--number-tensorflow-processes",
default=1,
type=int,
help="Number of Tensorflow processes to run in parallel")
parser.add_argument(
"--detection-threshold",
default=0.7,
type=float,
help="Detection threshold to use for Tensorflow detections")
parser.add_argument(
"--sleep-between-capture",
default=1.0,
type=float,
help=
"Seconds to sleep between each image capture loop iteration, which captures "
+ "an image from all cameras")
parser.add_argument(
"--detection-class",
default=1,
type=int,
help="Detection classes to use in the" +
"Tensorflow model; Default is to use 1, which is a person in the Coco dataset"
)
parser.add_argument(
"--max-processing-delay",
default=7.0,
type=float,
help="Maximum allowed delay for processing an image; " +
"any image older than this value will be skipped")
bosdyn.client.util.add_common_arguments(parser)
options = parser.parse_args(argv)
try:
# Make sure the model path is a valid file
if not _check_model_path(options.model_path):
return False
# Check for classes json file, otherwise use the COCO class dictionary
_check_and_load_json_classes(options.classes)
global TENSORFLOW_PROCESS_BARRIER # pylint: disable=global-statement
TENSORFLOW_PROCESS_BARRIER = Barrier(
options.number_tensorflow_processes + 1)
# Start Tensorflow processes
start_tensorflow_processes(options.number_tensorflow_processes,
options.model_path, options.detection_class,
options.detection_threshold,
options.max_processing_delay)
# sleep to give the Tensorflow processes time to initialize
try:
TENSORFLOW_PROCESS_BARRIER.wait()
except BrokenBarrierError as exc:
print(
f'Error waiting for Tensorflow processes to initialize: {exc}')
return False
# Start the API related things
# Create robot object with a world object client
sdk = bosdyn.client.create_standard_sdk('SpotFollowClient')
robot = sdk.create_robot(options.hostname)
robot.authenticate(options.username, options.password)
#Time sync is necessary so that time-based filter requests can be converted
robot.time_sync.wait_for_sync()
# Verify the robot is not estopped and that an external application has registered and holds
# an estop endpoint.
assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client," \
" such as the estop SDK example, to configure E-Stop."
# Create the sdk clients
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
lease_client = robot.ensure_client(LeaseClient.default_service_name)
image_client = robot.ensure_client(ImageClient.default_service_name)
source_list = get_source_list(image_client)
image_task = AsyncImage(image_client, source_list)
robot_state_task = AsyncRobotState(robot_state_client)
task_list = [image_task, robot_state_task]
_async_tasks = AsyncTasks(task_list)
print('Detect and follow client connected.')
lease = lease_client.acquire()
lease_keep = LeaseKeepAlive(lease_client)
# Power on the robot and stand it up
resp = robot.power_on()
try:
blocking_stand(robot_command_client)
except CommandFailedError as exc:
print(
f'Error ({exc}) occurred while trying to stand. Check robot surroundings.'
)
return False
except CommandTimedOutError as exc:
print(f'Stand command timed out: {exc}')
return False
print('Robot powered on and standing.')
params_set = get_mobility_params()
# This thread starts the async tasks for image and robot state retrieval
update_thread = Thread(target=_update_thread, args=[_async_tasks])
update_thread.daemon = True
update_thread.start()
# Wait for the first responses.
while any(task.proto is None for task in task_list):
time.sleep(0.1)
# Start image capture process
image_capture_thread = Thread(target=capture_images,
args=(
image_task,
options.sleep_between_capture,
))
image_capture_thread.start()
while True:
# This comes from the tensorflow processes and limits the rate of this loop
entry = PROCESSED_BOXES_QUEUE.get()
# find the highest confidence bounding box
highest_conf_source = _find_highest_conf_source(entry)
if highest_conf_source is None:
# no boxes or scores found
continue
capture_to_use = entry[highest_conf_source]
raw_time = capture_to_use['raw_image_time']
time_gap = time.time() - raw_time
if time_gap > options.max_processing_delay:
continue # Skip image due to delay
# Find the transform to highest confidence object using the depth sensor
world_tform_object = get_object_position(
capture_to_use['world_tform_cam'],
capture_to_use['visual_image'], capture_to_use['depth_image'],
capture_to_use['boxes'][0],
ROTATION_ANGLES[capture_to_use['source']])
# get_object_position can fail if there is insufficient depth sensor information
if not world_tform_object:
continue
scores = capture_to_use['scores']
print(
f'Transform for object with confidence {scores[0]}: {world_tform_object}'
)
print(
f'Process latency: {time.time() - capture_to_use["system_cap_time"]}'
)
tag_cmd = get_go_to(world_tform_object, robot_state_task.proto,
params_set)
end_time = 15.0
if tag_cmd is not None:
robot_command_client.robot_command(lease=None,
command=tag_cmd,
end_time_secs=time.time() +
end_time)
# Shutdown lease keep-alive and return lease gracefully.
lease_keep.shutdown()
lease_client.return_lease(lease)
return True
except Exception as exc: # pylint: disable=broad-except
LOGGER.error("Spot Tensorflow Detector threw an exception: %s", exc)
# Shutdown lease keep-alive and return lease gracefully.
lease_keep.shutdown()
lease_client.return_lease(lease)
return False
class Spot:
app_name = "twitch_client"
__robot = None
__sdk = None
__lease_client = None
__power_client = None
__estop_client = None
__state_client = None
__command_client = None
__estop_endpoint = None
__estop_keepalive = None
__lease = None
__lease_keep_alive = None
movement_helper = None
image_helper = None
def __init__(self, config):
logging.info(
f"Creating new robot instance for {config.name} {config.host}")
self.hostname = config.host
self.client_name = config.name
self.guid = config.guid
self.secret = config.secret
def connect(self, cb=None, retry=False):
self.__sdk = create_standard_sdk(self.app_name)
try:
self.__robot = self.__sdk.create_robot(self.hostname)
logging.info(f"Authenticating payload with guid {self.guid}")
self.__robot.authenticate_from_payload_credentials(
guid=self.guid, secret=self.secret)
logging.info("Starting time sync")
self.__robot.start_time_sync()
self.__preflight()
if cb is not None:
cb()
except RpcError:
logging.error(
f"Could not connect with robot using {self.hostname}")
if retry:
logging.info(f"Retrying using {self.hostname}")
self.connect(cb, retry)
except InvalidPayloadCredentialsError:
logging.error(f"Invalid guid '{self.guid}' or secret")
except Exception as exc:
logging.error(exc)
def __preflight(self):
logging.info("Ensuring clients")
self.__lease_client = self.__robot.ensure_client(
lease.LeaseClient.default_service_name)
self.__power_client = self.__robot.ensure_client(
power.PowerClient.default_service_name)
self.__state_client = self.__robot.ensure_client(
robot_state.RobotStateClient.default_service_name)
self.__estop_client = self.__robot.ensure_client(
estop.EstopClient.default_service_name)
self.__command_client = self.__robot.ensure_client(
robot_command.RobotCommandClient.default_service_name)
self.__image_client = self.__robot.ensure_client(
image.ImageClient.default_service_name)
logging.info("Initializing movement helper")
self.movement_helper = MovementHelper(self.__command_client)
logging.info("Initializing image helper")
self.image_helper = ImageViewer(self.__image_client)
def get_battery_level(self):
return self.__state_client.get_robot_state(
).battery_states[0].charge_percentage.value
def enable_movement(self):
if self.__lease is None:
logging.info("Acquiring lease")
self.__lease = self.__lease_client.take()
self.__lease_keep_alive = LeaseKeepAlive(self.__lease_client)
if self.__estop_endpoint is None:
logging.info("Creating estop endpoint")
self.__estop_endpoint = estop.EstopEndpoint(
client=self.__estop_client,
name='mt-node-payload',
estop_timeout=9)
self.__estop_endpoint.force_simple_setup()
self.__estop_keepalive = EstopKeepAlive(self.__estop_endpoint)
try:
logging.info("Powering motors")
power.power_on(self.__power_client)
except BatteryMissingError:
logging.error("Battery missing")
def disable_movement(self):
logging.info("Depowering motors")
power.power_off(self.__power_client)
if self.__lease is not None:
logging.info("Releasing lease")
self.__lease_keep_alive.shutdown()
self.__lease_client.return_lease(self.__lease)
self.__lease = None
if self.__estop_endpoint is not None:
logging.info("Releasing estop")
self.__estop_keepalive.stop()
self.__estop_keepalive = None
self.__estop_endpoint.stop()
self.__estop_endpoint = None
