WIP

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

Author: mhidalgo-bdai

spot_driver/CMakeLists.txt

@@ -15,6 +15,7 @@ if(CCACHE_PROGRAM)
 endif()
 
 set(THIS_PACKAGE_INCLUDE_ROS_DEPENDS
+  bondcpp
   bosdyn_api_msgs
   bosdyn_cmake_module
   bosdyn_spot_api_msgs
@@ -50,6 +51,7 @@ find_package(OpenCV 4 REQUIRED)
 add_library(spot_api
   src/api/default_kinematic_api.cpp
   src/api/default_image_client.cpp
+  src/api/default_lease_client.cpp
   src/api/default_spot_api.cpp
   src/api/default_state_client.cpp
   src/api/default_time_sync_api.cpp
@@ -71,6 +73,9 @@ add_library(spot_api
   src/interfaces/rclcpp_tf_broadcaster_interface.cpp
   src/interfaces/rclcpp_tf_listener_interface.cpp
   src/interfaces/rclcpp_wall_timer_interface.cpp
+  src/lease/lease_manager.cpp
+  src/lease/lease_manager_node.cpp
+  src/lease/lease_middleware_handle.cpp
   src/kinematic/kinematic_node.cpp
   src/kinematic/kinematic_service.cpp
   src/kinematic/kinematic_middleware_handle.cpp
@@ -145,6 +150,34 @@ rclcpp_components_register_node(
   PLUGIN "spot_ros2::StatePublisherNode"
   EXECUTABLE state_publisher_node_component)
 
+###
+# Spot lease manager
+###
+
+# Create executable to allow running LeaseManagerNode directly as a ROS 2 node
+add_executable(lease_manager_node src/lease/lease_manager_node_main.cpp)
+target_link_libraries(lease_manager_node PUBLIC spot_api)
+target_include_directories(lease_manager_node
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+# Register a composable node to allow loading StatePublisherNode in a component container
+add_library(lease_manager_component SHARED src/lease/lease_manager_component.cpp)
+target_include_directories(lease_manager_component
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(lease_manager_component PUBLIC spot_api)
+ament_target_dependencies(lease_manager_component PUBLIC rclcpp_components)
+
+rclcpp_components_register_node(
+  lease_manager_component
+  PLUGIN "spot_ros2::LeaseManagerNode"
+  EXECUTABLE lease_manager_node_component)
+
 ###
 # Spot IK
 ###

spot_driver/include/spot_driver/api/default_lease_client.hpp

@@ -0,0 +1,41 @@
+// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <bosdyn/client/lease/lease.h>
+#include <bosdyn/client/lease/lease_client.h>
+#include <bosdyn/client/lease/lease_keepalive.h>
+#include <bosdyn/client/lease/lease_wallet.h>
+#include <bosdyn/client/sdk/client_sdk.h>
+#include <spot_driver/api/lease_client_interface.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+namespace spot_ros2 {
+/**
+ * @brief Implements LeaseClientInterface to use the Spot C++ Lease Client.
+ */
+class DefaultLeaseClient : public LeaseClientInterface {
+ public:
+  explicit DefaultLeaseClient(::bosdyn::client::LeaseClient* lease_client);
+
+  [[nodiscard]] tl::expected<::bosdyn::client::Lease, std::string> acquireLease(
+      const std::string& resource_name, LeaseUseCallback retention_failure_callback) override;
+
+  [[nodiscard]] tl::expected<::bosdyn::client::Lease, std::string> takeLease(
+      const std::string& resource_name, LeaseUseCallback retention_failure_callback) override;
+
+  [[nodiscard]] tl::expected<bool, std::string> returnLease(const ::bosdyn::client::Lease& lease) override;
+
+  [[nodiscard]] std::shared_ptr<::bosdyn::client::LeaseWallet> getLeaseWallet() const override;
+
+  [[nodiscard]] const ::bosdyn::client::ResourceHierarchy& getResourceHierarchy() const override;
+
+ private:
+  ::bosdyn::client::LeaseClient* lease_client_;
+  std::unordered_map<std::string, std::unique_ptr<::bosdyn::client::LeaseKeepAlive>> keepalives_;
+  std::mutex keepalives_mutex_;
+};
+}  // namespace spot_ros2

spot_driver/include/spot_driver/api/default_spot_api.hpp

@@ -7,6 +7,7 @@
 #include <bosdyn/client/robot/robot.h>
 #include <bosdyn/client/sdk/client_sdk.h>
 
+#include <spot_driver/api/lease_client_interface.hpp>
 #include <spot_driver/api/state_client_interface.hpp>
 #include <spot_driver/api/time_sync_api.hpp>
 #include <spot_driver/api/world_object_client_interface.hpp>
@@ -32,6 +33,7 @@ class DefaultSpotApi : public SpotApi {
   [[nodiscard]] std::shared_ptr<KinematicApi> kinematicInterface() const override;
   [[nodiscard]] std::shared_ptr<ImageClientInterface> image_client_interface() const override;
   [[nodiscard]] std::shared_ptr<StateClientInterface> stateClientInterface() const override;
+  [[nodiscard]] std::shared_ptr<LeaseClientInterface> leaseClientInterface() const override;
   [[nodiscard]] std::shared_ptr<TimeSyncApi> timeSyncInterface() const override;
   [[nodiscard]] std::shared_ptr<WorldObjectClientInterface> worldObjectClientInterface() const override;
 
@@ -41,6 +43,7 @@ class DefaultSpotApi : public SpotApi {
   std::shared_ptr<KinematicApi> kinematic_interface_;
   std::shared_ptr<ImageClientInterface> image_client_interface_;
   std::shared_ptr<StateClientInterface> state_client_interface_;
+  std::shared_ptr<LeaseClientInterface> lease_client_interface_;
   std::shared_ptr<TimeSyncApi> time_sync_api_;
   std::shared_ptr<WorldObjectClientInterface> world_object_client_interface_;
   std::string robot_name_;

spot_driver/include/spot_driver/api/lease_client_interface.hpp

@@ -0,0 +1,41 @@
+// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <tl_expected/expected.hpp>
+
+#include <bosdyn/api/lease.pb.h>
+#include <bosdyn/client/lease/lease.h>
+#include <bosdyn/client/lease/lease_wallet.h>
+
+namespace spot_ros2 {
+class LeaseClientInterface {
+ public:
+  using LeaseUseCallback = std::function<void(const bosdyn::api::LeaseUseResult&)>;
+
+  // LeaseClientInterface is move-only
+  LeaseClientInterface() = default;
+  LeaseClientInterface(LeaseClientInterface&& other) = default;
+  LeaseClientInterface(const LeaseClientInterface&) = delete;
+  LeaseClientInterface& operator=(LeaseClientInterface&& other) = default;
+  LeaseClientInterface& operator=(const LeaseClientInterface&) = delete;
+
+  virtual ~LeaseClientInterface() = default;
+
+  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<bool, std::string> returnLease(const ::bosdyn::client::Lease& lease) = 0;
+
+  virtual std::shared_ptr<::bosdyn::client::LeaseWallet> getLeaseWallet() const = 0;
+
+  virtual const ::bosdyn::client::ResourceHierarchy& getResourceHierarchy() const = 0;
+};
+}  // namespace spot_ros2

spot_driver/include/spot_driver/api/spot_api.hpp

@@ -3,6 +3,7 @@
 #pragma once
 
 #include <spot_driver/api/kinematic_api.hpp>
+#include <spot_driver/api/lease_client_interface.hpp>
 #include <spot_driver/api/state_client_interface.hpp>
 #include <spot_driver/api/time_sync_api.hpp>
 #include <spot_driver/api/world_object_client_interface.hpp>
@@ -44,6 +45,7 @@ class SpotApi {
    * @return A shared_ptr to an instance of StateClientInterface which is owned by this object.
    */
   virtual std::shared_ptr<StateClientInterface> stateClientInterface() const = 0;
+  [[nodiscard]] virtual std::shared_ptr<LeaseClientInterface> leaseClientInterface() const = 0;
   virtual std::shared_ptr<TimeSyncApi> timeSyncInterface() const = 0;
   [[nodiscard]] virtual std::shared_ptr<WorldObjectClientInterface> worldObjectClientInterface() const = 0;
 };

spot_driver/include/spot_driver/lease/lease_manager.hpp

@@ -2,11 +2,11 @@
 
 #pragma once
 
-#include <spot_driver/api/kinematic_api.hpp>
-
+#include <spot_driver/api/lease_client_interface.hpp>
 #include <spot_driver/interfaces/logger_interface_base.hpp>
 
-#include <spot_msgs/srv/acquire_sublease.hpp>
+#include <spot_msgs/srv/acquire_lease.hpp>
+#include <spot_msgs/srv/return_lease.hpp>
 
 #include <memory>
 #include <string>
@@ -25,16 +25,20 @@ class LeaseManager {
    */
   class MiddlewareHandle {
    public:
+    class Bond {
+     public:
+      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 createReturnLeaseService(const std::string& service_name,
                                           std::function<void(const std::shared_ptr<ReturnLease::Request>,
                                                              std::shared_ptr<ReturnLease::Response>)> callback) = 0;
-    virtual void createBond(const std::string& node_name) = 0;
 
-    virtual void destroyBond(const std::string& node_name) = 0;
+    virtual std::shared_ptr<Bond> createBond(const std::string& node_name, std::function<void()> break_callback) = 0;
 
     virtual ~MiddlewareHandle() = default;
   };
@@ -45,9 +49,9 @@ class LeaseManager {
    * @param kinematic_api The Api to interact with the Spot SDK.
    * @param logger Logging interface.
    */
-   explicit LeaseManager(std::shared_ptr<LeaseApi> lease_api,
-                         std::shared_ptr<LoggerInterfaceBase> logger,
-                         std::unique_ptr<MiddlewareHandle> middleware_handle);
+  explicit LeaseManager(std::shared_ptr<LeaseClientInterface> lease_client,
+                        std::shared_ptr<LoggerInterfaceBase> logger,
+                        std::unique_ptr<MiddlewareHandle> middleware_handle);
 
   /** Initialize the service. */
   void initialize();
@@ -60,19 +64,26 @@ class LeaseManager {
   void acquireLease(const std::shared_ptr<AcquireLease::Request> request,
                     std::shared_ptr<AcquireLease::Response> response);
 
-  void returnLease(const std::shared_ptr<ReleaseLease::Request> request,
-                   std::shared_ptr<ReleaseLease::Response> response);
+  void returnLease(const std::shared_ptr<ReturnLease::Request> request,
+                   std::shared_ptr<ReturnLease::Response> response);
 
  private:
+  void onLeaseRetentionFailure(const bosdyn::api::LeaseUseResult& result);
+
   // The API to interact with Spot SDK.
-  std::shared_ptr<LeaseApi> lease_api_;
+  std::shared_ptr<LeaseClientInterface> lease_client_;
 
   // Logger.
   std::shared_ptr<LoggerInterfaceBase> logger_;
 
   // The service provider.
   std::unique_ptr<MiddlewareHandle> middleware_handle_;
 
-  std::unordered_map<std::string, std::string>
+  struct ManagedSublease {
+    bosdyn::client::Lease lease;
+    std::shared_ptr<MiddlewareHandle::Bond> bond;
+  };
+  std::unordered_map<std::string, ManagedSublease> subleases_;
+  std::recursive_mutex subleases_mutex_;
 };
 }  // namespace spot_ros2::lease

spot_driver/include/spot_driver/lease/lease_manager_node.hpp

@@ -0,0 +1,45 @@
+// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <spot_driver/lease/lease_manager.hpp>
+
+#include <spot_driver/api/spot_api.hpp>
+#include <spot_driver/interfaces/logger_interface_base.hpp>
+#include <spot_driver/interfaces/parameter_interface_base.hpp>
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_options.hpp>
+
+#include <memory>
+
+namespace spot_ros2::lease {
+class LeaseManagerNode {
+ public:
+  explicit LeaseManagerNode(std::shared_ptr<rclcpp::Node> node, std::unique_ptr<SpotApi> spot_api,
+                            std::shared_ptr<ParameterInterfaceBase> parameter_interface,
+                            std::shared_ptr<LoggerInterfaceBase> logger_interface);
+
+  explicit LeaseManagerNode(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions{});
+
+  /**
+   * @brief Returns the NodeBaseInterface of this class's node.
+   * @details This function exists to allow spinning the class's node as if it were derived from rclcpp::Node.
+   * This allows loading this class as a component node in a composable node container without requiring that it inherit
+   * from rclcpp::Node.
+   *
+   * @return A shared_ptr to the NodeBaseInterface of the node stored as a private member of this class.
+   */
+  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> get_node_base_interface();
+
+ private:
+  std::shared_ptr<rclcpp::Node> node_;
+  std::unique_ptr<SpotApi> spot_api_;
+  std::unique_ptr<LeaseManager> internal_;
+
+  void initialize(std::shared_ptr<rclcpp::Node> node, std::unique_ptr<SpotApi> spot_api,
+                  std::shared_ptr<ParameterInterfaceBase> parameter_interface,
+                  const std::shared_ptr<LoggerInterfaceBase> logger_interface);
+};
+}  // namespace spot_ros2::kinematic

spot_driver/include/spot_driver/lease/lease_middleware_handle.hpp

@@ -0,0 +1,38 @@
+// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <spot_driver/lease/lease_manager.hpp>
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/service.hpp>
+
+namespace spot_ros2::lease {
+
+class LeaseMiddlewareHandle : public LeaseManager::MiddlewareHandle {
+ public:
+  explicit LeaseMiddlewareHandle(std::shared_ptr<rclcpp::Node> node);
+
+  void createAcquireLeaseService(
+      const std::string& service_name,
+      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::shared_ptr<LeaseManager::MiddlewareHandle::Bond> createBond(
+      const std::string& node_name, std::function<void()> break_callback) override;
+
+ private:
+  std::shared_ptr<rclcpp::Node> node_;
+  std::shared_ptr<rclcpp::Service<AcquireLease>> acquire_lease_service_;
+  std::shared_ptr<rclcpp::Service<ReturnLease>> return_lease_service_;
+};
+}  // namespace spot_ros2::lease

spot_driver/package.xml

@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake_python</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
+  <depend>bondcpp</depend>
   <depend>bosdyn</depend>
   <depend>bosdyn_cmake_module</depend>
   <depend>bosdyn_msgs</depend>