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 _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 _create_new_edge(self, *args):
"""Create new edge between existing waypoints in map."""
if len(args[0]) != 2:
print(
"ERROR: Specify the two waypoints to connect (short code or annotation)."
)
return
if self._current_graph is None:
self._current_graph = self._graph_nav_client.download_graph()
from_id = graph_nav_util.find_unique_waypoint_id(
args[0][0], self._current_graph,
self._current_annotation_name_to_wp_id)
to_id = graph_nav_util.find_unique_waypoint_id(
args[0][1], self._current_graph,
self._current_annotation_name_to_wp_id)
print("Creating edge from {} to {}.".format(from_id, to_id))
from_wp = self._get_waypoint(from_id)
if from_wp is None:
return
to_wp = self._get_waypoint(to_id)
if to_wp is None:
return
# Get edge transform based on kinematic odometry
edge_transform = self._get_transform(from_wp, to_wp)
# Define new edge
new_edge = map_pb2.Edge()
new_edge.id.from_waypoint = from_id
new_edge.id.to_waypoint = to_id
new_edge.from_tform_to.CopyFrom(edge_transform)
print("edge transform =", new_edge.from_tform_to)
# Send request to add edge to map
self._recording_client.create_edge(edge=new_edge)
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 keep-alive and create a new sublease for graph nav.
self._lease = self._lease_wallet.advance()
sublease = self._lease.create_sublease()
self._lease_keepalive.shutdown()
nav_to_cmd_id = None
# 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).
try:
nav_to_cmd_id = self._graph_nav_client.navigate_to(
destination_waypoint,
1.0,
leases=[sublease.lease_proto],
command_id=nav_to_cmd_id)
except ResponseError as e:
print("Error while navigating {}".format(e))
break
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)