bosdyn-client 4.1.1__py3-none-any.whl → 5.0.1__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- bosdyn/client/__init__.py +1 -0
- bosdyn/client/audio_visual.py +339 -0
- bosdyn/client/audio_visual_helpers.py +112 -0
- bosdyn/client/command_line.py +15 -7
- bosdyn/client/data_acquisition_helpers.py +1 -1
- bosdyn/client/data_acquisition_store.py +2 -2
- bosdyn/client/directory_registration.py +34 -5
- bosdyn/client/error_callback_result.py +29 -0
- bosdyn/client/gps/NMEAParser.py +16 -6
- bosdyn/client/gps/gps_listener.py +31 -1
- bosdyn/client/gps/ntrip_client.py +240 -0
- bosdyn/client/graph_nav.py +16 -1
- bosdyn/client/gripper_camera_param.py +40 -0
- bosdyn/client/image.py +16 -0
- bosdyn/client/image_service_helpers.py +88 -68
- bosdyn/client/keepalive.py +37 -8
- bosdyn/client/log_status.py +6 -0
- bosdyn/client/math_helpers.py +18 -0
- bosdyn/client/payload_registration.py +40 -6
- bosdyn/client/payload_software_update.py +185 -0
- bosdyn/client/payload_software_update_initiation.py +79 -0
- bosdyn/client/point_cloud.py +9 -0
- bosdyn/client/robot.py +9 -4
- bosdyn/client/sdk.py +4 -2
- bosdyn/client/service_customization_helpers.py +19 -6
- bosdyn/client/spot_cam/__init__.py +2 -0
- bosdyn/client/spot_cam/ptz.py +20 -24
- bosdyn/client/token_manager.py +56 -27
- bosdyn/client/util.py +1 -1
- {bosdyn_client-4.1.1.dist-info → bosdyn_client-5.0.1.dist-info}/METADATA +4 -4
- {bosdyn_client-4.1.1.dist-info → bosdyn_client-5.0.1.dist-info}/RECORD +33 -27
- {bosdyn_client-4.1.1.dist-info → bosdyn_client-5.0.1.dist-info}/WHEEL +0 -0
- {bosdyn_client-4.1.1.dist-info → bosdyn_client-5.0.1.dist-info}/top_level.txt +0 -0
|
@@ -4,6 +4,7 @@
|
|
|
4
4
|
# is subject to the terms and conditions of the Boston Dynamics Software
|
|
5
5
|
# Development Kit License (20191101-BDSDK-SL).
|
|
6
6
|
|
|
7
|
+
import contextlib
|
|
7
8
|
import inspect
|
|
8
9
|
import logging
|
|
9
10
|
import sys
|
|
@@ -15,11 +16,12 @@ import numpy as np
|
|
|
15
16
|
|
|
16
17
|
from bosdyn.api import (header_pb2, image_pb2, image_service_pb2, image_service_pb2_grpc,
|
|
17
18
|
service_customization_pb2, service_fault_pb2)
|
|
19
|
+
from bosdyn.client.data_buffer import DataBufferClient
|
|
18
20
|
from bosdyn.client.exceptions import RpcError
|
|
19
21
|
from bosdyn.client.fault import (FaultClient, ServiceFaultAlreadyExistsError,
|
|
20
22
|
ServiceFaultDoesNotExistError)
|
|
21
23
|
from bosdyn.client.image import UnsupportedPixelFormatRequestedError
|
|
22
|
-
from bosdyn.client.server_util import populate_response_header
|
|
24
|
+
from bosdyn.client.server_util import ResponseContext, populate_response_header
|
|
23
25
|
from bosdyn.client.service_customization_helpers import create_value_validator, validate_dict_spec
|
|
24
26
|
from bosdyn.client.util import setup_logging
|
|
25
27
|
from bosdyn.util import sec_to_nsec, seconds_to_duration
|
|
@@ -430,7 +432,7 @@ class VisualImageSource():
|
|
|
430
432
|
exposure (float | function): The exposure time for an image in seconds. This can be a fixed
|
|
431
433
|
value or a function which returns the exposure time as a float.
|
|
432
434
|
request_custom_params (service_customization_pb2.DictParam): Custom Params associated with the image
|
|
433
|
-
request. Should not be 'None', but left as an option to
|
|
435
|
+
request. Should not be 'None', but left as an option to accommodate old callers
|
|
434
436
|
Returns:
|
|
435
437
|
An instance of the protobuf CaptureParameters message.
|
|
436
438
|
"""
|
|
@@ -562,7 +564,7 @@ class ImageCaptureThread():
|
|
|
562
564
|
"""Update the capture function to use custom_params and capture_func_kwargs if it can"""
|
|
563
565
|
|
|
564
566
|
# capture_func is likely provided through create_capture_thread, which already does handling for pre-3.3 blocking_capture
|
|
565
|
-
# Still, check for the custom_params argument to ensure we
|
|
567
|
+
# Still, check for the custom_params argument to ensure we accommodate pre-3.3 direct implementations of ImageCaptureThreads
|
|
566
568
|
if "custom_params" in inspect.signature(capture_func).parameters.keys():
|
|
567
569
|
#If using a blocking capture function that takes in custom params or kwargs, one should supply those
|
|
568
570
|
def output_capture_function():
|
|
@@ -607,10 +609,14 @@ class CameraBaseImageServicer(image_service_pb2_grpc.ImageServiceServicer):
|
|
|
607
609
|
the image service will call an image sources' blocking_capture_function during the GetImage request.
|
|
608
610
|
background_capture_params (service_customization_pb2.DictParam): If use_background_capture_thread is true,
|
|
609
611
|
custom image source parameters used for all of the background captures. Otherwise ignored
|
|
612
|
+
log_images (bool): if true, include image request/response messages in robot logs. This is turned off
|
|
613
|
+
by default.
|
|
614
|
+
|
|
610
615
|
"""
|
|
611
616
|
|
|
612
617
|
def __init__(self, bosdyn_sdk_robot, service_name, image_sources, logger=None,
|
|
613
|
-
use_background_capture_thread=True, background_capture_params=None
|
|
618
|
+
use_background_capture_thread=True, background_capture_params=None,
|
|
619
|
+
log_images=False):
|
|
614
620
|
super(CameraBaseImageServicer, self).__init__()
|
|
615
621
|
if logger is None:
|
|
616
622
|
# Set up the logger to remove duplicated messages and use a specific logging format.
|
|
@@ -627,6 +633,13 @@ class CameraBaseImageServicer(image_service_pb2_grpc.ImageServiceServicer):
|
|
|
627
633
|
# Fault client to report service faults
|
|
628
634
|
self.fault_client = self.bosdyn_sdk_robot.ensure_client(FaultClient.default_service_name)
|
|
629
635
|
|
|
636
|
+
if log_images:
|
|
637
|
+
# Data buffer client for logging messages
|
|
638
|
+
self.data_buffer_client = self.bosdyn_sdk_robot.ensure_client(
|
|
639
|
+
DataBufferClient.default_service_name)
|
|
640
|
+
else:
|
|
641
|
+
self.data_buffer_client = None
|
|
642
|
+
|
|
630
643
|
# Get a timesync endpoint from the robot instance such that the image timestamps can be
|
|
631
644
|
# reported in the robot's time.
|
|
632
645
|
self.bosdyn_sdk_robot.time_sync.wait_for_sync()
|
|
@@ -681,74 +694,81 @@ class CameraBaseImageServicer(image_service_pb2_grpc.ImageServiceServicer):
|
|
|
681
694
|
specified in the request.
|
|
682
695
|
"""
|
|
683
696
|
response = image_pb2.GetImageResponse()
|
|
684
|
-
|
|
685
|
-
|
|
686
|
-
|
|
687
|
-
|
|
688
|
-
|
|
689
|
-
|
|
690
|
-
img_resp
|
|
691
|
-
|
|
692
|
-
|
|
693
|
-
|
|
694
|
-
|
|
695
|
-
|
|
696
|
-
|
|
697
|
-
|
|
698
|
-
|
|
699
|
-
|
|
700
|
-
|
|
701
|
-
img_req.
|
|
702
|
-
if value_validation_error:
|
|
703
|
-
img_resp.status = image_pb2.ImageResponse.STATUS_CUSTOM_PARAMS_ERROR
|
|
704
|
-
img_resp.custom_param_error.CopyFrom(value_validation_error)
|
|
697
|
+
if self.data_buffer_client is not None:
|
|
698
|
+
response_context = ResponseContext(response, request, self.data_buffer_client)
|
|
699
|
+
else:
|
|
700
|
+
response_context = contextlib.nullcontext()
|
|
701
|
+
with response_context:
|
|
702
|
+
for img_req in request.image_requests:
|
|
703
|
+
img_resp = response.image_responses.add()
|
|
704
|
+
src_name = img_req.image_source_name
|
|
705
|
+
if src_name not in self.image_sources_mapped:
|
|
706
|
+
# The requested camera source does not match the name of the Ricoh Theta camera, so it cannot
|
|
707
|
+
# be completed and will have a failure status in the response message.
|
|
708
|
+
img_resp.status = image_pb2.ImageResponse.STATUS_UNKNOWN_CAMERA
|
|
709
|
+
self.logger.warning("Camera source '%s' is unknown.", src_name)
|
|
710
|
+
continue
|
|
711
|
+
|
|
712
|
+
if img_req.resize_ratio < 0 or img_req.resize_ratio > 1:
|
|
713
|
+
img_resp.status = image_pb2.ImageResponse.STATUS_UNSUPPORTED_RESIZE_RATIO_REQUESTED
|
|
714
|
+
self.logger.warning("Resize ratio %f is unsupported.", img_req.resize_ratio)
|
|
705
715
|
continue
|
|
706
716
|
|
|
707
|
-
|
|
708
|
-
|
|
717
|
+
if img_req.HasField("custom_params"):
|
|
718
|
+
value_validation_error = self.image_sources_mapped[src_name].value_validator(
|
|
719
|
+
img_req.custom_params)
|
|
720
|
+
if value_validation_error:
|
|
721
|
+
img_resp.status = image_pb2.ImageResponse.STATUS_CUSTOM_PARAMS_ERROR
|
|
722
|
+
img_resp.custom_param_error.CopyFrom(value_validation_error)
|
|
723
|
+
continue
|
|
724
|
+
|
|
725
|
+
# Set the image source information in the response.
|
|
726
|
+
img_resp.source.CopyFrom(self.image_sources_mapped[src_name].image_source_proto)
|
|
727
|
+
|
|
728
|
+
# Set the image capture parameters in the response.
|
|
729
|
+
img_resp.shot.capture_params.CopyFrom(
|
|
730
|
+
self.image_sources_mapped[src_name].get_image_capture_params(
|
|
731
|
+
img_req.custom_params))
|
|
732
|
+
|
|
733
|
+
if img_req.HasField("custom_params"):
|
|
734
|
+
#If future keyword arguments are added here, they'll need to be added to this call
|
|
735
|
+
#get_image_and_timestamp already calls a 'sanitized' capture function that handles pre-3.3 blocking_captures
|
|
736
|
+
captured_image, img_time_seconds = self.image_sources_mapped[
|
|
737
|
+
src_name].get_image_and_timestamp(custom_params=img_req.custom_params)
|
|
738
|
+
else:
|
|
739
|
+
#get_image_and_timestamp() can accommodate pre-3.3 blocking_capture calls if no custom params are supplied
|
|
740
|
+
captured_image, img_time_seconds = self.image_sources_mapped[
|
|
741
|
+
src_name].get_image_and_timestamp()
|
|
742
|
+
|
|
743
|
+
if captured_image is None or img_time_seconds is None:
|
|
744
|
+
img_resp.status = image_pb2.ImageResponse.STATUS_IMAGE_DATA_ERROR
|
|
745
|
+
error_message = "Failed to capture an image from %s on the server." % src_name
|
|
746
|
+
response.header.error.message = error_message
|
|
747
|
+
self.logger.warning(error_message)
|
|
748
|
+
continue
|
|
709
749
|
|
|
710
|
-
|
|
711
|
-
|
|
712
|
-
|
|
750
|
+
# Convert the image capture time from the local clock time into the robot's time. Then set it as
|
|
751
|
+
# the acquisition timestamp for the image data.
|
|
752
|
+
img_resp.shot.acquisition_time.CopyFrom(
|
|
753
|
+
self.bosdyn_sdk_robot.time_sync.robot_timestamp_from_local_secs(
|
|
754
|
+
img_time_seconds))
|
|
713
755
|
|
|
714
|
-
|
|
715
|
-
|
|
716
|
-
|
|
717
|
-
|
|
718
|
-
|
|
719
|
-
|
|
720
|
-
|
|
721
|
-
|
|
722
|
-
|
|
723
|
-
|
|
724
|
-
|
|
725
|
-
img_resp.status
|
|
726
|
-
|
|
727
|
-
|
|
728
|
-
|
|
729
|
-
|
|
730
|
-
|
|
731
|
-
# Convert the image capture time from the local clock time into the robot's time. Then set it as
|
|
732
|
-
# the acquisition timestamp for the image data.
|
|
733
|
-
img_resp.shot.acquisition_time.CopyFrom(
|
|
734
|
-
self.bosdyn_sdk_robot.time_sync.robot_timestamp_from_local_secs(img_time_seconds))
|
|
735
|
-
|
|
736
|
-
img_resp.shot.image.rows = img_resp.source.rows
|
|
737
|
-
img_resp.shot.image.cols = img_resp.source.cols
|
|
738
|
-
|
|
739
|
-
# Set the image data.
|
|
740
|
-
img_resp.shot.image.format = img_req.image_format
|
|
741
|
-
decode_status = self._set_format_and_decode(captured_image, img_resp.shot.image,
|
|
742
|
-
img_req)
|
|
743
|
-
if decode_status != image_pb2.ImageResponse.STATUS_OK:
|
|
744
|
-
img_resp.status = decode_status
|
|
745
|
-
|
|
746
|
-
# Set that we successfully got the image.
|
|
747
|
-
if img_resp.status == image_pb2.ImageResponse.STATUS_UNKNOWN:
|
|
748
|
-
img_resp.status = image_pb2.ImageResponse.STATUS_OK
|
|
749
|
-
|
|
750
|
-
# No header error codes, so set the response header as CODE_OK.
|
|
751
|
-
populate_response_header(response, request)
|
|
756
|
+
img_resp.shot.image.rows = img_resp.source.rows
|
|
757
|
+
img_resp.shot.image.cols = img_resp.source.cols
|
|
758
|
+
|
|
759
|
+
# Set the image data.
|
|
760
|
+
img_resp.shot.image.format = img_req.image_format
|
|
761
|
+
decode_status = self._set_format_and_decode(captured_image, img_resp.shot.image,
|
|
762
|
+
img_req)
|
|
763
|
+
if decode_status != image_pb2.ImageResponse.STATUS_OK:
|
|
764
|
+
img_resp.status = decode_status
|
|
765
|
+
|
|
766
|
+
# Set that we successfully got the image.
|
|
767
|
+
if img_resp.status == image_pb2.ImageResponse.STATUS_UNKNOWN:
|
|
768
|
+
img_resp.status = image_pb2.ImageResponse.STATUS_OK
|
|
769
|
+
|
|
770
|
+
# No header error codes, so set the response header as CODE_OK.
|
|
771
|
+
populate_response_header(response, request)
|
|
752
772
|
return response
|
|
753
773
|
|
|
754
774
|
def __del__(self):
|
bosdyn/client/keepalive.py
CHANGED
|
@@ -19,6 +19,7 @@ import bosdyn.util
|
|
|
19
19
|
from bosdyn.api.keepalive import keepalive_pb2, keepalive_service_pb2_grpc
|
|
20
20
|
from bosdyn.client.common import (BaseClient, common_header_errors, error_factory, error_pair,
|
|
21
21
|
handle_common_header_errors, handle_unset_status_error)
|
|
22
|
+
from bosdyn.client.error_callback_result import ErrorCallbackResult
|
|
22
23
|
from bosdyn.client.exceptions import ResponseError, RetryableRpcError
|
|
23
24
|
|
|
24
25
|
|
|
@@ -225,7 +226,7 @@ class PolicyKeepalive():
|
|
|
225
226
|
#pylint: disable=too-many-arguments
|
|
226
227
|
def __init__(self, client: KeepaliveClient, policy: Policy, rpc_timeout_seconds: float = None,
|
|
227
228
|
rpc_interval_seconds: float = None, logger: 'logging.Logger' = None,
|
|
228
|
-
remove_policy_on_exit: bool = False):
|
|
229
|
+
remove_policy_on_exit: bool = False, initial_retry_seconds: float = 1.0):
|
|
229
230
|
|
|
230
231
|
self.logger = logger or logging.getLogger()
|
|
231
232
|
self.remove_policy_on_exit = remove_policy_on_exit
|
|
@@ -238,6 +239,11 @@ class PolicyKeepalive():
|
|
|
238
239
|
# This will raise an exception if there's no action at all.
|
|
239
240
|
self._rpc_interval_seconds = rpc_interval_seconds or policy.shortest_action_delay() / 3
|
|
240
241
|
self._rpc_timeout_seconds = rpc_timeout_seconds
|
|
242
|
+
self._initial_retry_seconds = initial_retry_seconds
|
|
243
|
+
|
|
244
|
+
#: Callable[[Exception], ErrorCallbackResult] | None: Optional callback to be called when
|
|
245
|
+
#: an error occurs in the keepalive thread.
|
|
246
|
+
self.keepalive_error_callback = None
|
|
241
247
|
|
|
242
248
|
self._end_check_in_signal = threading.Event()
|
|
243
249
|
self._thread = threading.Thread(target=self._periodic_check_in)
|
|
@@ -277,24 +283,47 @@ class PolicyKeepalive():
|
|
|
277
283
|
self._end_check_in_signal.set()
|
|
278
284
|
|
|
279
285
|
def _periodic_check_in(self):
|
|
280
|
-
|
|
286
|
+
retry_interval = self._initial_retry_seconds
|
|
287
|
+
wait_time = self._rpc_interval_seconds
|
|
288
|
+
|
|
289
|
+
# Block and wait for the stop signal. If we receive it within the check-in period,
|
|
290
|
+
# leave the loop. Under normal conditions, wait up to self._check_in_period seconds, minus
|
|
291
|
+
# the RPC processing time. (values < 0 are OK and unblock immediately)
|
|
292
|
+
while not self._end_check_in_signal.wait(wait_time):
|
|
281
293
|
exec_start = time.time()
|
|
294
|
+
action = ErrorCallbackResult.RESUME_NORMAL_OPERATION
|
|
282
295
|
|
|
283
296
|
try:
|
|
284
297
|
self._check_in()
|
|
285
298
|
except RetryableRpcError as exc:
|
|
286
299
|
self.logger.warning('exception during check-in:\n%s\n', exc)
|
|
287
300
|
self.logger.info('continuing check-in')
|
|
288
|
-
|
|
301
|
+
except Exception as exc: # pylint: disable=broad-except
|
|
302
|
+
if self.keepalive_error_callback is not None:
|
|
303
|
+
action = ErrorCallbackResult.DEFAULT_ACTION
|
|
304
|
+
try:
|
|
305
|
+
action = self.keepalive_error_callback(exc)
|
|
306
|
+
except Exception: # pylint: disable=broad-except
|
|
307
|
+
self.logger.exception(
|
|
308
|
+
'Exception thrown in the provided keepalive error callback')
|
|
309
|
+
else:
|
|
310
|
+
raise
|
|
289
311
|
# How long did the RPC and processing of said RPC take?
|
|
290
312
|
exec_seconds = time.time() - exec_start
|
|
291
313
|
|
|
292
|
-
|
|
293
|
-
|
|
294
|
-
# Wait up to self._check_in_period seconds, minus the RPC processing time.
|
|
295
|
-
# (values < 0 are OK and will return immediately)
|
|
296
|
-
if self._end_check_in_signal.wait(self._rpc_interval_seconds - exec_seconds):
|
|
314
|
+
if action == ErrorCallbackResult.ABORT:
|
|
315
|
+
self.logger.warning('Callback directed the keepalive thread to exit.')
|
|
297
316
|
break
|
|
317
|
+
elif action == ErrorCallbackResult.RETRY_IMMEDIATELY:
|
|
318
|
+
wait_time = 0
|
|
319
|
+
continue
|
|
320
|
+
elif action == ErrorCallbackResult.RETRY_WITH_EXPONENTIAL_BACK_OFF:
|
|
321
|
+
wait_time = retry_interval - exec_seconds
|
|
322
|
+
retry_interval = min(2 * retry_interval, self._rpc_interval_seconds)
|
|
323
|
+
else:
|
|
324
|
+
# Success path, or default action (resume normal operation)
|
|
325
|
+
wait_time = self._rpc_interval_seconds - exec_seconds
|
|
326
|
+
retry_interval = self._initial_retry_seconds
|
|
298
327
|
self.logger.info('Policy check-in stopped')
|
|
299
328
|
|
|
300
329
|
|
bosdyn/client/log_status.py
CHANGED
|
@@ -4,6 +4,8 @@
|
|
|
4
4
|
# is subject to the terms and conditions of the Boston Dynamics Software
|
|
5
5
|
# Development Kit License (20191101-BDSDK-SL).
|
|
6
6
|
|
|
7
|
+
# Boston Dynamics, Inc. Confidential Information.
|
|
8
|
+
# Copyright 2025. All Rights Reserved.
|
|
7
9
|
"""Client for the log-status service.
|
|
8
10
|
|
|
9
11
|
This allows client code to start, extend or terminate experiment logs and start retro logs.
|
|
@@ -183,6 +185,7 @@ class LogStatusClient(BaseClient):
|
|
|
183
185
|
**kwargs)
|
|
184
186
|
|
|
185
187
|
|
|
188
|
+
|
|
186
189
|
_GET_LOG_STATUS_STATUS_TO_ERROR = \
|
|
187
190
|
collections.defaultdict(lambda: (LogStatusResponseError, None))
|
|
188
191
|
_GET_LOG_STATUS_STATUS_TO_ERROR.update({
|
|
@@ -232,6 +235,7 @@ _TERMINATE_LOG_STATUS_TO_ERROR.update({
|
|
|
232
235
|
})
|
|
233
236
|
|
|
234
237
|
|
|
238
|
+
|
|
235
239
|
@handle_common_header_errors
|
|
236
240
|
@handle_unset_status_error(unset='STATUS_UNKNOWN')
|
|
237
241
|
def get_log_status_error(response):
|
|
@@ -284,3 +288,5 @@ def terminate_log_error(response):
|
|
|
284
288
|
return error_factory(response, response.status,
|
|
285
289
|
status_to_string=log_status.TerminateLogResponse.Status.Name,
|
|
286
290
|
status_to_error=_TERMINATE_LOG_STATUS_TO_ERROR)
|
|
291
|
+
|
|
292
|
+
|
bosdyn/client/math_helpers.py
CHANGED
|
@@ -408,6 +408,10 @@ class SE2Velocity(object):
|
|
|
408
408
|
+ str(se2_vel_vector.shape[0]))
|
|
409
409
|
return None
|
|
410
410
|
else:
|
|
411
|
+
if (isinstance(se2_vel_vector[0], numpy.ndarray)):
|
|
412
|
+
return SE2Velocity(x=se2_vel_vector[0][0], y=se2_vel_vector[1][0],
|
|
413
|
+
angular=se2_vel_vector[2][0])
|
|
414
|
+
|
|
411
415
|
return SE2Velocity(x=se2_vel_vector[0], y=se2_vel_vector[1],
|
|
412
416
|
angular=se2_vel_vector[2])
|
|
413
417
|
|
|
@@ -521,6 +525,11 @@ class SE3Velocity(object):
|
|
|
521
525
|
+ str(se3_vel_vector.shape[0]))
|
|
522
526
|
return None
|
|
523
527
|
else:
|
|
528
|
+
if (isinstance(se3_vel_vector[0], numpy.ndarray)):
|
|
529
|
+
return SE3Velocity(lin_x=se3_vel_vector[0][0], lin_y=se3_vel_vector[1][0],
|
|
530
|
+
lin_z=se3_vel_vector[2][0], ang_x=se3_vel_vector[3][0],
|
|
531
|
+
ang_y=se3_vel_vector[4][0], ang_z=se3_vel_vector[5][0])
|
|
532
|
+
|
|
524
533
|
return SE3Velocity(lin_x=se3_vel_vector[0], lin_y=se3_vel_vector[1],
|
|
525
534
|
lin_z=se3_vel_vector[2], ang_x=se3_vel_vector[3],
|
|
526
535
|
ang_y=se3_vel_vector[4], ang_z=se3_vel_vector[5])
|
|
@@ -651,6 +660,10 @@ class SE3Pose(object):
|
|
|
651
660
|
ret[0:3, 3] = [self.x, self.y, self.z]
|
|
652
661
|
return ret
|
|
653
662
|
|
|
663
|
+
def translation_norm(self):
|
|
664
|
+
"""Calculates the Euclidean norm (magnitude) of the translation component pose."""
|
|
665
|
+
return math.sqrt(self.x**2 + self.y**2 + self.z**2)
|
|
666
|
+
|
|
654
667
|
def mult(self, se3pose):
|
|
655
668
|
"""
|
|
656
669
|
Computes the multiplication between the current math_helpers.SE3Pose and the input se3pose.
|
|
@@ -1100,6 +1113,11 @@ def skew_matrix_2d(vec2_proto):
|
|
|
1100
1113
|
return numpy.array([[vec2_proto.y, -vec2_proto.x]])
|
|
1101
1114
|
|
|
1102
1115
|
|
|
1116
|
+
def matrix_from_proto(proto):
|
|
1117
|
+
"""Converts a geometry_pb2.Matrix or geometry_pb2.Matrixf to a numpy array."""
|
|
1118
|
+
return numpy.array(proto.values).reshape(proto.rows, proto.cols)
|
|
1119
|
+
|
|
1120
|
+
|
|
1103
1121
|
def transform_se2velocity(a_adjoint_b_matrix, se2_velocity_in_b):
|
|
1104
1122
|
"""
|
|
1105
1123
|
Changes the frame that the SE(2) Velocity is expressed in. More specifically, it converts the
|
|
@@ -20,6 +20,7 @@ from bosdyn.client import (ResponseError, RetryableUnavailableError, TimedOutErr
|
|
|
20
20
|
TooManyRequestsError)
|
|
21
21
|
from bosdyn.client.common import (BaseClient, error_factory, handle_common_header_errors,
|
|
22
22
|
handle_lease_use_result_errors, handle_unset_status_error)
|
|
23
|
+
from bosdyn.client.error_callback_result import ErrorCallbackResult
|
|
23
24
|
|
|
24
25
|
LOGGER = logging.getLogger('payload_registration_client')
|
|
25
26
|
|
|
@@ -381,16 +382,23 @@ class PayloadRegistrationKeepAlive(object):
|
|
|
381
382
|
class name is acquired.
|
|
382
383
|
rpc_timeout_secs: Number of seconds to wait for a pay_reg_client RPC. Defaults to None,
|
|
383
384
|
for no timeout.
|
|
385
|
+
initial_retry_seconds: Number of seconds to wait before retrying registration that failed
|
|
386
|
+
due to unhandled errors including RPC transport issues.
|
|
384
387
|
"""
|
|
385
388
|
|
|
386
389
|
def __init__(self, pay_reg_client, payload, secret, registration_interval_secs=30, logger=None,
|
|
387
|
-
rpc_timeout_secs=None):
|
|
390
|
+
rpc_timeout_secs=None, initial_retry_seconds=1.0):
|
|
388
391
|
self.pay_reg_client = pay_reg_client
|
|
389
392
|
self.payload = payload
|
|
390
393
|
self.secret = secret
|
|
391
394
|
self._registration_interval_secs = registration_interval_secs
|
|
392
395
|
self.logger = logger or logging.getLogger(self.__class__.__name__)
|
|
393
396
|
self._rpc_timeout_secs = rpc_timeout_secs
|
|
397
|
+
self._initial_retry_seconds = initial_retry_seconds
|
|
398
|
+
|
|
399
|
+
#: Callable[[Exception], ErrorCallbackResult] | None: Optional callback to be called when
|
|
400
|
+
#: an error occurs in the re-registration thread.
|
|
401
|
+
self.reregistration_error_callback = None
|
|
394
402
|
|
|
395
403
|
# Configure the thread to do re-registration.
|
|
396
404
|
self._end_reregister_signal = threading.Event()
|
|
@@ -423,6 +431,7 @@ class PayloadRegistrationKeepAlive(object):
|
|
|
423
431
|
|
|
424
432
|
# This will raise an exception if the thread has already started.
|
|
425
433
|
self._thread.start()
|
|
434
|
+
return self
|
|
426
435
|
|
|
427
436
|
def is_alive(self):
|
|
428
437
|
"""Are we still periodically re-registering?
|
|
@@ -445,8 +454,12 @@ class PayloadRegistrationKeepAlive(object):
|
|
|
445
454
|
RpcError: Problem communicating with the robot.
|
|
446
455
|
"""
|
|
447
456
|
self.logger.info('Starting registration loop')
|
|
448
|
-
|
|
457
|
+
retry_interval = self._initial_retry_seconds
|
|
458
|
+
wait_time = self._registration_interval_secs
|
|
459
|
+
|
|
460
|
+
while not self._end_reregister_signal.wait(wait_time):
|
|
449
461
|
exec_start = time.time()
|
|
462
|
+
action = ErrorCallbackResult.RESUME_NORMAL_OPERATION
|
|
450
463
|
try:
|
|
451
464
|
self.pay_reg_client.register_payload(self.payload, self.secret)
|
|
452
465
|
except PayloadAlreadyExistsError:
|
|
@@ -459,10 +472,31 @@ class PayloadRegistrationKeepAlive(object):
|
|
|
459
472
|
self.logger.warning('Timed out, timeout set to "{}"'.format(self._rpc_timeout_secs))
|
|
460
473
|
except TooManyRequestsError:
|
|
461
474
|
self.logger.warning("Too many requests error")
|
|
462
|
-
except Exception as exc:
|
|
463
|
-
#
|
|
464
|
-
|
|
475
|
+
except Exception as exc: # pylint: disable=broad-except
|
|
476
|
+
# If the application provided an error handler, give it an opportunity to resolve
|
|
477
|
+
# the issue.
|
|
478
|
+
if self.reregistration_error_callback is not None:
|
|
479
|
+
action = ErrorCallbackResult.DEFAULT_ACTION
|
|
480
|
+
try:
|
|
481
|
+
action = self.reregistration_error_callback(exc)
|
|
482
|
+
except Exception: # pylint: disable=broad-except
|
|
483
|
+
self.logger.exception(
|
|
484
|
+
'Exception thrown in the provided re-registration error callback ')
|
|
485
|
+
else:
|
|
486
|
+
# Log all other exceptions, but continue looping in hopes that it resolves itself
|
|
487
|
+
self.logger.exception('Caught general exception.')
|
|
488
|
+
|
|
465
489
|
exec_sec = time.time() - exec_start
|
|
466
|
-
if
|
|
490
|
+
if action == ErrorCallbackResult.ABORT:
|
|
491
|
+
self.logger.warning('Callback directed the re-registration loop to exit.')
|
|
467
492
|
break
|
|
493
|
+
elif action == ErrorCallbackResult.RETRY_IMMEDIATELY:
|
|
494
|
+
wait_time = 0.0
|
|
495
|
+
elif action == ErrorCallbackResult.RETRY_WITH_EXPONENTIAL_BACK_OFF:
|
|
496
|
+
wait_time = retry_interval - exec_sec
|
|
497
|
+
retry_interval = min(retry_interval * 2, self._registration_interval_secs)
|
|
498
|
+
else:
|
|
499
|
+
# Success path, or default action (resume normal operation)
|
|
500
|
+
wait_time = self._registration_interval_secs - exec_sec
|
|
501
|
+
retry_interval = self._initial_retry_seconds
|
|
468
502
|
self.logger.info('Re-registration stopped')
|
|
@@ -0,0 +1,185 @@
|
|
|
1
|
+
# Copyright (c) 2023 Boston Dynamics, Inc. All rights reserved.
|
|
2
|
+
#
|
|
3
|
+
# Downloading, reproducing, distributing or otherwise using the SDK Software
|
|
4
|
+
# is subject to the terms and conditions of the Boston Dynamics Software
|
|
5
|
+
# Development Kit License (20191101-BDSDK-SL).
|
|
6
|
+
|
|
7
|
+
"""Payload software update service gRPC client.
|
|
8
|
+
|
|
9
|
+
This is used by Spot payloads to coordinate updates of their own software with Spot.
|
|
10
|
+
"""
|
|
11
|
+
|
|
12
|
+
import datetime
|
|
13
|
+
|
|
14
|
+
from google.protobuf import timestamp_pb2
|
|
15
|
+
|
|
16
|
+
from bosdyn.api.payload_software_update_pb2 import (GetAvailableSoftwareUpdatesRequest,
|
|
17
|
+
SendCurrentVersionInfoRequest,
|
|
18
|
+
SendSoftwareUpdateStatusRequest)
|
|
19
|
+
from bosdyn.api.payload_software_update_service_pb2_grpc import PayloadSoftwareUpdateServiceStub
|
|
20
|
+
from bosdyn.api.robot_id_pb2 import SoftwareVersion
|
|
21
|
+
from bosdyn.api.software_package_pb2 import SoftwarePackageVersion, SoftwareUpdateStatus
|
|
22
|
+
from bosdyn.client.common import BaseClient
|
|
23
|
+
|
|
24
|
+
|
|
25
|
+
class PayloadSoftwareUpdateClient(BaseClient):
|
|
26
|
+
"""A client for payloads to coordinate software updates with a robot."""
|
|
27
|
+
|
|
28
|
+
default_service_name = 'payload-software-update'
|
|
29
|
+
service_type = 'bosdyn.api.PayloadSoftwareUpdateService'
|
|
30
|
+
|
|
31
|
+
def __init__(self):
|
|
32
|
+
super(PayloadSoftwareUpdateClient, self).__init__(PayloadSoftwareUpdateServiceStub)
|
|
33
|
+
|
|
34
|
+
def send_current_software_info(
|
|
35
|
+
self, package_name: str, version: SoftwareVersion | list[int],
|
|
36
|
+
release_date: float | timestamp_pb2.Timestamp | datetime.datetime, build_id: str,
|
|
37
|
+
**kwargs):
|
|
38
|
+
"""Send version information about the currently installed payload software to Spot.
|
|
39
|
+
|
|
40
|
+
Args:
|
|
41
|
+
package_name: Name of the package, e.g., "coreio"
|
|
42
|
+
version: Current semantic version of the installed software.
|
|
43
|
+
release_date: Release date of the currently installed software.
|
|
44
|
+
build_id: Unique identifier of the build.
|
|
45
|
+
|
|
46
|
+
Returns:
|
|
47
|
+
SendCurrentVersionInfoResponse: The response object from Spot. Currently this message
|
|
48
|
+
contains no information other than a standard response header.
|
|
49
|
+
|
|
50
|
+
Raises:
|
|
51
|
+
RpcError: Problem communicating with the robot.
|
|
52
|
+
"""
|
|
53
|
+
request = self.make_info_request(package_name, version, release_date, build_id)
|
|
54
|
+
return self.call(self._stub.SendCurrentVersionInfo, request, **kwargs)
|
|
55
|
+
|
|
56
|
+
def send_current_software_info_async(
|
|
57
|
+
self, package_name: str, version: SoftwareVersion | list[int],
|
|
58
|
+
release_date: float | timestamp_pb2.Timestamp | datetime.datetime, build_id: str,
|
|
59
|
+
**kwargs):
|
|
60
|
+
"""Async version of send_current_software_info().
|
|
61
|
+
|
|
62
|
+
Args:
|
|
63
|
+
package_name: Name of the package, e.g., "coreio"
|
|
64
|
+
version: Current semantic version of the installed software.
|
|
65
|
+
release_date: Release date of the currently installed software.
|
|
66
|
+
build_id: Unique identifier of the build.
|
|
67
|
+
|
|
68
|
+
Returns:
|
|
69
|
+
SendCurrentVersionInfoResponse: The response object from Spot. Currently this message
|
|
70
|
+
contains no information other than a standard response header.
|
|
71
|
+
Raises:
|
|
72
|
+
RpcError: Problem communicating with the robot.
|
|
73
|
+
"""
|
|
74
|
+
request = self.make_info_request(package_name, version, release_date, build_id)
|
|
75
|
+
return self.call_async(self._stub.SendCurrentVersionInfo, request, **kwargs)
|
|
76
|
+
|
|
77
|
+
def get_available_updates(self, package_names: str | list[str], **kw_args):
|
|
78
|
+
"""Get a list of package information for the named package(s).
|
|
79
|
+
|
|
80
|
+
Args:
|
|
81
|
+
package_names: The package name or list of package names to query.
|
|
82
|
+
|
|
83
|
+
Returns:
|
|
84
|
+
GetAvailableSoftwareUpdatesResponse: The response object from Spot containing the
|
|
85
|
+
version info for packages cached by Spot.
|
|
86
|
+
|
|
87
|
+
Raises:
|
|
88
|
+
RpcError: Problem communicating with the robot.
|
|
89
|
+
"""
|
|
90
|
+
if not isinstance(package_names, list):
|
|
91
|
+
package_names = [package_names]
|
|
92
|
+
request = GetAvailableSoftwareUpdatesRequest(package_names=package_names)
|
|
93
|
+
return self.call(self._stub.GetAvailableSoftwareUpdates, request, **kw_args)
|
|
94
|
+
|
|
95
|
+
def get_available_updates_async(self, package_names: str | list[str], **kw_args):
|
|
96
|
+
"""Async version of get_available_updates().
|
|
97
|
+
|
|
98
|
+
Args:
|
|
99
|
+
package_names: The package name or list of package names to query.
|
|
100
|
+
|
|
101
|
+
Returns:
|
|
102
|
+
GetAvailableSoftwareUpdatesResponse: The response object from Spot containing the
|
|
103
|
+
version info for packages cached by Spot.
|
|
104
|
+
|
|
105
|
+
Raises:
|
|
106
|
+
RpcError: Problem communicating with the robot.
|
|
107
|
+
"""
|
|
108
|
+
if not isinstance(package_names, list):
|
|
109
|
+
package_names = [package_names]
|
|
110
|
+
request = GetAvailableSoftwareUpdatesRequest(package_names=package_names)
|
|
111
|
+
return self.call_async(self._stub.GetAvailableSoftwareUpdates, request, **kw_args)
|
|
112
|
+
|
|
113
|
+
def send_installation_status(self, package_name: str, status: SoftwareUpdateStatus.Status,
|
|
114
|
+
error_code: SoftwareUpdateStatus.ErrorCode, **kw_args):
|
|
115
|
+
"""Send a status update of a payload software installation operation to Spot.
|
|
116
|
+
|
|
117
|
+
Args:
|
|
118
|
+
package_name: Name of the package being updated
|
|
119
|
+
status: Status code of installation operation
|
|
120
|
+
error_code: Error code of the installation operation, or ERROR_NONE if no error has
|
|
121
|
+
been encountered.
|
|
122
|
+
|
|
123
|
+
Returns:
|
|
124
|
+
SendSoftwareUpdateStatusResponse: The response object from Spot. Currently this message
|
|
125
|
+
contains nothing beyond a standard response header.
|
|
126
|
+
|
|
127
|
+
Raises:
|
|
128
|
+
RpcError: Problem communicating with the robot.
|
|
129
|
+
"""
|
|
130
|
+
status = SoftwareUpdateStatus(package_name=package_name, status=status,
|
|
131
|
+
error_code=error_code)
|
|
132
|
+
request = SendSoftwareUpdateStatusRequest(update_status=status)
|
|
133
|
+
return self.call(self._stub.SendSoftwareUpdateStatus, request, **kw_args)
|
|
134
|
+
|
|
135
|
+
def send_installation_status_async(self, package_name: str, status: SoftwareUpdateStatus.Status,
|
|
136
|
+
error_code: SoftwareUpdateStatus.ErrorCode, **kw_args):
|
|
137
|
+
"""Async version of send_installation_status().
|
|
138
|
+
|
|
139
|
+
Args:
|
|
140
|
+
package_name: Name of the package being updated
|
|
141
|
+
status: Status code of installation operation
|
|
142
|
+
error_code: Error code of the installation operation, or ERROR_NONE if no error has
|
|
143
|
+
been encountered.
|
|
144
|
+
|
|
145
|
+
Returns:
|
|
146
|
+
SendSoftwareUpdateStatusResponse: The response object from Spot. Currently this message
|
|
147
|
+
contains nothing beyond a standard response header.
|
|
148
|
+
|
|
149
|
+
Raises:
|
|
150
|
+
RpcError: Problem communicating with the robot.
|
|
151
|
+
"""
|
|
152
|
+
status = SoftwareUpdateStatus(package_name=package_name, status=status,
|
|
153
|
+
error_code=error_code)
|
|
154
|
+
request = SendSoftwareUpdateStatusRequest(update_status=status)
|
|
155
|
+
return self.call_async(self._stub.SendSoftwareUpdateStatus, request, **kw_args)
|
|
156
|
+
|
|
157
|
+
@staticmethod
|
|
158
|
+
def make_info_request(package_name: str, version: SoftwareVersion,
|
|
159
|
+
release_date: float | timestamp_pb2.Timestamp | datetime.datetime,
|
|
160
|
+
build_id: str):
|
|
161
|
+
"""Make a SendCurrentVersionInfoRequest message using the supplied information.
|
|
162
|
+
|
|
163
|
+
Args:
|
|
164
|
+
package_name: Name of the package, e.g., "coreio"
|
|
165
|
+
version: Current semantic version of the installed software.
|
|
166
|
+
release_date: Release date of the currently installed software.
|
|
167
|
+
build_id: Unique identifier of the build.
|
|
168
|
+
|
|
169
|
+
Returns:
|
|
170
|
+
SendCurrentVersionInfoRequest: Message communicating to Spot the version information
|
|
171
|
+
of the currently installed payload software.
|
|
172
|
+
"""
|
|
173
|
+
if not isinstance(version, SoftwareVersion):
|
|
174
|
+
version = SoftwareVersion(major_version=version[0], minor_version=version[1],
|
|
175
|
+
patch_level=version[2])
|
|
176
|
+
if isinstance(release_date, float):
|
|
177
|
+
release_date = datetime.datetime.fromtimestamp(release_date)
|
|
178
|
+
if isinstance(release_date, datetime.datetime):
|
|
179
|
+
pb_release_date = timestamp_pb2.Timestamp()
|
|
180
|
+
pb_release_date.FromDatetime(release_date)
|
|
181
|
+
release_date = pb_release_date
|
|
182
|
+
|
|
183
|
+
return SendCurrentVersionInfoRequest(
|
|
184
|
+
package_version=SoftwarePackageVersion(package_name=package_name, version=version,
|
|
185
|
+
release_date=release_date, build_id=build_id))
|