bosdyn-client 3.3.2__py3-none-any.whl → 4.0.1__py3-none-any.whl

bosdyn/client/frame_helpers.py

@@ -32,6 +32,7 @@ FOOT_FRAME_NAMES = [
     FRONT_LEFT_FOOT_FRAME_NAME, FRONT_RIGHT_FOOT_FRAME_NAME, HIND_LEFT_FOOT_FRAME_NAME,
     HIND_RIGHT_FOOT_FRAME_NAME
 ]
+WR1_FRAME_NAME = "arm0.link_wr1"
 
 
 class Error(Exception):
@@ -178,7 +179,7 @@ def get_se2_a_tform_b(frame_tree_snapshot, frame_a, frame_b, validate=True):
 
     Returns:
         math_helpers.SE2Pose between frame_a and frame_b if they exist in the tree and
-        frame a is a gravity aligned frame. None otherwise.
+        frame_a is a gravity aligned frame. None otherwise.
     """
     # Validate that the transform is in a gravity aligned frame based on the string name.
     if not is_gravity_aligned_frame_name(frame_a):
@@ -274,6 +275,7 @@ class ChildFrameInTree(GenerateTreeError):
 
 def add_edge_to_tree(frame_tree_snapshot, parent_tform_child, parent_frame_name, child_frame_name):
     """Appends a child/parent and the transform to the FrameTreeSnapshot.
+
        Args:
             frame_tree_snapshot (dict) dictionary representing the child_to_parent_edge_map
             parent_tform_child (SE3Pose proto)

bosdyn/client/gps/NMEAParser.py

@@ -0,0 +1,189 @@
+# Copyright (c) 2023 Boston Dynamics, Inc.  All rights reserved.
+#
+# Downloading, reproducing, distributing or otherwise using the SDK Software
+# is subject to the terms and conditions of the Boston Dynamics Software
+# Development Kit License (20191101-BDSDK-SL).
+
+import datetime
+import math
+import time
+from typing import List, Tuple
+
+import pynmea2
+from google.protobuf.any_pb2 import Any
+from google.protobuf.timestamp_pb2 import Timestamp
+
+from bosdyn.api.gps.gps_pb2 import GpsDataPoint
+from bosdyn.util import RobotTimeConverter, now_timestamp, seconds_to_timestamp
+
+
+has_warned_no_zda = False
+
+
+class NMEAParser(object):
+
+    # The amount of time to wait before logging another decode error.
+    LOG_THROTTLE_TIME = 2.0  # seconds.
+
+    def __init__(self, logger):
+        self.data = ''
+        self.full_lines = []
+        self.logger = logger
+        # NMEA strings come in in "groups" we are just trying to
+        # figure out which group each message belongs to.  We do this
+        # by checking if their times are near to one another.
+        #
+        # If your GPS outputs data at 20 Hz, this constant must be less than 0.050 seconds.
+        self.grouping_timeout = 0.025
+        self.last_failed_read_log_time = None
+
+    def nmea_message_group_to_gps_data_point(self, nmea_messages: List[Tuple[str, str, int]],
+                                             time_converter: RobotTimeConverter):
+        """Convert a NMEA message group with the same UTC timestamp to a GpsDataPoint."""
+
+        global has_warned_no_zda
+
+        data_point = GpsDataPoint()
+        has_timestamp = False
+
+
+        # The NMEA message group should at least have a ZDA message.
+        # Parse each message depending on the NMEA sentence_type.
+        for nmea_message_list in nmea_messages:
+            data, raw_nmea_msg, client_timestamp = nmea_message_list
+
+            if data.sentence_type == 'GGA':
+                if data.latitude is not None and data.longitude is not None and data.altitude is not None:
+                    data_point.llh.latitude = data.latitude
+                    data_point.llh.longitude = data.longitude
+                    data_point.llh.height = data.altitude
+
+                if data.num_sats is not None and int(data.num_sats) > 0:
+                    for _ in range(int(data.num_sats)):
+                        sat = data_point.satellites.add()
+
+                # GPS Quality indicator:
+                # 0: Fix not valid
+                # 1: GPS fix
+                # 2: Differential GPS fix, OmniSTAR VBS
+                # 4: Real-Time Kinematic, fixed integers
+                # 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK
+                if data.gps_qual is not None:
+                    data_point.mode.value = data.gps_qual
+
+                if not has_timestamp:
+                    # If there is no ZDA message to provide a date, assume today's date.
+                    gps_timestamp = datetime.datetime.combine(datetime.date.today(), data.timestamp)
+                    data_point.timestamp_gps.FromDatetime(gps_timestamp)
+
+            elif data.sentence_type == 'GST':
+                if data.std_dev_latitude is not None:
+                    # Horizontal Root Mean Squared. Note we are not using "twice distance rms" or "2drms".
+                    hrms = math.sqrt((
+                        (math.pow(data.std_dev_latitude, 2) + math.pow(data.std_dev_longitude, 2)) /
+                        2))
+                    data_point.accuracy.horizontal = hrms
+                    data_point.accuracy.vertical = data.std_dev_altitude
+
+            elif data.sentence_type == 'ZDA':
+                gps_timestamp = data.datetime
+                # Protobuf timestamp does not use timezone aware timestamps.
+                gps_timestamp_no_tz = gps_timestamp.replace(tzinfo=None)
+                data_point.timestamp_gps.FromDatetime(gps_timestamp_no_tz)
+                has_timestamp = True
+
+            # Populate client and robot timestamps.
+            data_point.timestamp_client.CopyFrom(seconds_to_timestamp(client_timestamp))
+            if time_converter is not None:
+                data_point.timestamp_robot.CopyFrom(
+                    time_converter.robot_timestamp_from_local_secs(client_timestamp))
+            else:
+                data_point.timestamp_robot.CopyFrom(now_timestamp())
+
+
+        if not has_timestamp and not has_warned_no_zda:
+            self.logger.warning("GPS data does not include ZDA. Timestamp may be inaccurate.")
+            has_warned_no_zda = True
+
+
+        return data_point
+
+    def parse(self, new_data: str, time_converter: RobotTimeConverter,
+              check: bool = True) -> List[GpsDataPoint]:
+        self.data = self.data + new_data
+        timestamp = time.time()  # Client timestamp when received.
+
+        if len(self.data) == 0:
+            return []  # Protection because empty_string.splitlines() returns empty array
+
+        lines = self.data.splitlines(True)
+        self.data = lines[-1]
+        len_lines = len(lines)
+
+        # Parse each line.
+        for idx, line in enumerate(lines):
+            # Only parse the last line if it ends with a \n
+            if idx == len_lines - 1:
+                if len(line) > 0:
+                    if line[-1] == '\n':
+                        # This line is complete, any new data
+                        # will be on a new line.  Parse it.
+                        self.data = ""
+                    else:
+                        break
+                else:
+                    break
+            stripped = line.strip()
+            try:
+
+                nmea_msg = pynmea2.parse(stripped, check)
+            except KeyboardInterrupt:
+                raise
+            except Exception as e:
+                # Parsing error, log and skip.
+                # Throttle the logs.
+                now = time.time()
+                if self.last_failed_read_log_time is None or (
+                        now - self.last_failed_read_log_time) > self.LOG_THROTTLE_TIME:
+                    self.logger.exception(f"Failed to parse {stripped}. Is it NMEA?")
+                    self.last_failed_read_log_time = now
+                continue
+
+            # Only use NMEA messages that have a timestamp.
+            # For example, GSA and GST messages are not supported.
+            if isinstance(nmea_msg.timestamp, datetime.time):
+                self.full_lines.append((nmea_msg, stripped, timestamp))
+            elif isinstance(nmea_msg.timestamp, str) and nmea_msg.timestamp == '.':
+                # pynmea2 will set the timestamp to the string '.' when GPS
+                # spits out: "$GPZDA,.,,,,,00*66".  Silently ignore.
+                continue
+            elif nmea_msg.timestamp is not None:
+                self.logger.error("Invalid timestamp for \"" + stripped + "\" \"")
+                continue
+
+        found = True
+        found_subsets = []
+        # Group a subset of NMEA messages based on timestamp.
+        while found:
+            if len(self.full_lines) < 2:
+                break
+            first_time = self.full_lines[0][0].timestamp
+            found = False
+            for idx in range(1, len(self.full_lines)):
+
+                date = datetime.date(1, 1, 1)
+                datetime1 = datetime.datetime.combine(date, first_time)
+                datetime2 = datetime.datetime.combine(date, self.full_lines[idx][0].timestamp)
+
+                # Mod the total seconds by 3600 so that checking 23:59:59.99 and 00:00:00.00
+                # evaluate as close to each other.
+                time_elapsed = (datetime2 - datetime1).total_seconds() % 3600
+
+                if (time_elapsed < 0) or (time_elapsed > self.grouping_timeout):
+                    subset = self.full_lines[0:idx]
+                    self.full_lines = self.full_lines[idx:]
+                    found_subsets.append(subset)
+                    found = True
+                    break
+
+        return [self.nmea_message_group_to_gps_data_point(x, time_converter) for x in found_subsets]

bosdyn/client/gps/__init__.py

@@ -0,0 +1,6 @@
+# Copyright (c) 2023 Boston Dynamics, Inc.  All rights reserved.
+#
+# Downloading, reproducing, distributing or otherwise using the SDK Software
+# is subject to the terms and conditions of the Boston Dynamics Software
+# Development Kit License (20191101-BDSDK-SL).
+

bosdyn/client/gps/aggregator_client.py

@@ -0,0 +1,56 @@
+# Copyright (c) 2023 Boston Dynamics, Inc.  All rights reserved.
+#
+# Downloading, reproducing, distributing or otherwise using the SDK Software
+# is subject to the terms and conditions of the Boston Dynamics Software
+# Development Kit License (20191101-BDSDK-SL).
+
+"""For clients to use the Gps Aggregator service."""
+import collections
+
+from bosdyn.api.gps import aggregator_pb2, aggregator_service_pb2_grpc, gps_pb2
+from bosdyn.client.common import (BaseClient, common_header_errors, error_factory, error_pair,
+                                  handle_common_header_errors, handle_unset_status_error)
+from bosdyn.client.exceptions import ResponseError
+
+
+class AggregatorClient(BaseClient):
+    """Client for the Gps Aggregator service."""
+    default_service_name = 'gps-aggregator'
+    service_type = 'bosdyn.api.gps.AggregatorService'
+
+    def __init__(self):
+        super(AggregatorClient, self).__init__(aggregator_service_pb2_grpc.AggregatorServiceStub)
+
+    def new_gps_data(self, data_points, gps_device, **kwargs):
+        """Tell the robot about new GPS data that was collected.
+
+        Args:
+            data_points (collection of bosdyn.api.gps.GpsDataPoint): All the data you want to send.
+            gps_device (collection of bosdyn.api.gps.GpsDevice): The identifier of this device.
+
+        Returns:
+            An instance of bosdyn.api.NewGpsDataResponse
+
+        Raises:
+            RpcError: Problem communicating with the robot.
+        """
+        req = aggregator_pb2.NewGpsDataRequest()
+        req.data_points.extend(data_points)
+        req.gps_device.CopyFrom(gps_device)
+        return self.call(self._stub.NewGpsData, req, None, error_from_response=_new_gps_data_error,
+                         copy_request=False, **kwargs)
+
+    def new_gps_data_async(self, data_points, gps_device, **kwargs):
+        """Async version of new_gps_data()"""
+        req = aggregator_pb2.NewGpsDataRequest()
+        req.data_points.extend(data_points)
+        req.gps_device.CopyFrom(gps_device)
+        return self.call_async(self._stub.NewGpsData, req, None,
+                               error_from_response=_new_gps_data_error, copy_request=False,
+                               **kwargs)
+
+
+@handle_common_header_errors
+def _new_gps_data_error(response):
+    """Return an exception based on response from New GPS Data RPC, None if no error."""
+    return None

bosdyn/client/gps/gps_listener.py

@@ -0,0 +1,153 @@
+# Copyright (c) 2023 Boston Dynamics, Inc.  All rights reserved.
+#
+# Downloading, reproducing, distributing or otherwise using the SDK Software
+# is subject to the terms and conditions of the Boston Dynamics Software
+# Development Kit License (20191101-BDSDK-SL).
+
+"""Reads GPS data from a tcp/udp stream, and sends to aggregator service."""
+
+import logging
+import signal
+import socket
+import time
+from typing import List
+
+import bosdyn.api
+import bosdyn.client.util
+from bosdyn.api.gps.gps_pb2 import GpsDataPoint, GpsDevice
+from bosdyn.client.exceptions import ProxyConnectionError
+from bosdyn.client.gps.aggregator_client import AggregatorClient
+from bosdyn.client.gps.NMEAParser import NMEAParser
+from bosdyn.client.robot import UnregisteredServiceNameError
+from bosdyn.util import RobotTimeConverter, duration_to_seconds
+
+
+class NMEAStreamReader(object):
+
+    # The amount of time to wait before logging another decode error.
+    LOG_THROTTLE_TIME = 2.0  # seconds.
+
+    def __init__(self, logger, stream, body_tform_gps):
+        self.logger = logger
+        self.stream = stream
+        self.parser = NMEAParser(logger)
+        self.body_tform_gps = body_tform_gps.to_proto()
+        self.last_failed_read_log_time = None
+
+    def read_data(self, time_converter: RobotTimeConverter) -> List[GpsDataPoint]:
+        """This function returns an array of new GpsDataPoints."""
+        try:
+            raw_data = self.stream.readline()
+        except UnicodeDecodeError:
+            # Throttle the logs.
+            now = time.time()
+            if self.last_failed_read_log_time is None or (
+                    now - self.last_failed_read_log_time) > self.LOG_THROTTLE_TIME:
+                self.logger.exception("Failed to decode NMEA message. Is it not Unicode?")
+                self.last_failed_read_log_time = now
+            return None
+
+        if '$' not in raw_data:
+            # Not NMEA
+            return None
+
+        # Trim any leading characters before the NMEA sentence.
+        raw_data = raw_data[raw_data.index('$'):]
+        self.logger.info(f"Read: {raw_data}")
+        new_points = self.parser.parse(raw_data, time_converter, check=False)
+
+        # Offset for the GPS
+        for data_point in new_points:
+            data_point.body_tform_gps.CopyFrom(self.body_tform_gps)
+
+        return new_points
+
+
+class GpsListener:
+
+    def __init__(self, robot, time_converter, stream, name, body_tform_gps, logger):
+        self.logger = logger
+        self.robot = robot
+        self.time_converter = time_converter
+        self.reader = NMEAStreamReader(logger, stream, body_tform_gps)
+        self.gps_device = GpsDevice()
+        self.gps_device.name = name
+        self.aggregator_client = None
+
+    def run(self):
+        # It is possible for a payload to come up faster than the service. Loop a few times
+        # to give it time to come up.
+        MAX_ATTEMPTS = 45
+        SECS_PER_ATTEMPT = 2
+        num_attempts = 0
+
+        while self.aggregator_client is None and num_attempts < MAX_ATTEMPTS:
+            num_attempts += 1
+            try:
+                self.aggregator_client = self.robot.ensure_client(
+                    AggregatorClient.default_service_name)
+            except (UnregisteredServiceNameError, ProxyConnectionError):
+                self.logger.info("Waiting for the Aggregator Service")
+                time.sleep(SECS_PER_ATTEMPT)
+            except:
+                self.logger.exception(
+                    "Unexpected exception while waiting for the Aggregator Service")
+                time.sleep(SECS_PER_ATTEMPT)
+        if num_attempts == MAX_ATTEMPTS:
+            self.logger.error("Failed to connect to the Aggregator Service!")
+            return False
+
+        # Continue to send an empty GPS data request if connected to a device without GPS signal.
+        # These variables control the frequency with which these empty messages are sent.
+        every_x_seconds = 5
+        time_passed_since_last_rpc = 0
+        timestamp_of_last_rpc = 0
+
+        # Ensure that KeyboardInterrupt is raised on a SIGINT.
+        signal.signal(signal.SIGINT, signal.default_int_handler)
+
+        accumulated_data = []
+        agg_future = None
+
+        # Attach and run until a SIGINT is received.
+        self.logger.info('Looping')
+        try:
+            while True:
+                try:
+                    new_data = self.reader.read_data(self.time_converter)
+                except socket.timeout:
+                    self.logger.warn(
+                        """Socket timed out while reading GPS data. This may be normal, there
+                          could be a problem with the GPS receiver, or there may be a loose hardware
+                          connection.""")
+                    return False
+                except:
+                    self.logger.exception(
+                        "Caught exception while attempting to read from GPS stream.")
+                    return False
+
+                if new_data is None:
+                    continue
+
+                accumulated_data.extend(new_data)
+
+                if len(accumulated_data) > 0:
+                    if agg_future is None or agg_future.done():
+                        agg_future = self.aggregator_client.new_gps_data_async(
+                            accumulated_data, self.gps_device)
+                        accumulated_data.clear()
+                        timestamp_of_last_rpc = time.time()
+                        time_passed_since_last_rpc = 0
+                else:
+                    if time_passed_since_last_rpc > every_x_seconds:
+                        if agg_future is None or agg_future.done():
+                            agg_future = self.aggregator_client.new_gps_data_async([],
+                                                                                   self.gps_device)
+                        timestamp_of_last_rpc = time.time()
+                        time_passed_since_last_rpc = 0
+                    else:
+                        time_passed_since_last_rpc = time.time() - timestamp_of_last_rpc
+
+        except KeyboardInterrupt:
+            print()  # Get past the ^C in the console output
+            pass

bosdyn/client/gps/registration_client.py

@@ -0,0 +1,48 @@
+# Copyright (c) 2023 Boston Dynamics, Inc.  All rights reserved.
+#
+# Downloading, reproducing, distributing or otherwise using the SDK Software
+# is subject to the terms and conditions of the Boston Dynamics Software
+# Development Kit License (20191101-BDSDK-SL).
+
+from bosdyn.api.gps import registration_pb2, registration_service_pb2_grpc
+from bosdyn.client.common import BaseClient, handle_common_header_errors
+
+
+class RegistrationClient(BaseClient):
+    """ Client for the GPS Registration service. """
+    default_service_name = 'gps-registration'
+    service_type = 'bosdyn.api.gps.RegistrationService'
+
+    def __init__(self):
+        super(RegistrationClient,
+              self).__init__(registration_service_pb2_grpc.RegistrationServiceStub)
+
+    def get_location(self):
+        req = registration_pb2.GetLocationRequest()
+        return self.call(self._stub.GetLocation, req, None, error_from_response=_get_location_error,
+                         copy_request=False)
+
+    def get_location_async(self):
+        req = registration_pb2.GetLocationRequest()
+        return self.call_async(self._stub.GetLocation, req, None,
+                               error_from_response=_get_location_error, copy_request=False)
+
+    def reset_registration(self):
+        req = registration_pb2.ResetRegistrationRequest()
+        return self.call(self._stub.ResetRegistration, req, None,
+                         error_from_response=_get_location_error, copy_request=False)
+
+    def reset_registration_async(self):
+        req = registration_pb2.ResetRegistrationRequest()
+        return self.call_async(self._stub.ResetRegistration, req, None,
+                               error_from_response=_get_location_error, copy_request=False)
+
+
+@handle_common_header_errors
+def _get_location_error(response):
+    return None
+
+
+@handle_common_header_errors
+def _reset_registration_error(response):
+    return None

bosdyn/client/graph_nav.py

@@ -128,6 +128,7 @@ class GraphNavClient(BaseClient):
             request_live_world_objects=False,
             request_live_robot_state=False,
             waypoint_id=None,
+            request_gps_state=False,
             **kwargs):
         """Obtain current localization state of the robot.
 
@@ -141,21 +142,23 @@ class GraphNavClient(BaseClient):
             request_live_images=request_live_images,
             request_live_terrain_maps=request_live_terrain_maps,
             request_live_world_objects=request_live_world_objects,
-            request_live_robot_state=request_live_robot_state, waypoint_id=waypoint_id)
+            request_live_robot_state=request_live_robot_state, waypoint_id=waypoint_id,
+            request_gps_state=request_gps_state)
         return self.call(self._stub.GetLocalizationState, req, None, common_header_errors,
                          copy_request=False, **kwargs)
 
-    def get_localization_state_async(self, request_live_point_cloud=False,
-                                     request_live_images=False, request_live_terrain_maps=False,
-                                     request_live_world_objects=False,
-                                     request_live_robot_state=False, waypoint_id=None, **kwargs):
+    def get_localization_state_async(
+            self, request_live_point_cloud=False, request_live_images=False,
+            request_live_terrain_maps=False, request_live_world_objects=False,
+            request_live_robot_state=False, waypoint_id=None, request_gps_state=False, **kwargs):
         """Async version of get_localization_state()."""
         req = self._build_get_localization_state_request(
             request_live_point_cloud=request_live_point_cloud,
             request_live_images=request_live_images,
             request_live_terrain_maps=request_live_terrain_maps,
             request_live_world_objects=request_live_world_objects,
-            request_live_robot_state=request_live_robot_state, waypoint_id=waypoint_id)
+            request_live_robot_state=request_live_robot_state, waypoint_id=waypoint_id,
+            request_gps_state=request_gps_state)
         return self.call_async(self._stub.GetLocalizationState, req, None, common_header_errors,
                                copy_request=False, **kwargs)
 
@@ -302,7 +305,8 @@ class GraphNavClient(BaseClient):
             raise GraphNavServiceResponseError(response=None, error_message='No timesync endpoint!')
         request = self._build_navigate_to_request(destination_waypoint_id, travel_params,
                                                   route_params, cmd_duration, leases, used_endpoint,
-                                                  command_id, destination_waypoint_tform_body_goal)
+                                                  command_id, destination_waypoint_tform_body_goal,
+                                                  route_blocked_behavior)
         return self.call_async(self._stub.NavigateTo, request,
                                value_from_response=_command_id_from_navigate_route_response,
                                error_from_response=_navigate_to_error, copy_request=False, **kwargs)
@@ -340,7 +344,7 @@ class GraphNavClient(BaseClient):
     def navigate_to_anchor(self, seed_tform_goal, cmd_duration, route_params=None,
                            travel_params=None, leases=None, timesync_endpoint=None,
                            goal_waypoint_rt_seed_ewrt_seed_tolerance=None, command_id=None,
-                           **kwargs):
+                           gps_navigation_params=None, **kwargs):
         """Navigate to a pose in seed frame along a route chosen by the GraphNav service.
 
         Args:
@@ -353,6 +357,9 @@ class GraphNavClient(BaseClient):
             goal_waypoint_rt_seed_ewrt_seed_tolerance: Vec3 protobuf of the tolerances for goal waypoint selection.
             command_id: If not None, this continues an existing navigate_to command with the given ID. If None,
             a new command_id will be used.
+            gps_navigation_params: API GPSNavigationParams. If not None, this will be interpreted as a GPS-based
+            navigation command. seed_tform_goal will be ignored and whatever goal is passed in using the GPS
+            navigation params will be used.
         Returns:
             int: Command ID to use in feedback lookup.
         Raises:
@@ -372,10 +379,9 @@ class GraphNavClient(BaseClient):
         used_endpoint = timesync_endpoint or self._timesync_endpoint
         if not used_endpoint:
             raise GraphNavServiceResponseError(response=None, error_message='No timesync endpoint!')
-        request = self._build_navigate_to_anchor_request(seed_tform_goal, travel_params,
-                                                         route_params, cmd_duration, leases,
-                                                         used_endpoint, command_id,
-                                                         goal_waypoint_rt_seed_ewrt_seed_tolerance)
+        request = self._build_navigate_to_anchor_request(
+            seed_tform_goal, travel_params, route_params, cmd_duration, leases, used_endpoint,
+            command_id, goal_waypoint_rt_seed_ewrt_seed_tolerance, gps_navigation_params)
         return self.call(self._stub.NavigateToAnchor, request,
                          value_from_response=_command_id_from_navigate_route_response,
                          error_from_response=_navigate_to_anchor_error, copy_request=False,
@@ -384,15 +390,14 @@ class GraphNavClient(BaseClient):
     def navigate_to_anchor_async(self, seed_tform_goal, cmd_duration, route_params=None,
                                  travel_params=None, leases=None, timesync_endpoint=None,
                                  goal_waypoint_rt_seed_ewrt_seed_tolerance=None, command_id=None,
-                                 **kwargs):
+                                 gps_navigation_params=None, **kwargs):
         """Async version of navigate_to_anchor()."""
         used_endpoint = timesync_endpoint or self._timesync_endpoint
         if not used_endpoint:
             raise GraphNavServiceResponseError(response=None, error_message='No timesync endpoint!')
-        request = self._build_navigate_to_anchor_request(seed_tform_goal, travel_params,
-                                                         route_params, cmd_duration, leases,
-                                                         used_endpoint, command_id,
-                                                         goal_waypoint_rt_seed_ewrt_seed_tolerance)
+        request = self._build_navigate_to_anchor_request(
+            seed_tform_goal, travel_params, route_params, cmd_duration, leases, used_endpoint,
+            command_id, goal_waypoint_rt_seed_ewrt_seed_tolerance, gps_navigation_params)
         return self.call_async(self._stub.NavigateTo, request,
                                value_from_response=_command_id_from_navigate_route_response,
                                error_from_response=_navigate_to_anchor_error, copy_request=False,
@@ -628,13 +633,15 @@ class GraphNavClient(BaseClient):
     @staticmethod
     def _build_get_localization_state_request(request_live_point_cloud, request_live_images,
                                               request_live_terrain_maps, request_live_world_objects,
-                                              request_live_robot_state, waypoint_id):
+                                              request_live_robot_state, waypoint_id,
+                                              request_gps_state):
         return graph_nav_pb2.GetLocalizationStateRequest(
             request_live_point_cloud=request_live_point_cloud,
             request_live_images=request_live_images,
             request_live_terrain_maps=request_live_terrain_maps,
             request_live_world_objects=request_live_world_objects,
-            request_live_robot_state=request_live_robot_state, waypoint_id=waypoint_id)
+            request_live_robot_state=request_live_robot_state, waypoint_id=waypoint_id,
+            request_gps_state=request_gps_state)
 
     @staticmethod
     def _build_navigate_route_request(route, route_follow_params, travel_params, end_time_secs,
@@ -677,12 +684,16 @@ class GraphNavClient(BaseClient):
     @staticmethod
     def _build_navigate_to_anchor_request(seed_tform_goal, travel_params, route_params,
                                           end_time_secs, leases, timesync_endpoint, command_id,
-                                          goal_waypoint_rt_seed_ewrt_seed_tolerance):
+                                          goal_waypoint_rt_seed_ewrt_seed_tolerance,
+                                          gps_navigation_params):
         converter = timesync_endpoint.get_robot_time_converter()
         request = graph_nav_pb2.NavigateToAnchorRequest(
             seed_tform_goal=seed_tform_goal,
             goal_waypoint_rt_seed_ewrt_seed_tolerance=goal_waypoint_rt_seed_ewrt_seed_tolerance,
             clock_identifier=timesync_endpoint.clock_identifier)
+        # Note that this overrides the seed_tform_goal, which is a OneOf.
+        if gps_navigation_params:
+            request.gps_navigation_params.CopyFrom(gps_navigation_params)
         request.end_time.CopyFrom(
             converter.robot_timestamp_from_local_secs(time.time() + end_time_secs))
         if travel_params is not None:
@@ -925,6 +936,23 @@ class RobotLostError(RouteNavigationError):
     """Cannot issue a navigation request when the robot is already lost."""
 
 
+class InvalidGPSError(RouteNavigationError):
+    """Cannot issue the GPS command because it is invalid."""
+
+    def _gps_status_to_string(self, status):
+        if status == graph_nav_pb2.NavigateToAnchorResponse.GPS_STATUS_OK:
+            return 'OK'
+        elif status == graph_nav_pb2.NavigateToAnchorResponse.GPS_STATUS_NO_COORDS_IN_MAP:
+            return 'The uploaded map did not contain any valid GPS coordinates.'
+        elif status == graph_nav_pb2.NavigateToAnchorResponse.GPS_STATUS_TOO_FAR_FROM_MAP:
+            return 'The given coordinates were too far from any coordinates in the uploaded map.'
+        else:
+            return 'Unknown error'
+
+    def __str__(self):
+        return f'{self.error_message} (reason: {self._gps_status_to_string(self.response.gps_status)})'
+
+
 class RobotNotLocalizedToRouteError(RouteNavigationError):
     """The current localization doesn't refer to any waypoint in the route (possibly uninitialized localization)."""
 
@@ -1213,6 +1241,8 @@ _NAVIGATE_TO_ANCHOR_STATUS_TO_ERROR.update({
         error_pair(UnrecognizedCommandError),
     graph_nav_pb2.NavigateToAnchorResponse.STATUS_AREA_CALLBACK_ERROR:
         error_pair(AreaCallbackMapError),
+    graph_nav_pb2.NavigateToAnchorResponse.STATUS_INVALID_GPS_COMMAND:
+        error_pair(InvalidGPSError)
 })