WIP

Signed-off-by: Michel Hidalgo <[email protected]>

spot_driver/include/spot_driver/api/lease_client_interface.hpp

@@ -29,8 +29,8 @@ class LeaseClientInterface {
   virtual tl::expected<::bosdyn::client::Lease, std::string> acquireLease(
       const std::string& resource_name, LeaseUseCallback retention_failure_callback) = 0;
 
-  virtual tl::expected<::bosdyn::client::Lease, std::string> takeLease(
-      const std::string& resource_name, LeaseUseCallback retention_failure_callback) = 0;
+  virtual tl::expected<::bosdyn::client::Lease, std::string> takeLease(const std::string& resource_name,
+                                                                       LeaseUseCallback retention_failure_callback) = 0;
 
   virtual tl::expected<bool, std::string> returnLease(const ::bosdyn::client::Lease& lease) = 0;
 

spot_driver/include/spot_driver/lease/lease_manager.hpp

@@ -9,7 +9,9 @@
 #include <spot_msgs/srv/return_lease.hpp>
 
 #include <memory>
+#include <mutex>
 #include <string>
+#include <unordered_map>
 
 namespace spot_ros2::lease {
 
@@ -30,13 +32,15 @@ class LeaseManager {
       virtual ~Bond() = default;
     };
 
-    virtual void createAcquireLeaseService(const std::string& service_name,
-                                           std::function<void(const std::shared_ptr<AcquireLease::Request>,
-                                                              std::shared_ptr<AcquireLease::Response>)> callback) = 0;
+    virtual void createAcquireLeaseService(
+        const std::string& service_name,
+        std::function<void(const std::shared_ptr<AcquireLease::Request>, std::shared_ptr<AcquireLease::Response>)>
+            callback) = 0;
 
-    virtual void createReturnLeaseService(const std::string& service_name,
-                                          std::function<void(const std::shared_ptr<ReturnLease::Request>,
-                                                             std::shared_ptr<ReturnLease::Response>)> callback) = 0;
+    virtual void createReturnLeaseService(
+        const std::string& service_name,
+        std::function<void(const std::shared_ptr<ReturnLease::Request>, std::shared_ptr<ReturnLease::Response>)>
+            callback) = 0;
 
     virtual std::shared_ptr<Bond> createBond(const std::string& node_name, std::function<void()> break_callback) = 0;
 
@@ -49,8 +53,7 @@ class LeaseManager {
    * @param kinematic_api The Api to interact with the Spot SDK.
    * @param logger Logging interface.
    */
-  explicit LeaseManager(std::shared_ptr<LeaseClientInterface> lease_client,
-                        std::shared_ptr<LoggerInterfaceBase> logger,
+  explicit LeaseManager(std::shared_ptr<LeaseClientInterface> lease_client, std::shared_ptr<LoggerInterfaceBase> logger,
                         std::unique_ptr<MiddlewareHandle> middleware_handle);
 
   /** Initialize the service. */

spot_driver/include/spot_driver/lease/lease_manager_node.hpp

@@ -42,4 +42,4 @@ class LeaseManagerNode {
                   std::shared_ptr<ParameterInterfaceBase> parameter_interface,
                   const std::shared_ptr<LoggerInterfaceBase> logger_interface);
 };
-}  // namespace spot_ros2::kinematic
+}  // namespace spot_ros2::lease

spot_driver/include/spot_driver/lease/lease_middleware_handle.hpp

@@ -19,16 +19,16 @@ class LeaseMiddlewareHandle : public LeaseManager::MiddlewareHandle {
 
   void createAcquireLeaseService(
       const std::string& service_name,
-      std::function<void(const std::shared_ptr<AcquireLease::Request>,
-                         std::shared_ptr<AcquireLease::Response>)> callback) override;
+      std::function<void(const std::shared_ptr<AcquireLease::Request>, std::shared_ptr<AcquireLease::Response>)>
+          callback) override;
 
   void createReturnLeaseService(
       const std::string& service_name,
-      std::function<void(const std::shared_ptr<ReturnLease::Request>,
-                         std::shared_ptr<ReturnLease::Response>)> callback) override;
+      std::function<void(const std::shared_ptr<ReturnLease::Request>, std::shared_ptr<ReturnLease::Response>)> callback)
+      override;
 
-  std::shared_ptr<LeaseManager::MiddlewareHandle::Bond> createBond(
-      const std::string& node_name, std::function<void()> break_callback) override;
+  std::shared_ptr<LeaseManager::MiddlewareHandle::Bond> createBond(const std::string& node_name,
+                                                                   std::function<void()> break_callback) override;
 
  private:
   std::shared_ptr<rclcpp::Node> node_;

spot_driver/src/api/default_lease_client.cpp

@@ -6,87 +6,78 @@
 
 namespace spot_ros2 {
 
-DefaultLeaseClient::DefaultLeaseClient(bosdyn::client::LeaseClient* lease_client)
-    : lease_client_{lease_client} {}
+DefaultLeaseClient::DefaultLeaseClient(bosdyn::client::LeaseClient* lease_client) : lease_client_{lease_client} {}
 
 tl::expected<::bosdyn::client::Lease, std::string> DefaultLeaseClient::acquireLease(
-    const std::string& resource_name, LeaseUseCallback retention_failure_callback)
-{
-    try {
-        auto result = lease_client_->AcquireLease(resource_name);
-        if (!result) {
-            return tl::make_unexpected(result.status.Chain("Could not acquire lease").message());
-        }
-        auto callback = [this, callback=std::move(retention_failure_callback)] (
-            const ::bosdyn::client::Result<::bosdyn::api::RetainLeaseResponse>& result,
-            ::bosdyn::client::LeaseKeepAlive* lease_keep_alive)
-        {
-            callback(result.response.lease_use_result());
-            lease_keep_alive->StopKeepAliveThread();
-        };
-        constexpr auto kKeepAlivePeriod = std::chrono::seconds(1);
-        auto keepalive = std::make_unique<::bosdyn::client::LeaseKeepAlive>(
-            lease_client_, lease_client_->GetLeaseWallet(),
-            resource_name, kKeepAlivePeriod, std::move(callback));
-        std::lock_guard<std::mutex> lock(keepalives_mutex_);
-        keepalives_.emplace(std::make_pair(resource_name, std::move(keepalive)));
-        return ::bosdyn::client::Lease(result.response.lease());
-    } catch (const std::exception& ex) {
-        return tl::make_unexpected("Failed to acquire lease: " + std::string{ex.what()});
+    const std::string& resource_name, LeaseUseCallback retention_failure_callback) {
+  try {
+    auto result = lease_client_->AcquireLease(resource_name);
+    if (!result) {
+      return tl::make_unexpected(result.status.Chain("Could not acquire lease").message());
     }
+    auto callback = [this, callback = std::move(retention_failure_callback)](
+                        const ::bosdyn::client::Result<::bosdyn::api::RetainLeaseResponse>& result,
+                        ::bosdyn::client::LeaseKeepAlive* lease_keep_alive) {
+      callback(result.response.lease_use_result());
+      lease_keep_alive->StopKeepAliveThread();
+    };
+    constexpr auto kKeepAlivePeriod = std::chrono::seconds(1);
+    auto keepalive = std::make_unique<::bosdyn::client::LeaseKeepAlive>(
+        lease_client_, lease_client_->GetLeaseWallet(), resource_name, kKeepAlivePeriod, std::move(callback));
+    std::lock_guard<std::mutex> lock(keepalives_mutex_);
+    keepalives_.emplace(std::make_pair(resource_name, std::move(keepalive)));
+    return ::bosdyn::client::Lease(result.response.lease());
+  } catch (const std::exception& ex) {
+    return tl::make_unexpected("Failed to acquire lease: " + std::string{ex.what()});
+  }
 }
 
 tl::expected<bosdyn::client::Lease, std::string> DefaultLeaseClient::takeLease(
-    const std::string& resource_name, LeaseUseCallback retention_failure_callback)
-{
-    try {
-        auto result = lease_client_->TakeLease(resource_name);
-        if (!result) {
-            return tl::make_unexpected(result.status.Chain("Could not acquire lease").message());
-        }
-        constexpr auto kKeepAlivePeriod = std::chrono::seconds(1);
-        auto callback = [this, callback=std::move(retention_failure_callback)] (
-            const ::bosdyn::client::Result<::bosdyn::api::RetainLeaseResponse>& result,
-            ::bosdyn::client::LeaseKeepAlive* lease_keep_alive)
-        {
-            callback(result.response.lease_use_result());
-            lease_keep_alive->StopKeepAliveThread();
-        };
-        auto keepalive = std::make_unique<::bosdyn::client::LeaseKeepAlive>(
-            lease_client_, lease_client_->GetLeaseWallet(),
-            resource_name, kKeepAlivePeriod, std::move(callback));
-        std::lock_guard<std::mutex> lock(keepalives_mutex_);
-        keepalives_.emplace(std::make_pair(resource_name, std::move(keepalive)));
-        return ::bosdyn::client::Lease(result.response.lease());
-    } catch (const std::exception& ex) {
-        return tl::make_unexpected("Failed to take lease: " + std::string{ex.what()});
+    const std::string& resource_name, LeaseUseCallback retention_failure_callback) {
+  try {
+    auto result = lease_client_->TakeLease(resource_name);
+    if (!result) {
+      return tl::make_unexpected(result.status.Chain("Could not acquire lease").message());
     }
+    constexpr auto kKeepAlivePeriod = std::chrono::seconds(1);
+    auto callback = [this, callback = std::move(retention_failure_callback)](
+                        const ::bosdyn::client::Result<::bosdyn::api::RetainLeaseResponse>& result,
+                        ::bosdyn::client::LeaseKeepAlive* lease_keep_alive) {
+      callback(result.response.lease_use_result());
+      lease_keep_alive->StopKeepAliveThread();
+    };
+    auto keepalive = std::make_unique<::bosdyn::client::LeaseKeepAlive>(
+        lease_client_, lease_client_->GetLeaseWallet(), resource_name, kKeepAlivePeriod, std::move(callback));
+    std::lock_guard<std::mutex> lock(keepalives_mutex_);
+    keepalives_.emplace(std::make_pair(resource_name, std::move(keepalive)));
+    return ::bosdyn::client::Lease(result.response.lease());
+  } catch (const std::exception& ex) {
+    return tl::make_unexpected("Failed to take lease: " + std::string{ex.what()});
+  }
 }
 
-tl::expected<bool, std::string> DefaultLeaseClient::returnLease(const ::bosdyn::client::Lease& lease)
-{
-    try {
-        auto request = ::bosdyn::api::ReturnLeaseRequest();
-        *request.mutable_lease() = lease.GetProto();
-        auto result = lease_client_->ReturnLease(request);
-        if (!result) {
-            return tl::make_unexpected(result.status.Chain("Could not return lease").message());
-        }
-        std::lock_guard<std::mutex> lock(keepalives_mutex_);
-        keepalives_.erase(lease.GetResource());
-        return true;
-    } catch (const std::exception& ex) {
-        return tl::make_unexpected("Failed to take lease: " + std::string{ex.what()});
+tl::expected<bool, std::string> DefaultLeaseClient::returnLease(const ::bosdyn::client::Lease& lease) {
+  try {
+    auto request = ::bosdyn::api::ReturnLeaseRequest();
+    *request.mutable_lease() = lease.GetProto();
+    auto result = lease_client_->ReturnLease(request);
+    if (!result) {
+      return tl::make_unexpected(result.status.Chain("Could not return lease").message());
     }
+    std::lock_guard<std::mutex> lock(keepalives_mutex_);
+    keepalives_.erase(lease.GetResource());
+    return true;
+  } catch (const std::exception& ex) {
+    return tl::make_unexpected("Failed to take lease: " + std::string{ex.what()});
+  }
 }
 
 std::shared_ptr<::bosdyn::client::LeaseWallet> DefaultLeaseClient::getLeaseWallet() const {
-    return lease_client_->GetLeaseWallet();
+  return lease_client_->GetLeaseWallet();
 }
 
 const ::bosdyn::client::ResourceHierarchy& DefaultLeaseClient::getResourceHierarchy() const {
-    return ::bosdyn::client::DefaultResourceHierarchy(
-        ::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER);
+  return ::bosdyn::client::DefaultResourceHierarchy(::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER);
 }
 
 }  // namespace spot_ros2

spot_driver/src/api/default_spot_api.cpp

@@ -5,9 +5,9 @@
 #include <memory>
 #include <spot_driver/api/default_image_client.hpp>
 #include <spot_driver/api/default_kinematic_api.hpp>
+#include <spot_driver/api/default_lease_client.hpp>
 #include <spot_driver/api/default_spot_api.hpp>
 #include <spot_driver/api/default_state_client.hpp>
-#include <spot_driver/api/default_lease_client.hpp>
 #include <spot_driver/api/default_time_sync_api.hpp>
 #include <tl_expected/expected.hpp>
 #include "spot_driver/api/default_world_object_client.hpp"
@@ -86,7 +86,6 @@ tl::expected<void, std::string> DefaultSpotApi::authenticate(const std::string&
   }
   state_client_interface_ = std::make_shared<DefaultStateClient>(robot_state_result.response);
 
-
   // Lease API.
   const auto lease_client_result = robot_->EnsureServiceClient<::bosdyn::client::LeaseClient>(
       ::bosdyn::client::LeaseClient::GetDefaultServiceName());

spot_driver/src/lease/lease_manager.cpp

@@ -5,8 +5,8 @@
 #include <functional>
 #include <unordered_set>
 
-#include <spot_driver/lease/lease_middleware_handle.hpp>
 #include <spot_driver/interfaces/rclcpp_logger_interface.hpp>
+#include <spot_driver/lease/lease_middleware_handle.hpp>
 
 #include <bosdyn_api_msgs/conversions.hpp>
 
@@ -19,36 +19,34 @@ namespace spot_ros2::lease {
 LeaseManager::LeaseManager(std::shared_ptr<LeaseClientInterface> lease_client,
                            std::shared_ptr<LoggerInterfaceBase> logger,
                            std::unique_ptr<MiddlewareHandle> middleware_handle)
-    : lease_client_{std::move(lease_client)}, logger_{std::move(logger)}, middleware_handle_{std::move(middleware_handle)} {}
+    : lease_client_{std::move(lease_client)},
+      logger_{std::move(logger)},
+      middleware_handle_{std::move(middleware_handle)} {}
 
 void LeaseManager::initialize() {
   middleware_handle_->createAcquireLeaseService(
-    kAcquireLeaseServiceName,
-    [this](const std::shared_ptr<AcquireLease::Request> request,
-           std::shared_ptr<AcquireLease::Response> response) {
-      this->acquireLease(request, response);
-    });
+      kAcquireLeaseServiceName,
+      [this](const std::shared_ptr<AcquireLease::Request> request, std::shared_ptr<AcquireLease::Response> response) {
+        this->acquireLease(request, response);
+      });
   middleware_handle_->createReturnLeaseService(
-    kReturnLeaseServiceName,
-    [this](const std::shared_ptr<ReturnLease::Request> request,
-           std::shared_ptr<ReturnLease::Response> response) {
-      this->returnLease(request, response);
-    });
+      kReturnLeaseServiceName,
+      [this](const std::shared_ptr<ReturnLease::Request> request, std::shared_ptr<ReturnLease::Response> response) {
+        this->returnLease(request, response);
+      });
 }
 
 void LeaseManager::onLeaseRetentionFailure(const bosdyn::api::LeaseUseResult& result) {
   std::lock_guard<std::recursive_mutex> lock(subleases_mutex_);
   auto lease_wallet = lease_client_->getLeaseWallet();
-  const google::protobuf::EnumDescriptor* descriptor =
-    bosdyn::api::LeaseUseResult::Status_descriptor();
-  const std::string& status =
-    descriptor->FindValueByNumber(result.status())->name();
+  const google::protobuf::EnumDescriptor* descriptor = bosdyn::api::LeaseUseResult::Status_descriptor();
+  const std::string& status = descriptor->FindValueByNumber(result.status())->name();
   const std::string& resource_name = result.attempted_lease().resource();
   logger_->logInfo(resource_name + " lease was revoked: " + status);
   lease_wallet->RemoveLease(resource_name);
   logger_->logInfo("Revoking " + resource_name + " subleases");
   subleases_.erase(resource_name);
-};
+}
 
 void LeaseManager::acquireLease(const std::shared_ptr<AcquireLease::Request> request,
                                 std::shared_ptr<AcquireLease::Response> response) {
@@ -69,8 +67,7 @@ void LeaseManager::acquireLease(const std::shared_ptr<AcquireLease::Request> req
       potential_collisions.insert(name);
     }
   }
-  const auto& resource_hierarchy =
-    root_resource_hierarchy.GetHierarchy(request->resource_name);
+  const auto& resource_hierarchy = root_resource_hierarchy.GetHierarchy(request->resource_name);
   for (const auto& [name, _] : resource_hierarchy) {
     potential_collisions.insert(name);
   }
@@ -93,7 +90,7 @@ void LeaseManager::acquireLease(const std::shared_ptr<AcquireLease::Request> req
 
   auto lease = lease_wallet->AdvanceLease(request->resource_name);
   if (!lease) {
-    using namespace std::placeholders;
+    using std::placeholders::_1;
     auto callback = std::bind(&LeaseManager::onLeaseRetentionFailure, this, _1);
     if (must_take) {
       auto result = lease_client_->takeLease(request->resource_name, std::move(callback));
@@ -119,30 +116,29 @@ void LeaseManager::acquireLease(const std::shared_ptr<AcquireLease::Request> req
   bosdyn_api_msgs::conversions::Convert(sublease.GetProto(), &response->lease);
 
   auto bond = middleware_handle_->createBond(request->client_name, [this, sublease]() {
-      std::lock_guard<std::recursive_mutex> lock(subleases_mutex_);
-      const std::string& resource_name = sublease.GetResource();
-      if (subleases_.erase(resource_name) > 0) {
-        const auto& sublease_proto = sublease.GetProto();
-        const std::string& client_name = *sublease_proto.client_names().rbegin();
-        logger_->logError(client_name + " bond broken");
-
-        using namespace std::placeholders;
-        auto callback = std::bind(&LeaseManager::onLeaseRetentionFailure, this, _1);
-        auto result = lease_client_->takeLease(resource_name, std::move(callback));
-        if (!result) {
-          logger_->logError("Failed to (re)take " + resource_name + " sublease: " + result.error());
-        }
+    std::lock_guard<std::recursive_mutex> lock(subleases_mutex_);
+    const std::string& resource_name = sublease.GetResource();
+    if (subleases_.erase(resource_name) > 0) {
+      const auto& sublease_proto = sublease.GetProto();
+      const std::string& client_name = *sublease_proto.client_names().rbegin();
+      logger_->logError(client_name + " bond broken");
+
+      using std::placeholders::_1;
+      auto callback = std::bind(&LeaseManager::onLeaseRetentionFailure, this, _1);
+      auto result = lease_client_->takeLease(resource_name, std::move(callback));
+      if (!result) {
+        logger_->logError("Failed to (re)take " + resource_name + " sublease: " + result.error());
       }
-      logger_->logInfo(resource_name + " sublease revoked");
+    }
+    logger_->logInfo(resource_name + " sublease revoked");
   });
   subleases_[request->resource_name] = ManagedSublease{std::move(sublease), std::move(bond)};
   logger_->logInfo(request->resource_name + " sublease was acquired");
   response->success = true;
 }
 
 void LeaseManager::returnLease(const std::shared_ptr<ReturnLease::Request> request,
-                               std::shared_ptr<ReturnLease::Response> response)
-{
+                               std::shared_ptr<ReturnLease::Response> response) {
   std::lock_guard<std::recursive_mutex> lock(subleases_mutex_);
 
   bosdyn::api::Lease sublease_proto;

spot_driver/src/lease/lease_manager_node.cpp

@@ -1,7 +1,7 @@
 // Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved.
 
-#include <spot_driver/lease/lease_middleware_handle.hpp>
 #include <spot_driver/lease/lease_manager_node.hpp>
+#include <spot_driver/lease/lease_middleware_handle.hpp>
 
 #include <spot_driver/api/default_spot_api.hpp>
 #include <spot_driver/interfaces/rclcpp_logger_interface.hpp>
@@ -55,8 +55,7 @@ void LeaseManagerNode::initialize(std::shared_ptr<rclcpp::Node> node, std::uniqu
 
   auto lease_client = spot_api_->leaseClientInterface();
   if (!lease_client) {
-    constexpr auto errorMsg{
-        "Failed to initialize the Spot API's lease client, which is required to run this node."};
+    constexpr auto errorMsg{"Failed to initialize the Spot API's lease client, which is required to run this node."};
     logger_interface->logError(errorMsg);
     throw std::runtime_error(errorMsg);
   }