[SW-1999] Add support for Spot leasing by proxy to its hardware interface

spot_hardware_interface/CMakeLists.txt

@@ -14,11 +14,13 @@ endif()
 # Dependencies
 find_package(ament_cmake REQUIRED)
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  bondcpp
   hardware_interface
   bosdyn_cmake_module
   pluginlib
   rclcpp
   rclcpp_lifecycle
+  spot_msgs
 )
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
@@ -38,6 +40,7 @@ add_library(
   spot_hardware_interface
   SHARED
   src/spot_hardware_interface.cpp
+  src/spot_leasing_interface.cpp
 )
 target_compile_features(spot_hardware_interface PUBLIC cxx_std_20)
 target_include_directories(spot_hardware_interface PUBLIC

spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp

@@ -34,6 +34,7 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "spot_hardware_interface/spot_constants.hpp"
+#include "spot_hardware_interface/spot_leasing_interface.hpp"
 #include "spot_hardware_interface/visibility_control.h"
 
 #include "bosdyn/client/lease/lease_keepalive.h"
@@ -85,6 +86,8 @@ class StateStreamingHandler {
   std::mutex mutex_;
 };
 
+enum class LeasingMode { DIRECT, PROXIED };
+
 class SpotHardware : public hardware_interface::SystemInterface {
  public:
   RCLCPP_SHARED_PTR_DEFINITIONS(SpotHardware)
@@ -141,11 +144,14 @@ class SpotHardware : public hardware_interface::SystemInterface {
 
   // Shared BD clients.
   std::unique_ptr<::bosdyn::client::Robot> robot_;
-  ::bosdyn::client::LeaseClient* lease_client_;
   ::bosdyn::client::RobotStateStreamingClient* state_client_;
   ::bosdyn::client::RobotCommandStreamingClient* command_stream_service_;
   ::bosdyn::client::RobotCommandClient* command_client_;
 
+  std::unique_ptr<LeasingInterface> leasing_interface_;
+
+  LeasingMode leasing_mode_;
+
   // Holds joint states of the robot received from the BD SDK
   JointStates joint_states_;
   // Holds joint commands for the robot to send to BD SDK

spot_hardware_interface/include/spot_hardware_interface/spot_leasing_interface.hpp

@@ -0,0 +1,86 @@
+// Copyright (c) 2025 Boston Dynamics AI Institute LLC.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <unordered_map>
+
+#include <bondcpp/bond.hpp>
+
+#include <bosdyn/client/lease/lease.h>
+#include <bosdyn/client/lease/lease_client.h>
+#include <bosdyn/client/lease/lease_wallet.h>
+#include <bosdyn/client/robot/robot.h>
+
+#include <rclcpp/client.hpp>
+#include <rclcpp/executors/single_threaded_executor.hpp>
+#include <rclcpp/node.hpp>
+
+#include <spot_msgs/srv/acquire_lease.hpp>
+#include <spot_msgs/srv/return_lease.hpp>
+
+#include <tl_expected/expected.hpp>
+
+namespace spot_hardware_interface {
+
+class LeasingInterface {
+ public:
+  virtual ~LeasingInterface() = default;
+
+  virtual tl::expected<::bosdyn::client::Lease, std::string> AcquireLease(const std::string& resource) = 0;
+
+  virtual tl::expected<::bosdyn::client::Lease, std::string> ReturnLease(const std::string& resource) = 0;
+};
+
+class DirectLeasingInterface : public LeasingInterface {
+ public:
+  explicit DirectLeasingInterface(::bosdyn::client::Robot* robot);
+
+  tl::expected<::bosdyn::client::Lease, std::string> AcquireLease(const std::string& resource_name) override;
+
+  tl::expected<::bosdyn::client::Lease, std::string> ReturnLease(const std::string& resource_name) override;
+
+ private:
+  ::bosdyn::client::Robot* robot_;
+  ::bosdyn::client::LeaseClient* lease_client_;
+};
+
+class ProxiedLeasingInterface : public LeasingInterface {
+ public:
+  explicit ProxiedLeasingInterface(::bosdyn::client::Robot* robot);
+
+  ~ProxiedLeasingInterface() override;
+
+  tl::expected<::bosdyn::client::Lease, std::string> AcquireLease(const std::string& resource_name) override;
+
+  tl::expected<::bosdyn::client::Lease, std::string> ReturnLease(const std::string& resource_name) override;
+
+ private:
+  std::shared_ptr<::bosdyn::client::LeaseWallet> lease_wallet_;
+
+  std::shared_ptr<rclcpp::Node> foreground_node_;
+  std::shared_ptr<rclcpp::Client<spot_msgs::srv::AcquireLease>> acquire_lease_client_;
+  std::shared_ptr<rclcpp::Client<spot_msgs::srv::ReturnLease>> return_lease_client_;
+
+  std::jthread background_loop_;
+  std::shared_ptr<rclcpp::Node> background_node_;
+  std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> background_executor_;
+
+  std::unordered_map<std::string, std::unique_ptr<bond::Bond>> keepalive_bonds_;
+};
+
+}  // namespace spot_hardware_interface

spot_hardware_interface/package.xml

@@ -12,12 +12,15 @@ Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved.
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>bondcpp</depend>
+  <depend>bosdyn</depend>
+  <depend>bosdyn_cmake_module</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
-  <depend>bosdyn</depend>
-  <depend>bosdyn_cmake_module</depend>
+  <depend>spot_msgs</depend>
+  <depend>tl_expected</depend>
 
   <exec_depend>spot_description</exec_depend>
   <exec_depend>xacro</exec_depend>

spot_hardware_interface/src/spot_hardware_interface.cpp

@@ -99,6 +99,16 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac
     certificate_.reset();
   }
 
+  if (info_.hardware_parameters["leasing"] == "direct") {
+    leasing_mode_ = LeasingMode::DIRECT;
+  } else if (info_.hardware_parameters["leasing"] == "proxied") {
+    leasing_mode_ = LeasingMode::PROXIED;
+  } else {
+    RCLCPP_ERROR(rclcpp::get_logger("SpotHardware"), "Got %s for leasing mode, expected 'direct' or 'proxied'",
+                 info_.hardware_parameters["leasing"].c_str());
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
   hw_states_.resize(info_.joints.size() * interfaces_per_joint_, std::numeric_limits<double>::quiet_NaN());
   hw_commands_.resize(info_.joints.size() * interfaces_per_joint_, std::numeric_limits<double>::quiet_NaN());
 
@@ -193,6 +203,14 @@ hardware_interface::CallbackReturn SpotHardware::on_configure(const rclcpp_lifec
   if (!authenticate_robot(hostname_, username_, password_, port_, certificate_)) {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  switch (leasing_mode_) {
+    case LeasingMode::PROXIED:
+      leasing_interface_ = std::make_unique<ProxiedLeasingInterface>(robot_.get());
+      break;
+    case LeasingMode::DIRECT:
+      leasing_interface_ = std::make_unique<DirectLeasingInterface>(robot_.get());
+      break;
+  }
   if (!start_time_sync()) {
     return hardware_interface::CallbackReturn::ERROR;
   }
@@ -272,6 +290,7 @@ hardware_interface::CallbackReturn SpotHardware::on_shutdown(const rclcpp_lifecy
   if (!power_off()) {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  leasing_interface_.reset();
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
@@ -406,22 +425,13 @@ bool SpotHardware::check_estop() {
 }
 
 bool SpotHardware::get_lease() {
-  RCLCPP_INFO(rclcpp::get_logger("SpotHardware"), "Getting Lease");
-  // First create a lease client.
-  ::bosdyn::client::Result<::bosdyn::client::LeaseClient*> lease_client_resp =
-      robot_->EnsureServiceClient<::bosdyn::client::LeaseClient>();
-  if (!lease_client_resp) {
-    RCLCPP_ERROR(rclcpp::get_logger("SpotHardware"), "Could not create lease client");
+  RCLCPP_INFO(rclcpp::get_logger("SpotHardware"), "Taking lease...");
+  auto lease = leasing_interface_->AcquireLease("body");
+  if (!lease) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("SpotHardware"), lease.error());
     return false;
   }
-  lease_client_ = lease_client_resp.response;
-  // Then acquire the lease for the body.
-  const auto lease_res = lease_client_->AcquireLease("body");
-  if (!lease_res) {
-    RCLCPP_ERROR(rclcpp::get_logger("SpotHardware"), "Could not acquire body lease");
-    return false;
-  }
-  RCLCPP_INFO(rclcpp::get_logger("SpotHardware"), "Lease acquired!!");
+  RCLCPP_INFO(rclcpp::get_logger("SpotHardware"), "Lease taken!!");
   return true;
 }
 
@@ -611,12 +621,13 @@ void SpotHardware::send_command(const JointStates& joint_commands) {
 }
 
 void SpotHardware::release_lease() {
-  RCLCPP_INFO(rclcpp::get_logger("SpotHardware"), "Releasing Lease");
-  bosdyn::api::ReturnLeaseRequest msg;
-  auto lease_result = robot_->GetWallet()->GetOwnedLeaseProto("body");
-  msg.mutable_lease()->CopyFrom(lease_result.response);
-  auto resp = lease_client_->ReturnLease(msg);
-  RCLCPP_INFO(rclcpp::get_logger("SpotHardware"), "Return lease status: %s", resp.status.DebugString().c_str());
+  RCLCPP_INFO(rclcpp::get_logger("SpotHardware"), "Returning lease...");
+  auto lease = leasing_interface_->ReturnLease("body");
+  if (!lease) {
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("SpotHardware"), lease.error());
+    return;
+  }
+  RCLCPP_INFO(rclcpp::get_logger("SpotHardware"), "Lease returned!!");
 }
 
 }  // namespace spot_hardware_interface

spot_hardware_interface/src/spot_leasing_interface.cpp

@@ -0,0 +1,141 @@
+// Copyright (c) 2025 Boston Dynamics AI Institute LLC.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "spot_hardware_interface/spot_leasing_interface.hpp"
+
+#include <bosdyn/api/lease.pb.h>
+#include <bosdyn_api_msgs/conversions.hpp>
+
+namespace spot_hardware_interface {
+
+DirectLeasingInterface::DirectLeasingInterface(::bosdyn::client::Robot* robot) : robot_(robot) {}
+
+tl::expected<::bosdyn::client::Lease, std::string> DirectLeasingInterface::AcquireLease(
+    const std::string& resource_name) {
+  if (!lease_client_) {
+    ::bosdyn::client::Result<::bosdyn::client::LeaseClient*> result =
+        robot_->EnsureServiceClient<::bosdyn::client::LeaseClient>();
+    if (!result) {
+      return tl::make_unexpected(result.status.Chain("Could not create lease client").message());
+    }
+    lease_client_ = result.move();
+  }
+  // NOTE(mhidalgo-bdai): ROS 2 control does not manage Ctrl + C properly in Humble,
+  // so here we take the lease instead of acquiring it, as it won't return it properly
+  // on process interruption.
+  auto result = lease_client_->TakeLease(resource_name);
+  if (!result) {
+    return tl::make_unexpected(result.status.Chain("Could not create acquire lease").message());
+  }
+  return ::bosdyn::client::Lease(result.move().lease());
+}
+
+tl::expected<::bosdyn::client::Lease, std::string> DirectLeasingInterface::ReturnLease(
+    const std::string& resource_name) {
+  if (!lease_client_) {
+    return tl::make_unexpected("No lease client, did you acquire first?");
+  }
+  auto lease_wallet = robot_->GetWallet();
+  auto result = lease_wallet->GetOwnedLease(resource_name);
+  if (!result) {
+    return tl::make_unexpected(result.status.Chain("No lease owned for " + resource_name).message());
+  }
+  ::bosdyn::client::Lease lease = result.move();
+  bosdyn::api::ReturnLeaseRequest request;
+  request.mutable_lease()->CopyFrom(lease.GetProto());
+  auto response = lease_client_->ReturnLease(request);
+  if (!response) {
+    return tl::make_unexpected(response.status.Chain("Could not return lease").message());
+  }
+  return lease;
+}
+
+ProxiedLeasingInterface::ProxiedLeasingInterface(::bosdyn::client::Robot* robot) : lease_wallet_(robot->GetWallet()) {
+  foreground_node_ = std::make_shared<rclcpp::Node>("_foreground_" + lease_wallet_->GetClientName() + "_node");
+  acquire_lease_client_ = foreground_node_->create_client<spot_msgs::srv::AcquireLease>("acquire_lease");
+  return_lease_client_ = foreground_node_->create_client<spot_msgs::srv::ReturnLease>("return_lease");
+  background_node_ = std::make_shared<rclcpp::Node>("_background_" + lease_wallet_->GetClientName() + "_node");
+  background_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
+  background_executor_->add_node(foreground_node_);
+  background_loop_ = std::jthread([this]() {
+    background_executor_->spin();
+  });
+}
+
+ProxiedLeasingInterface::~ProxiedLeasingInterface() {
+  background_executor_->cancel();
+}
+
+tl::expected<::bosdyn::client::Lease, std::string> ProxiedLeasingInterface::AcquireLease(
+    const std::string& resource_name) {
+  auto request = std::make_shared<spot_msgs::srv::AcquireLease::Request>();
+  request->resource_name = resource_name;
+  request->client_name = lease_wallet_->GetClientName();
+  auto future = acquire_lease_client_->async_send_request(request);
+  auto outcome = rclcpp::spin_until_future_complete(foreground_node_, future, std::chrono::seconds(5));
+  if (outcome == rclcpp::FutureReturnCode::TIMEOUT) {
+    acquire_lease_client_->remove_pending_request(future);
+    return tl::make_unexpected("Timed out trying to acquire the lease");
+  }
+  if (outcome == rclcpp::FutureReturnCode::INTERRUPTED) {
+    acquire_lease_client_->remove_pending_request(future);
+    return tl::make_unexpected("Lease acquisition got interrupted");
+  }
+  auto response = future.get();
+  if (!response->success) {
+    return tl::make_unexpected("Could not acquire lease: " + response->message);
+  }
+  ::bosdyn::api::Lease lease_proto;
+  bosdyn_api_msgs::conversions::Convert(response->lease, &lease_proto);
+  auto lease = ::bosdyn::client::Lease(lease_proto);
+
+  using SubLease = ::bosdyn::client::LeaseWallet::SubLease;
+  lease_wallet_->AddLease(lease, SubLease::kNoSubLease);
+
+  keepalive_bonds_[resource_name] =
+      std::make_unique<bond::Bond>("bonds", lease_wallet_->GetClientName(), background_node_),
+  keepalive_bonds_[resource_name]->start();
+
+  return lease;
+}
+
+tl::expected<::bosdyn::client::Lease, std::string> ProxiedLeasingInterface::ReturnLease(
+    const std::string& resource_name) {
+  auto result = lease_wallet_->GetOwnedLease(resource_name);
+  if (!result) {
+    return tl::make_unexpected(result.status.Chain("No lease owned for " + resource_name).message());
+  }
+  ::bosdyn::client::Lease lease = result.move();
+  auto request = std::make_shared<spot_msgs::srv::ReturnLease::Request>();
+  bosdyn_api_msgs::conversions::Convert(lease.GetProto(), &request->lease);
+  auto future = return_lease_client_->async_send_request(request);
+  auto outcome = rclcpp::spin_until_future_complete(foreground_node_, future, std::chrono::seconds(5));
+  if (outcome == rclcpp::FutureReturnCode::TIMEOUT) {
+    return_lease_client_->remove_pending_request(future);
+    return tl::make_unexpected("Timed out trying to return the lease");
+  }
+  if (outcome == rclcpp::FutureReturnCode::INTERRUPTED) {
+    return_lease_client_->remove_pending_request(future);
+    return tl::make_unexpected("Lease return got interrupted");
+  }
+  auto response = future.get();
+  if (!response->success) {
+    return tl::make_unexpected(response->message);
+  }
+  lease_wallet_->RemoveLease(resource_name);
+  keepalive_bonds_.erase(resource_name);
+  return lease;
+}
+
+}  // namespace spot_hardware_interface