bosdyn-client 4.1.1__py3-none-any.whl → 5.0.1__py3-none-any.whl

bosdyn/client/gps/NMEAParser.py

@@ -14,7 +14,7 @@ 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
+from bosdyn.util import RobotTimeConverter, now_sec, seconds_to_timestamp
 
 
 has_warned_no_zda = False
@@ -36,6 +36,7 @@ class NMEAParser(object):
         # 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
+        self.last_gga = None
 
     def nmea_message_group_to_gps_data_point(self, nmea_messages: List[Tuple[str, str, int]],
                                              time_converter: RobotTimeConverter):
@@ -53,12 +54,13 @@ class NMEAParser(object):
             data, raw_nmea_msg, client_timestamp = nmea_message_list
 
             if data.sentence_type == 'GGA':
+                self.last_gga = raw_nmea_msg
                 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:
+                if data.num_sats is not None and data.num_sats != '' and int(data.num_sats) > 0:
                     for _ in range(int(data.num_sats)):
                         sat = data_point.satellites.add()
 
@@ -86,7 +88,12 @@ class NMEAParser(object):
                     data_point.accuracy.vertical = data.std_dev_altitude
 
             elif data.sentence_type == 'ZDA':
-                gps_timestamp = data.datetime
+                try:
+                    gps_timestamp = data.datetime
+                except:
+                    self.logger.exception("Failed to extract datetime from ZDA message.")
+                    continue
+
                 # 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)
@@ -111,7 +118,7 @@ class NMEAParser(object):
 
     def parse(self, new_data: str, time_converter: RobotTimeConverter,
               check: bool = True) -> List[GpsDataPoint]:
-        timestamp = time.time()  # Client timestamp when received.
+        timestamp = now_sec()  # Client timestamp when received.
         self.data = self.data + new_data
 
         if len(self.data) == 0:
@@ -143,14 +150,14 @@ class NMEAParser(object):
             except Exception as e:
                 # Parsing error, log and skip.
                 # Throttle the logs.
-                now = time.time()
+                now = now_sec()
                 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
 
-            # if the message does not contain a timestamp attribute, abandon the rest of the logic
+            # If the message does not contain a timestamp attribute, abandon the rest of the logic
             # and go to the beginning of the loop
             if not hasattr(nmea_msg, 'timestamp'):
                 continue
@@ -193,3 +200,6 @@ class NMEAParser(object):
                     break
 
         return [self.nmea_message_group_to_gps_data_point(x, time_converter) for x in found_subsets]
+
+    def get_latest_gga(self):
+        return self.last_gga

bosdyn/client/gps/gps_listener.py

@@ -17,6 +17,7 @@ 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.gps.ntrip_client import NtripClient, NtripClientParams
 from bosdyn.client.robot import UnregisteredServiceNameError
 from bosdyn.util import RobotTimeConverter, duration_to_seconds
 
@@ -70,6 +71,9 @@ class NMEAStreamReader(object):
 
         return new_points
 
+    def get_latest_gga(self):
+        return self.parser.get_latest_gga()
+
 
 class GpsListener:
 
@@ -77,10 +81,22 @@ class GpsListener:
         self.logger = logger
         self.robot = robot
         self.time_converter = time_converter
+        self.stream = stream
         self.reader = NMEAStreamReader(logger, stream, body_tform_gps, verbose)
         self.gps_device = GpsDevice()
         self.gps_device.name = name
         self.aggregator_client = None
+        self.ntrip_client = None
+
+    def run_ntrip_client(self, ntrip_params: NtripClientParams):
+        self.ntrip_client = NtripClient(self.stream, ntrip_params, self.logger)
+        self.ntrip_client.start_stream()
+
+    def stop_ntrip_client(self):
+        if self.ntrip_client is None:
+            return
+        self.ntrip_client.stop_stream()
+        self.ntrip_client = None
 
     def run(self):
         # It is possible for a payload to come up faster than the service. Loop a few times
@@ -118,7 +134,7 @@ class GpsListener:
         agg_future = None
 
         # Attach and run until a SIGINT is received.
-        self.logger.info('Looping')
+        self.logger.info('Listening for GPS data.')
         try:
             while True:
                 try:
@@ -156,5 +172,19 @@ class GpsListener:
                     else:
                         time_passed_since_last_rpc = time.time() - timestamp_of_last_rpc
 
+                # If we are running an NTRIP client, pass it the latest GGA message.
+                if self.ntrip_client is not None:
+                    # If the NTRIP Client's stream has been closed, restart it.
+                    if not self.ntrip_client.is_streaming():
+                        self.logger.info("Restarting NTRIP Client!")
+                        self.ntrip_client.start_stream()
+
+                    latest_gga = self.reader.get_latest_gga()
+                    if latest_gga is not None:
+                        self.ntrip_client.handle_nmea_gga(latest_gga)
+
         except KeyboardInterrupt:
             print()  # Get past the ^C in the console output
+
+        # Just in case there is an NTRIP client still running, stop it here.
+        self.stop_ntrip_client()

bosdyn/client/gps/ntrip_client.py

@@ -0,0 +1,240 @@
+# 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 base64
+import socket
+import ssl
+import time
+from threading import Thread
+
+SERVER_RECONNECT_DELAY = 60  # seconds of delay before retry after server error
+SOCKET_TIMEOUT = 10  # socket timeout in seconds
+SOCKET_MAX_RECV_TIMEOUTS = 12  # number of timeouts before reconnecting
+
+DEFAULT_NTRIP_SERVER = ""
+DEFAULT_NTRIP_PORT = 2101
+DEFAULT_NTRIP_TLS_PORT = 2102
+
+
+class NtripClientParams:
+    """
+    Class for storing parameters for connecting an NTRIP client to an NTRIP server.
+    """
+
+    def __init__(self, server=DEFAULT_NTRIP_SERVER, port=DEFAULT_NTRIP_PORT, user="", password="",
+                 mountpoint="", tls=False):
+        """
+        Constructor.
+        """
+        self.server = server
+        self.port = port
+        self.user = user
+        self.password = password
+        self.mountpoint = mountpoint
+        self.tls = tls
+
+
+class NtripClient:
+    """
+    Client used to connect to an NTRIP server to download GPS corrections. These corrections are
+    then forwarded on to the GPS device using the given stream.
+    """
+
+    def __init__(self, device, params: NtripClientParams, logger):
+        """
+        Constructor.
+        """
+        self.device = device
+        self.host = params.server
+        self.port = params.port
+        self.user = params.user
+        self.password = params.password
+        self.mountpoint = params.mountpoint
+        self.tls = params.tls
+
+        self.thread = None
+        self.streaming = False
+        self.sock = None
+
+        self.logger = logger
+
+    def make_request(self):
+        """
+        Make a connection request to an NTRIP server.
+        """
+        auth_str = base64.b64encode(f"{self.user}:{self.password}".encode()).decode()
+        request = (f"GET /{self.mountpoint} HTTP/1.1\r\n"
+                   f"Host: {self.host}:{self.port}\r\n"
+                   "User-Agent: NTRIP Python Client\r\n"
+                   "Accept: */*\r\n"
+                   f"Authorization: Basic {auth_str}\r\n"
+                   "Connection: close\r\n"
+                   "\r\n")
+        return request.encode()
+
+    def start_stream(self):
+        """
+        Start streaming data from an NTRIP server to a GPS receiver.
+        """
+        if self.streaming:
+            self.stop_stream()
+        self.thread = Thread(target=self.stream_data_worker, daemon=True)
+        self.thread.start()
+
+    def stop_stream(self):
+        """
+        Stop streaming NTRIP data.
+        """
+        self.streaming = False
+        if self.thread:
+            self.thread.join()
+        self.thread = None
+
+    def is_streaming(self):
+        """
+        Determine if we are streaming NTRIP data.
+        """
+        return self.streaming
+
+    def send_gga(self, gga):
+        """
+        Given a GPGGA message, send it to the NTRIP server. This helps the NTRIP server send
+        corrections that are applicable to the area in which the receiver is operating.
+        """
+        if self.sock:
+            try:
+                self.sock.send(gga.encode())
+            except OSError:
+                self.logger.warning("Error sending GGA")
+                return False
+            return True
+        return False
+
+    def create_icy_session(self):
+        """
+        NTRIP Rev1 uses Shoutcast (ICY). Create an ICY session to stream RTCM data.
+        """
+        try:
+            # Create a socket connection to the host and port.
+            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            if self.tls:
+                self.logger.info("Using secure connection")
+                context = ssl.create_default_context()
+                sock = context.wrap_socket(sock, server_hostname=self.host)
+            sock.settimeout(SOCKET_TIMEOUT)
+            self.logger.info("Connecting to NTRIP Server.")
+            sock.connect((self.host, self.port))
+            self.logger.info("Connected to NTRIP Server.")
+
+            # Send the request.
+            self.logger.info("Sending NTRIP Request.")
+            sock.send(self.make_request())
+            # Read a chunk of data, expecting to get the status line and headers.
+            response = sock.recv(1024)
+
+            response_lines = response.split(b"\r\n")
+            if len(response_lines) < 2:
+                self.logger.error("Invalid response from server: %s", response)
+                return False
+            status = response_lines[0].decode().split(" ")
+            self.logger.info(response_lines[0].decode())
+            if status[1] != "200":
+                self.logger.error("HTTP Error: %s, retrying in %d seconds",
+                                  response_lines[0].decode(), SERVER_RECONNECT_DELAY)
+                sock.close()
+                time.sleep(SERVER_RECONNECT_DELAY)
+                return False
+            self.logger.info("NTRIP Request Response received.")
+            for line in range(1, len(response_lines)):
+                if response_lines[line] == b"":
+                    # Empty line, end of headers.
+                    # The rest of the response contains data.
+                    response = b"\r\n".join(response_lines[line + 1:])
+                    break
+                self.logger.info(response_lines[line])
+
+            if response:
+                self.handle_ntrip_data(response)
+
+            # Make the socket available for sending GGA.
+            self.sock = sock
+            return True
+        except (socket.herror, socket.gaierror):
+            self.logger.warning("Error connecting to server %s:%d", self.host, self.port)
+            return False
+        except (TimeoutError, socket.timeout):
+            self.logger.warning("Connection timeout")
+            return False
+
+    def stream_data(self):
+        """
+        Stream NTRIP data from a connected server and send it to a GPS receiver.
+        """
+
+        # NTRIP Rev1 uses Shoutcast (ICY), which looks a lot like HTTP but isn't quite the same.
+        # Create an ICY session here to talk to an NTRIP Rev1 caster.
+        ok = self.create_icy_session()
+        if not ok:
+            self.logger.error("Failed to create NTRIP Rev1 ICY session.")
+            return
+
+        timeouts = 0
+        has_data = False
+        while self.streaming and timeouts < SOCKET_MAX_RECV_TIMEOUTS:
+            try:
+                response = self.sock.recv(2048)
+                if response:
+                    timeouts = 0
+                    self.handle_ntrip_data(response)
+                    if not has_data:
+                        self.logger.info("Received NTRIP data.")
+                        has_data = True
+                else:
+                    self.logger.warning("Connection closed by server")
+                    break
+            except (socket.herror, socket.gaierror):
+                self.logger.warning("Error connecting to server %s:%d", self.host, self.port)
+            except (TimeoutError, socket.timeout):
+                self.logger.exception("Socket timeout.")
+                timeouts += 1
+
+        self.logger.info("NTRIP stream closed. Closing socket.")
+        # Close the socket.
+        try:
+            self.sock.close()
+        except OSError:
+            pass
+        self.sock = None
+
+        self.logger.info("NTRIP client finished.")
+
+    def stream_data_worker(self):
+        """
+        NTRIP client worker thread function.
+        """
+        self.streaming = True
+        while self.streaming:
+            self.stream_data()
+
+    def handle_ntrip_data(self, data):
+        """
+        Callback for handling NTRIP data.
+        """
+        # Send to receiver as is.
+        self.device.write(data)
+
+    def handle_nmea_gga(self, sentence):
+        """
+        Process an NMEA-GGA sentence passed in as a string.
+        """
+        fields = sentence.split(",")
+        if len(fields) < 7:
+            return
+        quality = int(fields[6] or 0)
+        if quality not in (0, 6):  # No fix or estimated.
+            if not self.send_gga(sentence + "\r\n"):
+                # Something went wrong, restart the stream.
+                self.start_stream()

bosdyn/client/graph_nav.py

@@ -413,11 +413,26 @@ class GraphNavClient(BaseClient):
         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,
+        return self.call_async(self._stub.NavigateToAnchor, request,
                                value_from_response=_command_id_from_navigate_route_response,
                                error_from_response=_navigate_to_anchor_error, copy_request=False,
                                **kwargs)
 
+    def navigate_to_anchor_full_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, gps_navigation_params=None, **kwargs):
+        """Async version of navigate_to_anchor() that returns the full response."""
+        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, gps_navigation_params)
+        return self.call_async(self._stub.NavigateToAnchor, request,
+                               error_from_response=_navigate_to_anchor_error, copy_request=False,
+                               **kwargs)
+
     def navigation_feedback(self, command_id=0, **kwargs):
         """Returns the feedback corresponding to the active route follow command.
 

bosdyn/client/gripper_camera_param.py

@@ -4,6 +4,8 @@
 # is subject to the terms and conditions of the Boston Dynamics Software
 # Development Kit License (20191101-BDSDK-SL).
 
+from deprecated.sphinx import deprecated
+
 from bosdyn.api import gripper_camera_param_service_pb2_grpc
 from bosdyn.client.common import BaseClient, common_header_errors
 
@@ -52,3 +54,41 @@ class GripperCameraParamClient(BaseClient):
         """Async version of gripper_camera_get_param_service_command()."""
         return self.call_async(self._stub.GetParams, gripper_camera_get_param_request,
                                error_from_response=common_header_errors, **kwargs)
+
+    def set_camera_calib(self, set_gripper_camera_calib_request, **kwargs):
+        """Issue gripper camera calibration
+
+        Args:
+            set_gripper_camera_calib_request (gripper_camera_params_pb2.GripperCameraCalibrationRequest) : The command request to set gripper camera calibration
+
+        Returns:
+            The GripperCameraCalibrationResponse message.
+        """
+        return self.call(self._stub.SetCamCalib, set_gripper_camera_calib_request,
+                         error_from_response=common_header_errors, **kwargs)
+
+    def set_camera_calib_async(self, set_gripper_camera_calib_request, **kwargs):
+        """Async version of set_camera_calib()."""
+        return self.call_async(self._stub.SetCamCalib, set_gripper_camera_calib_request,
+                               error_from_response=common_header_errors, **kwargs)
+
+    def get_camera_calib(self, get_gripper_camera_calib_request, **kwargs):
+        """Issue gripper camera get calibration
+
+        Arge:
+            get_gripper_camera_calib_request (gripper_camera_params_pb2.GripperCameraGetCalibrationRequest) : The command reqeust to get gripper camera calibration
+
+        Returns:
+            The GripperCameraGetCalibrationResponse message, which contains the GripperCameraCalibrationProto
+        """
+        return self.call(self._stub.GetCamCalib, get_gripper_camera_calib_request,
+                         error_from_response=common_header_errors, **kwargs)
+
+    def get_camera_calib_async(self, get_gripper_camera_calib_request, **kwargs):
+        """Async version of get_camera_calib()."""
+        return self.call_async(self._stub.GetCamCalib, get_gripper_camera_calib_request,
+                               error_from_response=common_header_errors, **kwargs)
+
+    @deprecated(version='5.0', reason='Use get_camera_calib_async() instead.')
+    def get_camera_calib_asnyc(self, get_gripper_camera_calib_request, **kwargs):
+        return self.get_camera_calib_async(get_gripper_camera_calib_request, **kwargs)

bosdyn/client/image.py

@@ -204,6 +204,22 @@ def _get_image_value(response):
     return response.image_responses
 
 
+def pixel_format_to_numpy_type(pixel_format):
+    if pixel_format == image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8:
+        dtype = np.uint8
+    elif pixel_format == image_pb2.Image.PIXEL_FORMAT_RGB_U8:
+        dtype = np.uint8
+    elif pixel_format == image_pb2.Image.PIXEL_FORMAT_RGBA_U8:
+        dtype = np.uint8
+    elif pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16:
+        dtype = np.uint16
+    elif pixel_format == image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U16:
+        dtype = ">u2"  # Big endian
+    else:
+        raise Exception('Image PixelFormat type {} not supported'.format(pixel_format))
+    return dtype
+
+
 def write_pgm_or_ppm(image_response, filename="", filepath=".", include_pixel_format=False):
     """Write raw data from image_response to a PGM file.