From 87be405ffa3bcccbf1ad99911022cfba411ecd82 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 10 Feb 2025 15:07:39 -0300 Subject: [PATCH 1/3] Add lease management node Signed-off-by: Michel Hidalgo --- spot_driver/CMakeLists.txt | 33 ++ .../spot_driver/api/default_lease_client.hpp | 41 ++ .../spot_driver/api/default_spot_api.hpp | 3 + .../api/lease_client_interface.hpp | 41 ++ .../include/spot_driver/api/spot_api.hpp | 2 + .../spot_driver/lease/lease_manager.hpp | 92 +++ .../spot_driver/lease/lease_manager_node.hpp | 45 ++ .../lease/lease_middleware_handle.hpp | 38 ++ spot_driver/package.xml | 1 + spot_driver/src/api/default_lease_client.cpp | 83 +++ spot_driver/src/api/default_spot_api.cpp | 13 + spot_driver/src/lease/lease_manager.cpp | 166 ++++++ .../src/lease/lease_manager_component.cpp | 7 + spot_driver/src/lease/lease_manager_node.cpp | 71 +++ .../src/lease/lease_manager_node_main.cpp | 15 + .../src/lease/lease_middleware_handle.cpp | 44 ++ spot_driver/test/CMakeLists.txt | 12 + .../spot_driver/mock/mock_lease_client.hpp | 27 + .../spot_driver/mock/mock_spot_api.hpp | 2 + .../test/src/lease/test_lease_manager.cpp | 525 ++++++++++++++++++ spot_msgs/CMakeLists.txt | 2 + spot_msgs/srv/AcquireLease.srv | 6 + spot_msgs/srv/ReturnLease.srv | 4 + 23 files changed, 1273 insertions(+) create mode 100644 spot_driver/include/spot_driver/api/default_lease_client.hpp create mode 100644 spot_driver/include/spot_driver/api/lease_client_interface.hpp create mode 100644 spot_driver/include/spot_driver/lease/lease_manager.hpp create mode 100644 spot_driver/include/spot_driver/lease/lease_manager_node.hpp create mode 100644 spot_driver/include/spot_driver/lease/lease_middleware_handle.hpp create mode 100644 spot_driver/src/api/default_lease_client.cpp create mode 100644 spot_driver/src/lease/lease_manager.cpp create mode 100644 spot_driver/src/lease/lease_manager_component.cpp create mode 100644 spot_driver/src/lease/lease_manager_node.cpp create mode 100644 spot_driver/src/lease/lease_manager_node_main.cpp create mode 100644 spot_driver/src/lease/lease_middleware_handle.cpp create mode 100644 spot_driver/test/include/spot_driver/mock/mock_lease_client.hpp create mode 100644 spot_driver/test/src/lease/test_lease_manager.cpp create mode 100644 spot_msgs/srv/AcquireLease.srv create mode 100644 spot_msgs/srv/ReturnLease.srv diff --git a/spot_driver/CMakeLists.txt b/spot_driver/CMakeLists.txt index bcee60cd5..a297dfe6d 100644 --- a/spot_driver/CMakeLists.txt +++ b/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 + $ + $ +) + +# 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 + $ + $ +) +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 ### diff --git a/spot_driver/include/spot_driver/api/default_lease_client.hpp b/spot_driver/include/spot_driver/api/default_lease_client.hpp new file mode 100644 index 000000000..9642ede4f --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#include +#include +#include + +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 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> keepalives_; + std::mutex keepalives_mutex_; +}; +} // namespace spot_ros2 diff --git a/spot_driver/include/spot_driver/api/default_spot_api.hpp b/spot_driver/include/spot_driver/api/default_spot_api.hpp index 583427ef2..4652a3ef6 100644 --- a/spot_driver/include/spot_driver/api/default_spot_api.hpp +++ b/spot_driver/include/spot_driver/api/default_spot_api.hpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -32,6 +33,7 @@ class DefaultSpotApi : public SpotApi { [[nodiscard]] std::shared_ptr kinematicInterface() const override; [[nodiscard]] std::shared_ptr image_client_interface() const override; [[nodiscard]] std::shared_ptr stateClientInterface() const override; + [[nodiscard]] std::shared_ptr leaseClientInterface() const override; [[nodiscard]] std::shared_ptr timeSyncInterface() const override; [[nodiscard]] std::shared_ptr worldObjectClientInterface() const override; @@ -41,6 +43,7 @@ class DefaultSpotApi : public SpotApi { std::shared_ptr kinematic_interface_; std::shared_ptr image_client_interface_; std::shared_ptr state_client_interface_; + std::shared_ptr lease_client_interface_; std::shared_ptr time_sync_api_; std::shared_ptr world_object_client_interface_; std::string robot_name_; diff --git a/spot_driver/include/spot_driver/api/lease_client_interface.hpp b/spot_driver/include/spot_driver/api/lease_client_interface.hpp new file mode 100644 index 000000000..e5b6f2e0b --- /dev/null +++ b/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 +#include +#include + +#include + +#include +#include +#include + +namespace spot_ros2 { +class LeaseClientInterface { + public: + using LeaseUseCallback = std::function; + + // 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 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 diff --git a/spot_driver/include/spot_driver/api/spot_api.hpp b/spot_driver/include/spot_driver/api/spot_api.hpp index e0d558e80..2ab50eda3 100644 --- a/spot_driver/include/spot_driver/api/spot_api.hpp +++ b/spot_driver/include/spot_driver/api/spot_api.hpp @@ -3,6 +3,7 @@ #pragma once #include +#include #include #include #include @@ -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() const = 0; + [[nodiscard]] virtual std::shared_ptr leaseClientInterface() const = 0; virtual std::shared_ptr timeSyncInterface() const = 0; [[nodiscard]] virtual std::shared_ptr worldObjectClientInterface() const = 0; }; diff --git a/spot_driver/include/spot_driver/lease/lease_manager.hpp b/spot_driver/include/spot_driver/lease/lease_manager.hpp new file mode 100644 index 000000000..ad79a0e68 --- /dev/null +++ b/spot_driver/include/spot_driver/lease/lease_manager.hpp @@ -0,0 +1,92 @@ +// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. + +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace spot_ros2::lease { + +using spot_msgs::srv::AcquireLease; +using spot_msgs::srv::ReturnLease; + +class LeaseManager { + public: + /** + * This middleware handle is used to register a service and assign to it a + * callback. In testing, it can be mocked to avoid using the ROS + * infrastructure. + */ + class MiddlewareHandle { + public: + class Bond { + public: + virtual ~Bond() = default; + }; + + virtual void createAcquireLeaseService( + const std::string& service_name, + std::function, std::shared_ptr)> + callback) = 0; + + virtual void createReturnLeaseService( + const std::string& service_name, + std::function, std::shared_ptr)> + callback) = 0; + + virtual std::shared_ptr createBond(const std::string& node_name, std::function break_callback) = 0; + + virtual ~MiddlewareHandle() = default; + }; + + /** + * Create the logic for the GetInverseKinematicSolutions service. + * @param node The ROS node. + * @param kinematic_api The Api to interact with the Spot SDK. + * @param logger Logging interface. + */ + explicit LeaseManager(std::shared_ptr lease_client, std::shared_ptr logger, + std::unique_ptr middleware_handle); + + /** Initialize the service. */ + void initialize(); + + /** + * Invoke the Spot SDK to get IK solutions. + * @param request The ROS request. + * @param response A ROS response to be filled. + */ + void acquireLease(const std::shared_ptr request, + std::shared_ptr response); + + void returnLease(const std::shared_ptr request, + std::shared_ptr response); + + private: + void onLeaseRetentionFailure(const bosdyn::api::LeaseUseResult& result); + + // The API to interact with Spot SDK. + std::shared_ptr lease_client_; + + // Logger. + std::shared_ptr logger_; + + // The service provider. + std::unique_ptr middleware_handle_; + + struct ManagedSublease { + bosdyn::client::Lease lease; + std::shared_ptr bond; + }; + std::unordered_map subleases_; + std::recursive_mutex subleases_mutex_; +}; +} // namespace spot_ros2::lease diff --git a/spot_driver/include/spot_driver/lease/lease_manager_node.hpp b/spot_driver/include/spot_driver/lease/lease_manager_node.hpp new file mode 100644 index 000000000..8a19e4116 --- /dev/null +++ b/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 + +#include +#include +#include + +#include +#include +#include + +#include + +namespace spot_ros2::lease { +class LeaseManagerNode { + public: + explicit LeaseManagerNode(std::shared_ptr node, std::unique_ptr spot_api, + std::shared_ptr parameter_interface, + std::shared_ptr 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 get_node_base_interface(); + + private: + std::shared_ptr node_; + std::unique_ptr spot_api_; + std::unique_ptr internal_; + + void initialize(std::shared_ptr node, std::unique_ptr spot_api, + std::shared_ptr parameter_interface, + const std::shared_ptr logger_interface); +}; +} // namespace spot_ros2::lease diff --git a/spot_driver/include/spot_driver/lease/lease_middleware_handle.hpp b/spot_driver/include/spot_driver/lease/lease_middleware_handle.hpp new file mode 100644 index 000000000..5c4901142 --- /dev/null +++ b/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 + +#include +#include +#include + +#include +#include + +namespace spot_ros2::lease { + +class LeaseMiddlewareHandle : public LeaseManager::MiddlewareHandle { + public: + explicit LeaseMiddlewareHandle(std::shared_ptr node); + + void createAcquireLeaseService( + const std::string& service_name, + std::function, std::shared_ptr)> + callback) override; + + void createReturnLeaseService( + const std::string& service_name, + std::function, std::shared_ptr)> callback) + override; + + std::shared_ptr createBond(const std::string& node_name, + std::function break_callback) override; + + private: + std::shared_ptr node_; + std::shared_ptr> acquire_lease_service_; + std::shared_ptr> return_lease_service_; +}; +} // namespace spot_ros2::lease diff --git a/spot_driver/package.xml b/spot_driver/package.xml index f642052a0..458ab5c20 100644 --- a/spot_driver/package.xml +++ b/spot_driver/package.xml @@ -11,6 +11,7 @@ ament_cmake_python rosidl_default_generators + bondcpp bosdyn bosdyn_cmake_module bosdyn_msgs diff --git a/spot_driver/src/api/default_lease_client.cpp b/spot_driver/src/api/default_lease_client.cpp new file mode 100644 index 000000000..d678d7ea3 --- /dev/null +++ b/spot_driver/src/api/default_lease_client.cpp @@ -0,0 +1,83 @@ +// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. + +#include + +#include + +namespace spot_ros2 { + +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 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 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 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 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 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(); +} + +const ::bosdyn::client::ResourceHierarchy& DefaultLeaseClient::getResourceHierarchy() const { + return ::bosdyn::client::DefaultResourceHierarchy(::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER); +} + +} // namespace spot_ros2 diff --git a/spot_driver/src/api/default_spot_api.cpp b/spot_driver/src/api/default_spot_api.cpp index c19cdbaae..80d4d6b14 100644 --- a/spot_driver/src/api/default_spot_api.cpp +++ b/spot_driver/src/api/default_spot_api.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -85,6 +86,14 @@ tl::expected DefaultSpotApi::authenticate(const std::string& } state_client_interface_ = std::make_shared(robot_state_result.response); + // Lease API. + const auto lease_client_result = robot_->EnsureServiceClient<::bosdyn::client::LeaseClient>( + ::bosdyn::client::LeaseClient::GetDefaultServiceName()); + if (!lease_client_result.status) { + return tl::make_unexpected("Failed to create Lease client."); + } + lease_client_interface_ = std::make_shared(lease_client_result.response); + // Kinematic API. const auto kinematic_api_result = robot_->EnsureServiceClient<::bosdyn::client::InverseKinematicsClient>( ::bosdyn::client::InverseKinematicsClient::GetDefaultServiceName()); @@ -135,6 +144,10 @@ std::shared_ptr DefaultSpotApi::stateClientInterface() con return state_client_interface_; } +std::shared_ptr DefaultSpotApi::leaseClientInterface() const { + return lease_client_interface_; +} + std::shared_ptr DefaultSpotApi::kinematicInterface() const { return kinematic_interface_; } diff --git a/spot_driver/src/lease/lease_manager.cpp b/spot_driver/src/lease/lease_manager.cpp new file mode 100644 index 000000000..fec653a77 --- /dev/null +++ b/spot_driver/src/lease/lease_manager.cpp @@ -0,0 +1,166 @@ +// Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. + +#include + +#include +#include + +#include +#include + +#include + +namespace { +constexpr auto kAcquireLeaseServiceName = "acquire_lease"; +constexpr auto kReturnLeaseServiceName = "release_lease"; +} // namespace + +namespace spot_ros2::lease { +LeaseManager::LeaseManager(std::shared_ptr lease_client, + std::shared_ptr logger, + std::unique_ptr 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 request, std::shared_ptr response) { + this->acquireLease(request, response); + }); + middleware_handle_->createReturnLeaseService( + kReturnLeaseServiceName, + [this](const std::shared_ptr request, std::shared_ptr response) { + this->returnLease(request, response); + }); +} + +void LeaseManager::onLeaseRetentionFailure(const bosdyn::api::LeaseUseResult& result) { + std::lock_guard 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 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 request, + std::shared_ptr response) { + std::lock_guard lock(subleases_mutex_); + + const auto& root_resource_hierarchy = lease_client_->getResourceHierarchy(); + if (!root_resource_hierarchy.HasResource(request->resource_name)) { + response->message = request->resource_name + " is not a known resource"; + response->success = false; + return; + } + + auto lease_wallet = lease_client_->getLeaseWallet(); + + std::unordered_set potential_collisions{root_resource_hierarchy.Resource()}; + for (const auto& [name, hierarchy] : root_resource_hierarchy) { + if (hierarchy.HasResource(request->resource_name)) { + potential_collisions.insert(name); + } + } + const auto& resource_hierarchy = root_resource_hierarchy.GetHierarchy(request->resource_name); + for (const auto& [name, _] : resource_hierarchy) { + potential_collisions.insert(name); + } + const auto& leaf_resources = resource_hierarchy.LeafResources(); + potential_collisions.insert(leaf_resources.begin(), leaf_resources.end()); + + bool must_take = false; + for (const auto& resource_name : potential_collisions) { + if (subleases_.count(resource_name) > 0) { + const auto& sublease_proto = subleases_[resource_name].lease.GetProto(); + const std::string& client_name = *sublease_proto.client_names().rbegin(); + response->message = resource_name + " is busy, subleased by " + client_name; + response->success = false; + return; + } + if (auto existing_lease = lease_wallet->GetLease(resource_name); existing_lease) { + must_take = true; + } + } + + auto lease = lease_wallet->AdvanceLease(request->resource_name); + if (!lease) { + 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)); + if (!result) { + response->message = "failed to take lease: " + result.error(); + response->success = false; + return; + } + logger_->logInfo(request->resource_name + " lease was taken"); + } else { + auto result = lease_client_->acquireLease(request->resource_name, std::move(callback)); + if (!result) { + response->message = "failed to acquire lease: " + result.error(); + response->success = false; + return; + } + logger_->logInfo(request->resource_name + " lease was acquired"); + } + lease = lease_wallet->GetLease(request->resource_name); + } + + auto sublease = lease.move().CreateSublease(request->client_name); + bosdyn_api_msgs::conversions::Convert(sublease.GetProto(), &response->lease); + + auto bond = middleware_handle_->createBond(request->client_name, [this, sublease]() { + std::lock_guard 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"); + }); + 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 request, + std::shared_ptr response) { + std::lock_guard lock(subleases_mutex_); + + bosdyn::api::Lease sublease_proto; + bosdyn_api_msgs::conversions::Convert(request->lease, &sublease_proto); + auto sublease = bosdyn::client::Lease(sublease_proto); + + const std::string& resource_name = sublease.GetResource(); + if (subleases_.count(resource_name) == 0) { + response->message = resource_name + " is not subleased, cannot return"; + response->success = false; + return; + } + using CompareResult = bosdyn::client::Lease::CompareResult; + if (subleases_[resource_name].lease.Compare(sublease) != CompareResult::SAME) { + response->message = resource_name + " sublease does not match the known lease"; + response->success = false; + return; + } + subleases_.erase(resource_name); + response->success = true; + + logger_->logInfo(resource_name + " sublease was returned"); +} + +} // namespace spot_ros2::lease diff --git a/spot_driver/src/lease/lease_manager_component.cpp b/spot_driver/src/lease/lease_manager_component.cpp new file mode 100644 index 000000000..fed843be9 --- /dev/null +++ b/spot_driver/src/lease/lease_manager_component.cpp @@ -0,0 +1,7 @@ +// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. + +#include +#include +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(spot_ros2::lease::LeaseManagerNode) diff --git a/spot_driver/src/lease/lease_manager_node.cpp b/spot_driver/src/lease/lease_manager_node.cpp new file mode 100644 index 000000000..43562238b --- /dev/null +++ b/spot_driver/src/lease/lease_manager_node.cpp @@ -0,0 +1,71 @@ +// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. + +#include +#include + +#include +#include +#include + +#include +#include + +namespace spot_ros2::lease { + +LeaseManagerNode::LeaseManagerNode(std::shared_ptr node, std::unique_ptr spot_api, + std::shared_ptr parameter_interface, + const std::shared_ptr logger_interface) { + initialize(node, std::move(spot_api), parameter_interface, logger_interface); +} + +LeaseManagerNode::LeaseManagerNode(const rclcpp::NodeOptions& node_options) { + auto node = std::make_shared("lease_manager", node_options); + auto parameter_interface = std::make_shared(node); + auto logger_interface = std::make_shared(node->get_logger()); + const auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); + auto spot_api = + std::make_unique("lease_manager", timesync_timeout, parameter_interface->getCertificate()); + initialize(node, std::move(spot_api), parameter_interface, logger_interface); +} + +void LeaseManagerNode::initialize(std::shared_ptr node, std::unique_ptr spot_api, + std::shared_ptr parameter_interface, + const std::shared_ptr logger_interface) { + node_ = node; + spot_api_ = std::move(spot_api); + + const auto hostname = parameter_interface->getHostname(); + const auto port = parameter_interface->getPort(); + const auto robot_name = parameter_interface->getSpotName(); + const auto username = parameter_interface->getUsername(); + const auto password = parameter_interface->getPassword(); + + // create and authenticate robot + if (const auto create_robot_result = spot_api_->createRobot(robot_name, hostname, port); !create_robot_result) { + const auto error_msg{std::string{"Failed to create interface to robot: "}.append(create_robot_result.error())}; + logger_interface->logError(error_msg); + throw std::runtime_error(error_msg); + } + + if (const auto authenticationResult = spot_api_->authenticate(username, password); !authenticationResult) { + const auto errorMsg{std::string{"Failed to authenticate robot: "}.append(authenticationResult.error())}; + logger_interface->logError(errorMsg); + throw std::runtime_error(errorMsg); + } + + 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."}; + logger_interface->logError(errorMsg); + throw std::runtime_error(errorMsg); + } + + internal_ = std::make_unique(std::move(lease_client), logger_interface, + std::make_unique(node_)); + internal_->initialize(); +} + +std::shared_ptr LeaseManagerNode::get_node_base_interface() { + return node_->get_node_base_interface(); +} +} // namespace spot_ros2::lease diff --git a/spot_driver/src/lease/lease_manager_node_main.cpp b/spot_driver/src/lease/lease_manager_node_main.cpp new file mode 100644 index 000000000..23202f7fe --- /dev/null +++ b/spot_driver/src/lease/lease_manager_node_main.cpp @@ -0,0 +1,15 @@ +// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. + +#include +#include + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + spot_ros2::lease::LeaseManagerNode node; + + // Spins the node with the default single-threaded executor. + rclcpp::spin(node.get_node_base_interface()); + + return 0; +} diff --git a/spot_driver/src/lease/lease_middleware_handle.cpp b/spot_driver/src/lease/lease_middleware_handle.cpp new file mode 100644 index 000000000..5a8b5a755 --- /dev/null +++ b/spot_driver/src/lease/lease_middleware_handle.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. + +#include + +#include + +namespace spot_ros2::lease { + +LeaseMiddlewareHandle::LeaseMiddlewareHandle(std::shared_ptr node) : node_{node} {} + +void LeaseMiddlewareHandle::createAcquireLeaseService( + const std::string& service_name, + std::function, std::shared_ptr)> + callback) { + acquire_lease_service_ = node_->create_service(service_name, callback); +} + +void LeaseMiddlewareHandle::createReturnLeaseService( + const std::string& service_name, + std::function, std::shared_ptr)> callback) { + return_lease_service_ = node_->create_service(service_name, callback); +} + +namespace { + +class BondHolder : public LeaseManager::MiddlewareHandle::Bond { + public: + explicit BondHolder(std::shared_ptr node, const std::string& id, std::function break_callback) + : bond_("lease/bonds", id, std::move(node), std::move(break_callback)) {} + + ~BondHolder() override {} + + private: + bond::Bond bond_; +}; + +} // namespace + +std::shared_ptr LeaseMiddlewareHandle::createBond( + const std::string& node_name, std::function break_callback) { + return std::make_shared(node_, node_name, std::move(break_callback)); +} + +} // namespace spot_ros2::lease diff --git a/spot_driver/test/CMakeLists.txt b/spot_driver/test/CMakeLists.txt index 5325f8dfa..57c84cbe3 100644 --- a/spot_driver/test/CMakeLists.txt +++ b/spot_driver/test/CMakeLists.txt @@ -118,6 +118,18 @@ ament_add_gmock(test_common_conversions ) target_link_libraries(test_common_conversions spot_api) +# # test_lease_manager + +ament_add_gmock(test_lease_manager + src/lease/test_lease_manager.cpp +) +target_include_directories(test_lease_manager + PUBLIC + $ + $ +) +target_link_libraries(test_lease_manager spot_api) + # test_kinematic_service ament_add_gmock(test_kinematic_service diff --git a/spot_driver/test/include/spot_driver/mock/mock_lease_client.hpp b/spot_driver/test/include/spot_driver/mock/mock_lease_client.hpp new file mode 100644 index 000000000..e82ac36ef --- /dev/null +++ b/spot_driver/test/include/spot_driver/mock/mock_lease_client.hpp @@ -0,0 +1,27 @@ +// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. + +#pragma once + +#include + +#include +#include + +#include + +namespace spot_ros2::test { +class MockLeaseClient : public LeaseClientInterface { + public: + MOCK_METHOD((tl::expected<::bosdyn::client::Lease, std::string>), acquireLease, + (const std::string&, LeaseUseCallback), (override)); + + MOCK_METHOD((tl::expected<::bosdyn::client::Lease, std::string>), takeLease, (const std::string&, LeaseUseCallback), + (override)); + + MOCK_METHOD((tl::expected), returnLease, (const ::bosdyn::client::Lease&), (override)); + + MOCK_METHOD((std::shared_ptr<::bosdyn::client::LeaseWallet>), getLeaseWallet, (), (const, override)); + + MOCK_METHOD((const ::bosdyn::client::ResourceHierarchy&), getResourceHierarchy, (), (const, override)); +}; +} // namespace spot_ros2::test diff --git a/spot_driver/test/include/spot_driver/mock/mock_spot_api.hpp b/spot_driver/test/include/spot_driver/mock/mock_spot_api.hpp index 74c8c2e5c..1fc6298d6 100644 --- a/spot_driver/test/include/spot_driver/mock/mock_spot_api.hpp +++ b/spot_driver/test/include/spot_driver/mock/mock_spot_api.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -25,6 +26,7 @@ class MockSpotApi : public SpotApi { MOCK_METHOD(std::shared_ptr, kinematicInterface, (), (const, override)); MOCK_METHOD(std::shared_ptr, image_client_interface, (), (const, override)); MOCK_METHOD(std::shared_ptr, stateClientInterface, (), (const, override)); + MOCK_METHOD(std::shared_ptr, leaseClientInterface, (), (const, override)); MOCK_METHOD(std::shared_ptr, timeSyncInterface, (), (const, override)); MOCK_METHOD(std::shared_ptr, worldObjectClientInterface, (), (const, override)); }; diff --git a/spot_driver/test/src/lease/test_lease_manager.cpp b/spot_driver/test/src/lease/test_lease_manager.cpp new file mode 100644 index 000000000..7e0f5a8e1 --- /dev/null +++ b/spot_driver/test/src/lease/test_lease_manager.cpp @@ -0,0 +1,525 @@ +// Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. + +#include + +#include + +#include + +#include + +#include +#include +#include + +#include + +namespace spot_ros2::lease::test { + +using LeaseUseCallback = LeaseClientInterface::LeaseUseCallback; + +using ::bosdyn::client::kArmResource; +using ::bosdyn::client::kBodyResource; + +using ::testing::_; +using ::testing::AnyNumber; +using ::testing::Return; +using ::testing::SaveArg; + +class MockMiddlewareHandle : public LeaseManager::MiddlewareHandle { + public: + MOCK_METHOD( + (void), createAcquireLeaseService, + (const std::string& serviceName, + std::function, std::shared_ptr)> + callback), + (override)); + + MOCK_METHOD((void), createReturnLeaseService, + (const std::string& serviceName, + std::function, std::shared_ptr)> + callback), + (override)); + + MOCK_METHOD(std::shared_ptr, createBond, + (const std::string& nodeName, std::function break_callback), (override)); +}; + +/** + * Test lease manager initialization. + */ +TEST(TestLeaseManager, initializationIsComplete) { + auto lease_client = std::make_unique(); + auto logger = std::make_unique(); + auto middleware = std::make_unique(); + EXPECT_CALL(*middleware, createAcquireLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createReturnLeaseService(_, _)).Times(1); + + auto lease_manager = + std::make_unique(std::move(lease_client), std::move(logger), std::move(middleware)); + lease_manager->initialize(); +} + +/** + * Test a single body sublease succeeds. + */ +TEST(TestLeaseManager, simpleLeasingSucceeds) { + // GIVEN body lease acquisition succeeds the first time it is called + auto lease_client = std::make_unique(); + + auto lease_wallet = std::make_shared<::bosdyn::client::LeaseWallet>("test_manager"); + EXPECT_CALL(*lease_client, getLeaseWallet()).WillOnce(Return(lease_wallet)); + + EXPECT_CALL(*lease_client, getResourceHierarchy()).WillOnce([]() -> const ::bosdyn::client::ResourceHierarchy& { + const auto requirement = ::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER; + return ::bosdyn::client::DefaultResourceHierarchy(requirement); + }); + + auto lease_proto = ::bosdyn::api::Lease(); + lease_proto.set_resource(kBodyResource); + lease_proto.add_sequence(0); + auto lease = ::bosdyn::client::Lease(lease_proto); + + EXPECT_CALL(*lease_client, acquireLease(kBodyResource, _)).WillOnce([&](const std::string&, LeaseUseCallback) { + lease_wallet->AddLease(lease); + return lease; + }); + + auto logger = std::make_unique(); + EXPECT_CALL(*logger, logInfo(_)).Times(AnyNumber()); + auto middleware = std::make_unique(); + EXPECT_CALL(*middleware, createAcquireLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createReturnLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createBond(_, _)).Times(1); + + auto lease_manager = + std::make_unique(std::move(lease_client), std::move(logger), std::move(middleware)); + lease_manager->initialize(); + + // WHEN a sublease request is placed + const std::string kClientNodeName = "dummy_node"; + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + + // THEN the response should bear a valid sublease + ASSERT_TRUE(response->success); + auto sublease_proto = ::bosdyn::api::Lease(); + bosdyn_api_msgs::conversions::Convert(response->lease, &sublease_proto); + const auto sublease = ::bosdyn::client::Lease(sublease_proto); + ASSERT_TRUE(sublease.IsValid()); + ASSERT_EQ(sublease.GetResource(), kBodyResource); + using CompareResult = ::bosdyn::client::Lease::CompareResult; + ASSERT_EQ(lease.Compare(sublease), CompareResult::SUPER_LEASE); + ASSERT_GT(sublease_proto.client_names_size(), 0); + ASSERT_EQ(*sublease_proto.client_names().rbegin(), kClientNodeName); +} + +/** + * Test body can be subleased multiple times in sequence. + */ +TEST(TestLeaseManager, sequentialLeasingSucceeds) { + const std::string kClientNodeName = "dummy_node"; + + // GIVEN body lease acquisition succeeds the first time it is called + auto lease_client = std::make_unique(); + + auto lease_wallet = std::make_shared<::bosdyn::client::LeaseWallet>("test_manager"); + EXPECT_CALL(*lease_client, getLeaseWallet()).WillRepeatedly(Return(lease_wallet)); + + EXPECT_CALL(*lease_client, getResourceHierarchy()).WillRepeatedly([]() -> const ::bosdyn::client::ResourceHierarchy& { + const auto requirement = ::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER; + return ::bosdyn::client::DefaultResourceHierarchy(requirement); + }); + + auto lease_proto = ::bosdyn::api::Lease(); + lease_proto.set_resource(kBodyResource); + lease_proto.add_sequence(0); + auto lease = ::bosdyn::client::Lease(lease_proto); + + EXPECT_CALL(*lease_client, acquireLease(kBodyResource, _)).WillOnce([&](const std::string&, LeaseUseCallback) { + lease_wallet->AddLease(lease); + return lease; + }); + + auto logger = std::make_unique(); + EXPECT_CALL(*logger, logInfo(_)).Times(AnyNumber()); + auto middleware = std::make_unique(); + EXPECT_CALL(*middleware, createAcquireLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createReturnLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createBond(_, _)).Times(2); + + auto lease_manager = + std::make_unique(std::move(lease_client), std::move(logger), std::move(middleware)); + lease_manager->initialize(); + + ::bosdyn::client::Lease first_sublease; + { + // WHEN a sublease acquisition request is placed + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + + // THEN the response should bear a valid sublease + ASSERT_TRUE(response->success); + auto sublease_proto = ::bosdyn::api::Lease(); + bosdyn_api_msgs::conversions::Convert(response->lease, &sublease_proto); + first_sublease = ::bosdyn::client::Lease(sublease_proto); + ASSERT_TRUE(first_sublease.IsValid()); + ASSERT_EQ(first_sublease.GetResource(), kBodyResource); + using CompareResult = ::bosdyn::client::Lease::CompareResult; + ASSERT_EQ(lease.Compare(first_sublease), CompareResult::SUPER_LEASE); + ASSERT_GT(sublease_proto.client_names_size(), 0); + ASSERT_EQ(*sublease_proto.client_names().rbegin(), kClientNodeName); + } + + { + // WHEN a sublease return request is placed + auto request = std::make_shared(); + bosdyn_api_msgs::conversions::Convert(first_sublease.GetProto(), &request->lease); + auto response = std::make_shared(); + lease_manager->returnLease(request, response); + + // THEN the response should indicate success + ASSERT_TRUE(response->success) << response->message; + } + + ::bosdyn::client::Lease second_sublease; + { + // WHEN a second sublease acquisition request is placed + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + + // THEN the response should bear a valid sublease + ASSERT_TRUE(response->success); + auto sublease_proto = ::bosdyn::api::Lease(); + bosdyn_api_msgs::conversions::Convert(response->lease, &sublease_proto); + second_sublease = ::bosdyn::client::Lease(sublease_proto); + ASSERT_TRUE(second_sublease.IsValid()); + ASSERT_EQ(second_sublease.GetResource(), kBodyResource); + using CompareResult = ::bosdyn::client::Lease::CompareResult; + ASSERT_EQ(lease.Compare(second_sublease), CompareResult::SUPER_LEASE); + ASSERT_GT(sublease_proto.client_names_size(), 0); + ASSERT_EQ(*sublease_proto.client_names().rbegin(), kClientNodeName); + } + + using CompareResult = ::bosdyn::client::Lease::CompareResult; + ASSERT_EQ(first_sublease.Compare(second_sublease), CompareResult::OLDER); +} + +/** + * Test body may not be subleased twice at the same time. + */ +TEST(TestLeaseManager, doubleLeasingFails) { + // GIVEN a body lease is held by the lease manager + auto lease_client = std::make_unique(); + auto lease_wallet = std::make_shared<::bosdyn::client::LeaseWallet>("test_manager"); + EXPECT_CALL(*lease_client, getLeaseWallet()).WillRepeatedly(Return(lease_wallet)); + + EXPECT_CALL(*lease_client, getResourceHierarchy()).WillRepeatedly([]() -> const ::bosdyn::client::ResourceHierarchy& { + const auto requirement = ::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER; + return ::bosdyn::client::DefaultResourceHierarchy(requirement); + }); + + auto logger = std::make_unique(); + EXPECT_CALL(*logger, logInfo(_)).Times(AnyNumber()); + + auto middleware = std::make_unique(); + EXPECT_CALL(*middleware, createAcquireLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createReturnLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createBond(_, _)).Times(1); + + auto lease_manager = + std::make_unique(std::move(lease_client), std::move(logger), std::move(middleware)); + lease_manager->initialize(); + + auto lease_proto = ::bosdyn::api::Lease(); + lease_proto.set_resource(kBodyResource); + lease_proto.add_sequence(0); + auto lease = ::bosdyn::client::Lease(lease_proto); + + lease_wallet->AddLease(lease); + + { + // WHEN a first sublease request is placed + const std::string kClientNodeName = "dummy_node"; + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + // THEN the response should bear a valid sublease + ASSERT_TRUE(response->success); + } + + { + // WHEN a second lease request is placed + const std::string kClientNodeName = "other_dummy_node"; + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + // THEN the response should indicate failure + ASSERT_FALSE(response->success); + } +} + +/** + * Test a body sublease prevents an arm sublease + */ +TEST(TestLeaseManager, rootLeaseBlocksLeafLeasing) { + // GIVEN a body lease is held by the lease manager + auto lease_client = std::make_unique(); + auto lease_wallet = std::make_shared<::bosdyn::client::LeaseWallet>("test_manager"); + EXPECT_CALL(*lease_client, getLeaseWallet()).WillRepeatedly(Return(lease_wallet)); + + EXPECT_CALL(*lease_client, getResourceHierarchy()).WillRepeatedly([]() -> const ::bosdyn::client::ResourceHierarchy& { + const auto requirement = ::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER; + return ::bosdyn::client::DefaultResourceHierarchy(requirement); + }); + + auto logger = std::make_unique(); + EXPECT_CALL(*logger, logInfo(_)).Times(AnyNumber()); + + auto middleware = std::make_unique(); + EXPECT_CALL(*middleware, createAcquireLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createReturnLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createBond(_, _)).Times(1); + + auto lease_manager = + std::make_unique(std::move(lease_client), std::move(logger), std::move(middleware)); + lease_manager->initialize(); + + auto body_lease_proto = ::bosdyn::api::Lease(); + body_lease_proto.set_resource(kBodyResource); + body_lease_proto.add_sequence(0); + auto body_lease = ::bosdyn::client::Lease(body_lease_proto); + lease_wallet->AddLease(body_lease); + + { + // WHEN a body sublease request is placed + const std::string kClientNodeName = "dummy_node"; + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + // THEN the response should bear a valid sublease + ASSERT_TRUE(response->success); + } + + { + // WHEN an arm sublease request is placed + const std::string kClientNodeName = "other_dummy_node"; + auto request = std::make_shared(); + request->resource_name = kArmResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + // THEN the response should indicate failure + ASSERT_FALSE(response->success); + } +} + +/** + * Test an arm lease prevents a body lease + */ +TEST(TestLeaseManager, leafLeaseBlocksRootLeasing) { + // GIVEN an arm lease is held by the lease manager + auto lease_client = std::make_unique(); + auto lease_wallet = std::make_shared<::bosdyn::client::LeaseWallet>("test_manager"); + EXPECT_CALL(*lease_client, getLeaseWallet()).WillRepeatedly(Return(lease_wallet)); + + EXPECT_CALL(*lease_client, getResourceHierarchy()).WillRepeatedly([]() -> const ::bosdyn::client::ResourceHierarchy& { + const auto requirement = ::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER; + return ::bosdyn::client::DefaultResourceHierarchy(requirement); + }); + + auto logger = std::make_unique(); + EXPECT_CALL(*logger, logInfo(_)).Times(AnyNumber()); + + auto middleware = std::make_unique(); + EXPECT_CALL(*middleware, createAcquireLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createReturnLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createBond(_, _)).Times(1); + + auto lease_manager = + std::make_unique(std::move(lease_client), std::move(logger), std::move(middleware)); + lease_manager->initialize(); + + auto arm_lease_proto = ::bosdyn::api::Lease(); + arm_lease_proto.set_resource(kArmResource); + arm_lease_proto.add_sequence(0); + auto arm_lease = ::bosdyn::client::Lease(arm_lease_proto); + lease_wallet->AddLease(arm_lease); + + { + // WHEN an arm sublease request is placed + const std::string kClientNodeName = "dummy_node"; + auto request = std::make_shared(); + request->resource_name = kArmResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + // THEN the response should bear a valid sublease + ASSERT_TRUE(response->success); + } + + { + // WHEN a body sublease request is placed + const std::string kClientNodeName = "other_dummy_node"; + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + // THEN the response should indicate failure + ASSERT_FALSE(response->success); + } +} + +/** + * Test lease retention failure leads to sublease revocation + */ +TEST(TestLeaseManager, revocationOnLeaseRetentionFailure) { + // GIVEN body sublease acquisition succeeds the first time it is called + auto lease_client = std::make_unique(); + auto lease_wallet = std::make_shared<::bosdyn::client::LeaseWallet>("test_manager"); + EXPECT_CALL(*lease_client, getLeaseWallet()).WillRepeatedly(Return(lease_wallet)); + + EXPECT_CALL(*lease_client, getResourceHierarchy()).WillRepeatedly([]() -> const ::bosdyn::client::ResourceHierarchy& { + const auto requirement = ::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER; + return ::bosdyn::client::DefaultResourceHierarchy(requirement); + }); + + auto lease_proto = ::bosdyn::api::Lease(); + lease_proto.set_resource(kBodyResource); + lease_proto.add_sequence(0); + auto lease = ::bosdyn::client::Lease(lease_proto); + + LeaseUseCallback retention_failure_callback; + EXPECT_CALL(*lease_client, acquireLease(kBodyResource, _)) + .WillOnce([&](const std::string&, LeaseUseCallback callback) { + lease_wallet->AddLease(lease); + retention_failure_callback = std::move(callback); + return lease; + }); + + auto logger = std::make_unique(); + EXPECT_CALL(*logger, logInfo(_)).Times(AnyNumber()); + auto middleware = std::make_unique(); + EXPECT_CALL(*middleware, createAcquireLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createReturnLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createBond(_, _)).Times(1); + + auto lease_manager = + std::make_unique(std::move(lease_client), std::move(logger), std::move(middleware)); + lease_manager->initialize(); + + { + const std::string kClientNodeName = "dummy_node"; + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + ASSERT_TRUE(response->success); + } + ASSERT_TRUE(lease_wallet->GetLease(kBodyResource)); + + // WHEN body lease retention fails + auto lease_use_result = ::bosdyn::api::LeaseUseResult(); + *lease_use_result.mutable_attempted_lease() = lease_proto; + retention_failure_callback(lease_use_result); + + // THEN both lease and sublease are revoked + { + auto request = std::make_shared(); + bosdyn_api_msgs::conversions::Convert(lease.GetProto(), &request->lease); + auto response = std::make_shared(); + lease_manager->returnLease(request, response); + ASSERT_FALSE(response->success) << response->message; + } + ASSERT_FALSE(lease_wallet->GetLease(kBodyResource)); +} + +/** + * Test client bond breakage leads to sublease revocation + */ +TEST(TestLeaseManager, revocationOnBondBreakage) { + // GIVEN body sublease acquisition succeeds the first time it is called + auto lease_client = std::make_unique(); + auto lease_wallet = std::make_shared<::bosdyn::client::LeaseWallet>("test_manager"); + EXPECT_CALL(*lease_client, getLeaseWallet()).WillRepeatedly(Return(lease_wallet)); + + EXPECT_CALL(*lease_client, getResourceHierarchy()).WillRepeatedly([]() -> const ::bosdyn::client::ResourceHierarchy& { + const auto requirement = ::bosdyn::client::LeaseHierarchyRequirements::ARM_AND_GRIPPER; + return ::bosdyn::client::DefaultResourceHierarchy(requirement); + }); + + auto logger = std::make_unique(); + EXPECT_CALL(*logger, logInfo(_)).Times(AnyNumber()); + EXPECT_CALL(*logger, logError(_)).Times(AnyNumber()); + + { + auto initial_lease_proto = ::bosdyn::api::Lease(); + initial_lease_proto.set_resource(kBodyResource); + initial_lease_proto.add_sequence(0); + auto initial_lease = ::bosdyn::client::Lease(initial_lease_proto); + lease_wallet->AddLease(initial_lease); + } + + EXPECT_CALL(*lease_client, takeLease(kBodyResource, _)).WillOnce([&](const std::string&, LeaseUseCallback) { + auto lease_proto = ::bosdyn::api::Lease(); + lease_proto.set_resource(kBodyResource); + lease_proto.add_sequence(1); + auto lease = ::bosdyn::client::Lease(lease_proto); + lease_wallet->AddLease(lease); + return lease; + }); + + auto middleware = std::make_unique(); + EXPECT_CALL(*middleware, createAcquireLeaseService(_, _)).Times(1); + EXPECT_CALL(*middleware, createReturnLeaseService(_, _)).Times(1); + + const std::string kClientNodeName = "dummy_node"; + std::function bond_break_callback; + EXPECT_CALL(*middleware, createBond(kClientNodeName, _)) + .WillOnce([&](const std::string&, std::function callback) { + bond_break_callback = std::move(callback); + return nullptr; + }); + + auto lease_manager = + std::make_unique(std::move(lease_client), std::move(logger), std::move(middleware)); + lease_manager->initialize(); + + auto request = std::make_shared(); + request->resource_name = kBodyResource; + request->client_name = kClientNodeName; + auto response = std::make_shared(); + lease_manager->acquireLease(request, response); + ASSERT_TRUE(response->success); + auto sublease_proto = ::bosdyn::api::Lease(); + bosdyn_api_msgs::conversions::Convert(response->lease, &sublease_proto); + const auto sublease = ::bosdyn::client::Lease(sublease_proto); + ASSERT_TRUE(sublease.IsValid()); + + // WHEN client bond breaks + bond_break_callback(); + + // THEN sublease is revoked + auto lease = lease_wallet->GetLease(kBodyResource).move(); + using CompareResult = ::bosdyn::client::Lease::CompareResult; + ASSERT_EQ(lease.Compare(sublease), CompareResult::NEWER); +} + +} // namespace spot_ros2::lease::test diff --git a/spot_msgs/CMakeLists.txt b/spot_msgs/CMakeLists.txt index b3d2a9ec7..d85364dbe 100644 --- a/spot_msgs/CMakeLists.txt +++ b/spot_msgs/CMakeLists.txt @@ -56,6 +56,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/LeaseResource.msg" "msg/PowerState.msg" "msg/SystemFaultState.msg" + "srv/AcquireLease.srv" "srv/ChoreographyRecordedStateToAnimation.srv" "srv/ChoreographyStartRecordingState.srv" "srv/ChoreographyStopRecordingState.srv" @@ -63,6 +64,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/GetInverseKinematicSolutions.srv" "srv/ListGraph.srv" "srv/ListWorldObjects.srv" + "srv/ReturnLease.srv" "srv/SetLocomotion.srv" "srv/SetVelocity.srv" "srv/ListAllDances.srv" diff --git a/spot_msgs/srv/AcquireLease.srv b/spot_msgs/srv/AcquireLease.srv new file mode 100644 index 000000000..eb84d20ea --- /dev/null +++ b/spot_msgs/srv/AcquireLease.srv @@ -0,0 +1,6 @@ +string client_name +string resource_name +--- +bool success +string message +bosdyn_api_msgs/Lease lease diff --git a/spot_msgs/srv/ReturnLease.srv b/spot_msgs/srv/ReturnLease.srv new file mode 100644 index 000000000..3b362a295 --- /dev/null +++ b/spot_msgs/srv/ReturnLease.srv @@ -0,0 +1,4 @@ +bosdyn_api_msgs/Lease lease +--- +bool success +string message From 5ce02cfac27b50189187a4be3b29072579576b72 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 18 Feb 2025 21:45:30 -0300 Subject: [PATCH 2/3] Polish patch Signed-off-by: Michel Hidalgo --- .../spot_driver/api/default_lease_client.hpp | 2 +- .../api/lease_client_interface.hpp | 29 +++++++++++ .../spot_driver/lease/lease_manager.hpp | 50 +++++++++++++++---- spot_driver/src/lease/lease_manager.cpp | 2 +- .../src/lease/lease_middleware_handle.cpp | 2 +- 5 files changed, 71 insertions(+), 14 deletions(-) diff --git a/spot_driver/include/spot_driver/api/default_lease_client.hpp b/spot_driver/include/spot_driver/api/default_lease_client.hpp index 9642ede4f..f1f0239b5 100644 --- a/spot_driver/include/spot_driver/api/default_lease_client.hpp +++ b/spot_driver/include/spot_driver/api/default_lease_client.hpp @@ -15,7 +15,7 @@ namespace spot_ros2 { /** - * @brief Implements LeaseClientInterface to use the Spot C++ Lease Client. + * @brief Implements LeaseClientInterface. */ class DefaultLeaseClient : public LeaseClientInterface { public: diff --git a/spot_driver/include/spot_driver/api/lease_client_interface.hpp b/spot_driver/include/spot_driver/api/lease_client_interface.hpp index e5b6f2e0b..65852131b 100644 --- a/spot_driver/include/spot_driver/api/lease_client_interface.hpp +++ b/spot_driver/include/spot_driver/api/lease_client_interface.hpp @@ -13,6 +13,9 @@ #include namespace spot_ros2 { +/** + * @brief Interface class to interact with Spot SDK's lease client. + */ class LeaseClientInterface { public: using LeaseUseCallback = std::function; @@ -26,16 +29,42 @@ class LeaseClientInterface { virtual ~LeaseClientInterface() = default; + /** + * @brief Acquire the lease for a given resource. + * @param resource_name Name of the resource. + * @param retention_failure_callback Callback on lease retention failure, invoked by the underlying lease keepalive. + * @return Returns an expected bearing the Lease if the lease was acquired. Otherwise, it will carry an error message + * describing the failure. + */ virtual tl::expected<::bosdyn::client::Lease, std::string> acquireLease( const std::string& resource_name, LeaseUseCallback retention_failure_callback) = 0; + /** + * @brief Take (forcefully) the lease for a given resource. + * @param resource_name Name of the resource. + * @param retention_failure_callback Callback on lease retention failure, invoked by the underlying lease keepalive. + * @return Returns an expected bearing the Lease if the lease was taken. Otherwise, it will carry an error message + * describing the failure. + */ virtual tl::expected<::bosdyn::client::Lease, std::string> takeLease(const std::string& resource_name, LeaseUseCallback retention_failure_callback) = 0; + /** + * @brief Return a given lease. + * @param lease Lease to be returned. + * @return Returns an expected bearing true if the lease was returned. + * Otherwise, it will carry an error message describing the failure. + */ virtual tl::expected returnLease(const ::bosdyn::client::Lease& lease) = 0; + /** + * @return The underlying lease wallet. + */ virtual std::shared_ptr<::bosdyn::client::LeaseWallet> getLeaseWallet() const = 0; + /** + * @return The resource hierarchy describing the robot hardware. + */ virtual const ::bosdyn::client::ResourceHierarchy& getResourceHierarchy() const = 0; }; } // namespace spot_ros2 diff --git a/spot_driver/include/spot_driver/lease/lease_manager.hpp b/spot_driver/include/spot_driver/lease/lease_manager.hpp index ad79a0e68..ea593121c 100644 --- a/spot_driver/include/spot_driver/lease/lease_manager.hpp +++ b/spot_driver/include/spot_driver/lease/lease_manager.hpp @@ -18,11 +18,18 @@ namespace spot_ros2::lease { using spot_msgs::srv::AcquireLease; using spot_msgs::srv::ReturnLease; +/** + * Lease management for a ROS 2 graph. + * + * This manager acts as a proxy for leasing, allowing multiple + * ROS 2 nodes to cooperate without losing ownership over robot + * hardware. + */ class LeaseManager { public: /** - * This middleware handle is used to register a service and assign to it a - * callback. In testing, it can be mocked to avoid using the ROS + * This middleware handle is used to register services and instantiate + * node bonds. In testing, it can be mocked to avoid using the ROS * infrastructure. */ class MiddlewareHandle { @@ -48,10 +55,10 @@ class LeaseManager { }; /** - * Create the logic for the GetInverseKinematicSolutions service. - * @param node The ROS node. - * @param kinematic_api The Api to interact with the Spot SDK. + * Instantiate the lease manager. + * @param lease_client The ROS node. * @param logger Logging interface. + * @param middleware_handle */ explicit LeaseManager(std::shared_ptr lease_client, std::shared_ptr logger, std::unique_ptr middleware_handle); @@ -60,33 +67,54 @@ class LeaseManager { void initialize(); /** - * Invoke the Spot SDK to get IK solutions. + * Acquire lease on request. + * + * Typically used to implement the associated service. + * * @param request The ROS request. * @param response A ROS response to be filled. */ void acquireLease(const std::shared_ptr request, std::shared_ptr response); + /** + * Return lease on request. + * + * Typically used to implement the associated service. + * + * @param request The ROS request. + * @param response A ROS response to be filled. + */ void returnLease(const std::shared_ptr request, std::shared_ptr response); private: + /** + * Handle lease retention failure. + * + * Typically hooked up to the underlying lease keepalive. + * + * @param result The lease use result returned on lease retention failure. + */ void onLeaseRetentionFailure(const bosdyn::api::LeaseUseResult& result); - // The API to interact with Spot SDK. + // An interface to the lease API. std::shared_ptr lease_client_; - // Logger. + // An interface to logging API. std::shared_ptr logger_; - // The service provider. + // An interface to the middleware provider. std::unique_ptr middleware_handle_; struct ManagedSublease { - bosdyn::client::Lease lease; - std::shared_ptr bond; + bosdyn::client::Lease lease; // Actual SDK sublease. + std::shared_ptr bond; // ROS layer keepalive. }; + // Storage for active subleases (and associated keepalives). std::unordered_map subleases_; + + // Synchronization primitive for sublease storage. std::recursive_mutex subleases_mutex_; }; } // namespace spot_ros2::lease diff --git a/spot_driver/src/lease/lease_manager.cpp b/spot_driver/src/lease/lease_manager.cpp index fec653a77..36611f72b 100644 --- a/spot_driver/src/lease/lease_manager.cpp +++ b/spot_driver/src/lease/lease_manager.cpp @@ -12,7 +12,7 @@ namespace { constexpr auto kAcquireLeaseServiceName = "acquire_lease"; -constexpr auto kReturnLeaseServiceName = "release_lease"; +constexpr auto kReturnLeaseServiceName = "return_lease"; } // namespace namespace spot_ros2::lease { diff --git a/spot_driver/src/lease/lease_middleware_handle.cpp b/spot_driver/src/lease/lease_middleware_handle.cpp index 5a8b5a755..a16af6dcd 100644 --- a/spot_driver/src/lease/lease_middleware_handle.cpp +++ b/spot_driver/src/lease/lease_middleware_handle.cpp @@ -26,7 +26,7 @@ namespace { class BondHolder : public LeaseManager::MiddlewareHandle::Bond { public: explicit BondHolder(std::shared_ptr node, const std::string& id, std::function break_callback) - : bond_("lease/bonds", id, std::move(node), std::move(break_callback)) {} + : bond_("bonds", id, std::move(node), std::move(break_callback)) {} ~BondHolder() override {} From cf0ddfad6f9927883d6b0db10e726c62102184c6 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 18 Feb 2025 21:46:38 -0300 Subject: [PATCH 3/3] Add support for Spot leasing by proxy to its hardware interface Signed-off-by: Michel Hidalgo --- spot_hardware_interface/CMakeLists.txt | 3 + .../spot_hardware_interface.hpp | 8 +- .../spot_leasing_interface.hpp | 86 +++++++++++ spot_hardware_interface/package.xml | 7 +- .../src/spot_hardware_interface.cpp | 51 ++++--- .../src/spot_leasing_interface.cpp | 141 ++++++++++++++++++ 6 files changed, 273 insertions(+), 23 deletions(-) create mode 100644 spot_hardware_interface/include/spot_hardware_interface/spot_leasing_interface.hpp create mode 100644 spot_hardware_interface/src/spot_leasing_interface.cpp diff --git a/spot_hardware_interface/CMakeLists.txt b/spot_hardware_interface/CMakeLists.txt index 31a8d610e..84d37cb28 100644 --- a/spot_hardware_interface/CMakeLists.txt +++ b/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 diff --git a/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp b/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp index e2739a8de..3bff24d7b 100644 --- a/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp +++ b/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 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 diff --git a/spot_hardware_interface/include/spot_hardware_interface/spot_leasing_interface.hpp b/spot_hardware_interface/include/spot_hardware_interface/spot_leasing_interface.hpp new file mode 100644 index 000000000..ffbe14d40 --- /dev/null +++ b/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 +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +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 foreground_node_; + std::shared_ptr> acquire_lease_client_; + std::shared_ptr> return_lease_client_; + + std::jthread background_loop_; + std::shared_ptr background_node_; + std::unique_ptr background_executor_; + + std::unordered_map> keepalive_bonds_; +}; + +} // namespace spot_hardware_interface diff --git a/spot_hardware_interface/package.xml b/spot_hardware_interface/package.xml index 18b53c760..783f3175c 100644 --- a/spot_hardware_interface/package.xml +++ b/spot_hardware_interface/package.xml @@ -12,12 +12,15 @@ Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. ament_cmake + bondcpp + bosdyn + bosdyn_cmake_module hardware_interface pluginlib rclcpp rclcpp_lifecycle - bosdyn - bosdyn_cmake_module + spot_msgs + tl_expected spot_description xacro diff --git a/spot_hardware_interface/src/spot_hardware_interface.cpp b/spot_hardware_interface/src/spot_hardware_interface.cpp index 5609563b0..36e8ff5e0 100644 --- a/spot_hardware_interface/src/spot_hardware_interface.cpp +++ b/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::quiet_NaN()); hw_commands_.resize(info_.joints.size() * interfaces_per_joint_, std::numeric_limits::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(robot_.get()); + break; + case LeasingMode::DIRECT: + leasing_interface_ = std::make_unique(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 diff --git a/spot_hardware_interface/src/spot_leasing_interface.cpp b/spot_hardware_interface/src/spot_leasing_interface.cpp new file mode 100644 index 000000000..7f0275bf8 --- /dev/null +++ b/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 +#include + +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("_foreground_" + lease_wallet_->GetClientName() + "_node"); + acquire_lease_client_ = foreground_node_->create_client("acquire_lease"); + return_lease_client_ = foreground_node_->create_client("return_lease"); + background_node_ = std::make_shared("_background_" + lease_wallet_->GetClientName() + "_node"); + background_executor_ = std::make_unique(); + 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(); + 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("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(); + 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