Merge remote-tracking branch 'upstream/master' into mhidalgo-bdai/5.0.1

Author: mhidalgo-rai

README.md

@@ -21,7 +21,7 @@ The official Spot SDK documentation also contains information relevant to the C+
 - [Payload developer documentation](https://dev.bostondynamics.com/docs/payload/readme). Payloads add additional sensing, communication, and control capabilities beyond what the base platform provides. The Payload ICD covers the mechanical, electrical, and software interfaces that Spot supports.
 - [Spot API protocol definition](https://dev.bostondynamics.com/docs/protos/readme). This reference guide covers the details of the protocol applications used to communicate to Spot. Application developers who wish to use a language other than Python can implement clients that speak the protocol.
 
-This is version 5.0.0 of the C++ SDK. Please review the [Release Notes](docs/cpp_release_notes.md) to see what has changed.
+This is version 5.0.1 of the C++ SDK. Please review the [Release Notes](docs/cpp_release_notes.md) to see what has changed.
 
 ## Contents
 

cpp/CMakeLists.txt

@@ -7,7 +7,7 @@
 # This file is autogenerated.
 
 cmake_minimum_required (VERSION 3.10.2)
-project (bosdyn VERSION 5.0.0)
+project (bosdyn VERSION 5.0.1)
 
 # Dependencies:
 find_package(Protobuf REQUIRED)

cpp/bosdyn/client/directory/service_wait.cpp

@@ -0,0 +1,81 @@
+/**
+ * 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).
+ */
+
+
+#include "service_wait.h"
+#include "bosdyn/common/success_condition.h"
+
+namespace bosdyn::directory::util {
+
+// Error category for WaitError.
+class WaitCategory : public std::error_category {
+ public:
+    const char* name() const noexcept override { return "wait_error"; }
+    std::string message(int ev) const override {
+        switch (static_cast<WaitError>(ev)) {
+            case WaitError::kSuccess:
+                return "Success";
+            case WaitError::kTimeout:
+                return "Timeout";
+        }
+        return "Unknown";
+    }
+    std::error_condition default_error_condition(int ev) const noexcept override {
+        return std::error_condition(ev, *this);
+    }
+    bool equivalent(int code, const std::error_condition& condition) const noexcept override {
+        if (condition == SuccessCondition::Success) {
+            return code == static_cast<int>(WaitError::kSuccess);
+        }
+        return false;
+    }
+};
+
+const WaitCategory s_wait_category{};
+
+std::error_code make_error_code(WaitError value) {
+    return {static_cast<int>(value), s_wait_category};
+}
+
+WaitResult WaitForAllServices(const std::set<std::string>& service_names,
+                              bosdyn::client::Robot& robot, const bosdyn::common::Duration& timeout,
+                              const bosdyn::common::Duration& interval) {
+    auto start_time = bosdyn::common::NowTimePoint();
+    auto end_time = start_time + timeout;
+    WaitResult result;
+
+    do {
+        bosdyn::client::Result<std::vector<::bosdyn::api::ServiceEntry>> list_result =
+            robot.ListServices();
+        if (!list_result) {
+            if (list_result.status.code() != RetryableRPCCondition::Retryable) {
+                // Return immediately if the error is not retryable.
+                result.status = list_result.status;
+                return result;
+            }
+            // Retryable error, keep trying.
+        } else {
+            result.missing_services = service_names;
+            for (const api::ServiceEntry& entry : list_result.response) {
+                result.missing_services.erase(entry.name());
+            }
+            if (result.missing_services.empty()) {
+                result.status = {WaitError::kSuccess};
+                return result;
+            }
+        }
+        std::this_thread::sleep_for(interval);
+    } while (bosdyn::common::NowTimePoint() < end_time);
+
+    // Failed to find all services.  Return the ones that are missing according to the latest set of
+    // found services.
+    result.status = {WaitError::kTimeout};
+    return result;
+}
+
+}  // namespace bosdyn::directory::util

cpp/bosdyn/client/directory/service_wait.h

@@ -0,0 +1,52 @@
+/**
+ * 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).
+ */
+
+
+#pragma once
+
+#include <set>
+
+#include "bosdyn/client/robot/robot.h"
+#include "bosdyn/common/time.h"
+
+namespace bosdyn::directory::util {
+
+enum class WaitError {
+    kSuccess = 0,
+    kTimeout = 1,
+};
+
+struct WaitResult {
+    // Normally a WaitError, but may be an RPC failure.
+    bosdyn::common::Status status;
+    // List of any services that were missing when a timeout was hit.
+    std::set<std::string> missing_services;
+};
+
+/**
+ * Block until all requested services are registered with the directory.  Useful during startup to
+ * wait until the services you depend on are all running before you start creating clients.
+ *
+ * The status code will normally be a WaitError for success or timeout.  If it fails with a timeout,
+ * the services that were not yet registered will be returned in missing_services.  If the calls to
+ * list the services fail in an un-retryable manner, that error code will be returned in the status
+ * instead.
+ */
+WaitResult WaitForAllServices(
+    const std::set<std::string>& service_names, bosdyn::client::Robot& robot,
+    const bosdyn::common::Duration& timeout,
+    const bosdyn::common::Duration& interval = std::chrono::milliseconds(100));
+
+std::error_code make_error_code(WaitError value);
+
+}  // namespace bosdyn::directory::util
+
+namespace std {
+template <>
+struct is_error_code_enum<bosdyn::directory::util::WaitError> : true_type {};
+}  // namespace std

cpp/bosdyn/client/directory_registration/directory_registration_helpers.cpp

@@ -20,22 +20,27 @@ namespace client {
 DirectoryRegistrationKeepAlive::DirectoryRegistrationKeepAlive(
     DirectoryRegistrationClient* directory_registration_client,
     const ::bosdyn::api::ServiceEntry& service_entry, const ::bosdyn::api::Endpoint& endpoint,
-    ::bosdyn::common::Duration rpc_interval, FaultClient* fault_client)
+    ::bosdyn::common::Duration rpc_interval, FaultClient* fault_client,
+    std::function<ErrorCallbackResult(const ::bosdyn::common::Status&)> error_callback,
+    ::bosdyn::common::Duration registration_initial_retry_interval)
     : m_directory_registration_client(directory_registration_client),
       m_service_entry(service_entry),
       m_endpoint(endpoint),
       m_registration_interval(rpc_interval),
+      m_registration_initial_retry_interval(registration_initial_retry_interval),
       m_thread(),
+      m_periodic_thread_helper(std::make_unique<PeriodicThreadHelper>()),
       m_thread_stopped(true),
-      m_fault_client(fault_client) {}
+      m_fault_client(fault_client),
+      m_error_callback(error_callback) {}
+
 
 DirectoryRegistrationKeepAlive::~DirectoryRegistrationKeepAlive() {
     Shutdown();
     Unregister().IgnoreError();
 }
 
 void DirectoryRegistrationKeepAlive::Start(const std::vector<std::string>& fault_attributes) {
-    m_should_exit = false;
     m_thread_stopped = false;
 
     // Populate the registration fault with default values.
@@ -61,16 +66,20 @@ void DirectoryRegistrationKeepAlive::Start(const std::vector<std::string>& fault
 bool DirectoryRegistrationKeepAlive::IsAlive() const { return !m_thread_stopped; }
 
 void DirectoryRegistrationKeepAlive::Shutdown() {
-    m_should_exit = true;
-    m_thread.join();
+    m_periodic_thread_helper->Stop();
+    if (m_thread.joinable()) {
+        m_thread.join();
+    }
 }
 
 UnregisterServiceResultType DirectoryRegistrationKeepAlive::Unregister() {
     return m_directory_registration_client->UnregisterService(m_service_entry.name());
 }
 
 void DirectoryRegistrationKeepAlive::PeriodicReregisterThreadMethod() {
-    bool first_time = true;
+
+    ::bosdyn::common::Duration retry_interval = m_registration_initial_retry_interval;
+    ::bosdyn::common::Duration wait_interval = m_registration_interval;
 
     // Set up register and update requests
     ::bosdyn::api::RegisterServiceRequest register_service_request;
@@ -81,19 +90,15 @@ void DirectoryRegistrationKeepAlive::PeriodicReregisterThreadMethod() {
     update_service_request.mutable_service_entry()->CopyFrom(m_service_entry);
 
     // Continually attempt to register and update the service.
-    while (!m_should_exit) {
-        if (!first_time) {
-            std::this_thread::sleep_for(m_registration_interval);
-        }
-        first_time = false;
-
+    do {
         // Register service.
         auto res_register =
             m_directory_registration_client->RegisterService(register_service_request);
         // Successful registration. Service faults are automatically cleared for the new service
         // by the directory registration service.
         if (res_register) {
             m_registration_fault_active = false;
+            wait_interval = m_registration_interval;
             continue;
         }
         // If registration failed in a bad way.
@@ -102,6 +107,27 @@ void DirectoryRegistrationKeepAlive::PeriodicReregisterThreadMethod() {
             if (m_fault_client && m_fault_client->TriggerServiceFault(m_service_fault)) {
                 m_registration_fault_active = true;
             }
+            auto action = ErrorCallbackResult::kResumeNormalOperation;
+            if (m_error_callback) {
+                try {
+                    action = m_error_callback(res_register.status);
+                } catch (const std::exception& e) {
+                    std::cerr << "Exception thrown in error callback: " << e.what() << std::endl;
+                }
+            }
+            if (action == ErrorCallbackResult::kAbort) {
+                break;
+            }
+            if (action == ErrorCallbackResult::kRetryImmediately) {
+                wait_interval = std::chrono::seconds(0);
+            } else if (action == ErrorCallbackResult::kRetryWithExponentialBackOff) {
+                // Exponentially increase the retry interval.
+                wait_interval = retry_interval;
+                retry_interval = std::min(retry_interval * 2, m_registration_interval);
+            } else {
+                // Default action is to continue with the next iteration.
+                wait_interval = m_registration_interval;
+            }
             continue;
         }
 
@@ -112,6 +138,27 @@ void DirectoryRegistrationKeepAlive::PeriodicReregisterThreadMethod() {
             if (m_fault_client && m_fault_client->TriggerServiceFault(m_service_fault)) {
                 m_registration_fault_active = true;
             }
+            auto action = ErrorCallbackResult::kResumeNormalOperation;
+            if (m_error_callback) {
+                try {
+                    action = m_error_callback(res_register.status);
+                } catch (const std::exception& e) {
+                    std::cerr << "Exception thrown in error callback: " << e.what() << std::endl;
+                }
+            }
+            if (action == ErrorCallbackResult::kAbort) {
+                break;
+            }
+            if (action == ErrorCallbackResult::kRetryImmediately) {
+                wait_interval = std::chrono::seconds(0);
+            } else if (action == ErrorCallbackResult::kRetryWithExponentialBackOff) {
+                // Exponentially increase the retry interval.
+                wait_interval = retry_interval;
+                retry_interval = std::min(retry_interval * 2, m_registration_interval);
+            } else {
+                // Default action is to continue with the next iteration.
+                wait_interval = m_registration_interval;
+            }
             continue;
         }
 
@@ -121,9 +168,12 @@ void DirectoryRegistrationKeepAlive::PeriodicReregisterThreadMethod() {
                 m_registration_fault_active = false;
             }
         }
-    }
+        wait_interval = m_registration_interval;
+        retry_interval = m_registration_initial_retry_interval;
+    } while (m_periodic_thread_helper->WaitForInterval(wait_interval));
 
     m_thread_stopped = true;
+    m_periodic_thread_helper->Stop();
 }
 
 }  // namespace client

cpp/bosdyn/client/directory_registration/directory_registration_helpers.h

@@ -9,12 +9,17 @@
 
 #pragma once
 
-#include <mutex>
+#include <functional>
+#include <memory>
+#include <string>
 #include <thread>
+#include <vector>
 
 #include "directory_registration_client.h"
+#include "bosdyn/client/error_callback/error_callback_result.h"
 #include "bosdyn/client/fault/fault_client.h"
 #include "bosdyn/client/service_client/common_result_types.h"
+#include "bosdyn/client/util/periodic_thread_helper.h"
 
 #include <bosdyn/api/directory.pb.h>
 
@@ -32,7 +37,9 @@ class DirectoryRegistrationKeepAlive {
         DirectoryRegistrationClient* directory_registration_client,
         const ::bosdyn::api::ServiceEntry& service_entry, const ::bosdyn::api::Endpoint& endpoint,
         ::bosdyn::common::Duration rpc_interval = std::chrono::seconds(30),
-        FaultClient* fault_client = nullptr);
+        FaultClient* fault_client = nullptr,
+        std::function<ErrorCallbackResult(const ::bosdyn::common::Status&)> error_callback = {},
+        ::bosdyn::common::Duration registration_initial_retry_interval = std::chrono::seconds(1));
 
     ~DirectoryRegistrationKeepAlive();
 
@@ -52,18 +59,22 @@ class DirectoryRegistrationKeepAlive {
     // Background thread that continually registers/updates the service.
     void PeriodicReregisterThreadMethod();
 
+    // Wait for the refresh interval and coordinate with the destructor on the condition variable.
+    bool WaitForInterval(::bosdyn::common::Duration interval);
+
     DirectoryRegistrationClient* m_directory_registration_client = nullptr;
 
     ::bosdyn::api::ServiceEntry m_service_entry;
     ::bosdyn::api::Endpoint m_endpoint;
 
     // Time between registration/update calls. Should be shorter than the liveness timeout.
     ::bosdyn::common::Duration m_registration_interval;
+    ::bosdyn::common::Duration m_registration_initial_retry_interval;
 
     std::thread m_thread;
 
-    // Thread should exit.
-    bool m_should_exit;
+    // Object used to coordinate the lifecycle and timing of the periodic thread.
+    std::unique_ptr<PeriodicThreadHelper> m_periodic_thread_helper;
 
     // Is thread stopped.
     bool m_thread_stopped;
@@ -76,6 +87,9 @@ class DirectoryRegistrationKeepAlive {
 
     // Bool to track if there is a failure.
     bool m_registration_fault_active;
+
+    // Callback to invoke in the event of a periodic reregistration failure.
+    std::function<ErrorCallbackResult(const ::bosdyn::common::Status&)> m_error_callback;
 };
 
 }  // namespace client

cpp/bosdyn/client/error_callback/error_callback_result.h

@@ -0,0 +1,24 @@
+/**
+ * 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).
+ */
+
+
+#pragma once
+
+namespace bosdyn {
+namespace client {
+
+enum class ErrorCallbackResult {
+    kDefaultAction,
+    kRetryImmediately,
+    kRetryWithExponentialBackOff,
+    kResumeNormalOperation,
+    kAbort,
+};
+
+}  // namespace client
+}  // namespace bosdyn