Complete lease manager API

- Add claim/release services
- Include periodic lease status

Signed-off-by: Michel Hidalgo <mhidalgo@theaiinstitute.com>

spot_driver/CMakeLists.txt

@@ -31,6 +31,7 @@ set(THIS_PACKAGE_INCLUDE_ROS_DEPENDS
   tf2_ros
   tl_expected
   spot_msgs
+  std_srvs
   nav_msgs
 )
 

spot_driver/include/spot_driver/api/default_lease_client.hpp

@@ -12,6 +12,7 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
+#include <vector>
 
 namespace spot_ros2 {
 /**
@@ -29,6 +30,8 @@ class DefaultLeaseClient : public LeaseClientInterface {
 
   [[nodiscard]] tl::expected<bool, std::string> returnLease(const ::bosdyn::client::Lease& lease) override;
 
+  [[nodiscard]] tl::expected<std::vector<::bosdyn::api::LeaseResource>, std::string> listLeases() override;
+
   [[nodiscard]] std::shared_ptr<::bosdyn::client::LeaseWallet> getLeaseWallet() const override;
 
   [[nodiscard]] const ::bosdyn::client::ResourceHierarchy& getResourceHierarchy() const override;

spot_driver/include/spot_driver/api/lease_client_interface.hpp

@@ -5,6 +5,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include <tl_expected/expected.hpp>
 
@@ -57,6 +58,13 @@ class LeaseClientInterface {
    */
   virtual tl::expected<bool, std::string> returnLease(const ::bosdyn::client::Lease& lease) = 0;
 
+  /**
+   * @brief Return all active leases.
+   * @return Returns an expected bearing a collection of leased resources.
+   * Otherwise, it will carry an error message describing the failure.
+   */
+  virtual tl::expected<std::vector<::bosdyn::api::LeaseResource>, std::string> listLeases() = 0;
+
   /**
    * @return The underlying lease wallet.
    */

spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp

@@ -48,6 +48,7 @@ class ParameterInterfaceBase {
   virtual tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(bool has_arm,
                                                                                     bool gripperless) const = 0;
   virtual std::chrono::seconds getTimeSyncTimeout() const = 0;
+  virtual std::optional<double> getLeaseRate() const = 0;
 
  protected:
   // These are the definitions of the default values for optional parameters.
@@ -67,5 +68,6 @@ class ParameterInterfaceBase {
   static constexpr auto kCamerasWithoutHand = {"frontleft", "frontright", "left", "right", "back"};
   static constexpr auto kCamerasWithHand = {"frontleft", "frontright", "left", "right", "back", "hand"};
   static constexpr std::chrono::seconds kDefaultTimeSyncTimeout{5};
+  static constexpr double kDefaultLeaseRate{0.0};
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp

@@ -44,6 +44,7 @@ class RclcppParameterInterface : public ParameterInterfaceBase {
   [[nodiscard]] tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(
       const bool has_arm, const bool gripperless) const override;
   [[nodiscard]] std::chrono::seconds getTimeSyncTimeout() const override;
+  [[nodiscard]] std::optional<double> getLeaseRate() const override;
 
  private:
   std::shared_ptr<rclcpp::Node> node_;

spot_driver/include/spot_driver/lease/lease_manager.hpp

@@ -4,19 +4,25 @@
 
 #include <spot_driver/api/lease_client_interface.hpp>
 #include <spot_driver/interfaces/logger_interface_base.hpp>
+#include <spot_driver/interfaces/timer_interface_base.hpp>
 
+#include <spot_msgs/msg/lease_array.hpp>
 #include <spot_msgs/srv/acquire_lease.hpp>
 #include <spot_msgs/srv/return_lease.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <unordered_map>
 
 namespace spot_ros2::lease {
 
+using spot_msgs::msg::LeaseArray;
 using spot_msgs::srv::AcquireLease;
 using spot_msgs::srv::ReturnLease;
+using std_srvs::srv::Trigger;
 
 /**
  * Lease management for a ROS 2 graph.
@@ -39,6 +45,10 @@ class LeaseManager {
       virtual ~Bond() = default;
     };
 
+    virtual void createClaimLeasesService(
+        const std::string& service_name,
+        std::function<void(const std::shared_ptr<Trigger::Request>, std::shared_ptr<Trigger::Response>)> callback) = 0;
+
     virtual void createAcquireLeaseService(
         const std::string& service_name,
         std::function<void(const std::shared_ptr<AcquireLease::Request>, std::shared_ptr<AcquireLease::Response>)>
@@ -49,23 +59,46 @@ class LeaseManager {
         std::function<void(const std::shared_ptr<ReturnLease::Request>, std::shared_ptr<ReturnLease::Response>)>
             callback) = 0;
 
+    virtual void createReleaseLeasesService(
+        const std::string& service_name,
+        std::function<void(const std::shared_ptr<Trigger::Request>, std::shared_ptr<Trigger::Response>)> callback) = 0;
+
     virtual std::shared_ptr<Bond> createBond(const std::string& node_name, std::function<void()> break_callback) = 0;
 
+    virtual void publishLeaseArray(std::unique_ptr<LeaseArray> message) = 0;
+
     virtual ~MiddlewareHandle() = default;
   };
 
   /**
    * Instantiate the lease manager.
    * @param lease_client The ROS node.
-   * @param logger Logging interface.
-   * @param middleware_handle
+   * @param logger_interface Logging interface.
+   * @param timer_interface Timers interface.
+   * @param middleware_handle Middleware interfaces' provider.
+   * @param lease_check_rate Optional rate, in Hz, to check and report on robot leasing status.
    */
-  explicit LeaseManager(std::shared_ptr<LeaseClientInterface> lease_client, std::shared_ptr<LoggerInterfaceBase> logger,
-                        std::unique_ptr<MiddlewareHandle> middleware_handle);
+  explicit LeaseManager(std::shared_ptr<LeaseClientInterface> lease_client,
+                        std::unique_ptr<LoggerInterfaceBase> logger_interface,
+                        std::unique_ptr<TimerInterfaceBase> timer_interface,
+                        std::unique_ptr<MiddlewareHandle> middleware_handle, std::optional<double> lease_check_rate);
 
-  /** Initialize the service. */
+  /**
+   * Initialize the lease manager.
+   */
   void initialize();
 
+  /**
+   * Claim leases on request.
+   *
+   * This is achieved by acquiring the root resource lease.
+   * Typically used to implement the associated service.
+   *
+   * @param request The ROS request.
+   * @param response A ROS response to be filled.
+   */
+  void claimLeases(const std::shared_ptr<Trigger::Request> request, std::shared_ptr<Trigger::Response> response);
+
   /**
    * Acquire lease on request.
    *
@@ -88,7 +121,22 @@ class LeaseManager {
   void returnLease(const std::shared_ptr<ReturnLease::Request> request,
                    std::shared_ptr<ReturnLease::Response> response);
 
+  /**
+   * Release all acquired leases on request.
+   *
+   * Typically used to implement the associated service.
+   *
+   * @param request The ROS request.
+   * @param response A ROS response to be filled.
+   */
+  void releaseLeases(const std::shared_ptr<Trigger::Request> request, std::shared_ptr<Trigger::Response> response);
+
  private:
+  /**
+   * Check (and report) leasing status.
+   */
+  void checkLeasingStatus();
+
   /**
    * Handle lease retention failure.
    *
@@ -102,19 +150,25 @@ class LeaseManager {
   std::shared_ptr<LeaseClientInterface> lease_client_;
 
   // An interface to logging API.
-  std::shared_ptr<LoggerInterfaceBase> logger_;
+  std::unique_ptr<LoggerInterfaceBase> logger_interface_;
+
+  // An interface to the timers API.
+  std::unique_ptr<TimerInterfaceBase> timer_interface_;
 
   // An interface to the middleware provider.
   std::unique_ptr<MiddlewareHandle> middleware_handle_;
 
+  // Optional lease check rate.
+  std::optional<double> lease_check_rate_;
+
   struct ManagedSublease {
     bosdyn::client::Lease lease;                   // Actual SDK sublease.
     std::shared_ptr<MiddlewareHandle::Bond> bond;  // ROS layer keepalive.
   };
   // Storage for active subleases (and associated keepalives).
   std::unordered_map<std::string, ManagedSublease> subleases_;
 
-  // Synchronization primitive for sublease storage.
-  std::recursive_mutex subleases_mutex_;
+  // Synchronization primitive.
+  std::recursive_mutex mutex_;
 };
 }  // namespace spot_ros2::lease

spot_driver/include/spot_driver/lease/lease_manager_node.hpp

@@ -18,8 +18,10 @@ namespace spot_ros2::lease {
 class LeaseManagerNode {
  public:
   explicit LeaseManagerNode(std::shared_ptr<rclcpp::Node> node, std::unique_ptr<SpotApi> spot_api,
-                            std::shared_ptr<ParameterInterfaceBase> parameter_interface,
-                            std::shared_ptr<LoggerInterfaceBase> logger_interface);
+                            std::unique_ptr<LeaseManager::MiddlewareHandle> middleware_handle,
+                            std::unique_ptr<ParameterInterfaceBase> parameter_interface,
+                            std::unique_ptr<TimerInterfaceBase> timer_interface,
+                            std::unique_ptr<LoggerInterfaceBase> logger_interface);
 
   explicit LeaseManagerNode(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions{});
 
@@ -39,7 +41,9 @@ class LeaseManagerNode {
   std::unique_ptr<LeaseManager> internal_;
 
   void initialize(std::shared_ptr<rclcpp::Node> node, std::unique_ptr<SpotApi> spot_api,
-                  std::shared_ptr<ParameterInterfaceBase> parameter_interface,
-                  const std::shared_ptr<LoggerInterfaceBase> logger_interface);
+                  std::unique_ptr<LeaseManager::MiddlewareHandle> middleware_handle,
+                  std::unique_ptr<ParameterInterfaceBase> parameter_interface,
+                  std::unique_ptr<TimerInterfaceBase> timer_interface,
+                  std::unique_ptr<LoggerInterfaceBase> logger_interface);
 };
 }  // namespace spot_ros2::lease

spot_driver/include/spot_driver/lease/lease_middleware_handle.hpp

@@ -9,6 +9,7 @@
 #include <string>
 
 #include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
 #include <rclcpp/service.hpp>
 
 namespace spot_ros2::lease {
@@ -17,6 +18,11 @@ class LeaseMiddlewareHandle : public LeaseManager::MiddlewareHandle {
  public:
   explicit LeaseMiddlewareHandle(std::shared_ptr<rclcpp::Node> node);
 
+  void createClaimLeasesService(
+      const std::string& service_name,
+      std::function<void(const std::shared_ptr<Trigger::Request>, std::shared_ptr<Trigger::Response>)> callback)
+      override;
+
   void createAcquireLeaseService(
       const std::string& service_name,
       std::function<void(const std::shared_ptr<AcquireLease::Request>, std::shared_ptr<AcquireLease::Response>)>
@@ -27,12 +33,22 @@ class LeaseMiddlewareHandle : public LeaseManager::MiddlewareHandle {
       std::function<void(const std::shared_ptr<ReturnLease::Request>, std::shared_ptr<ReturnLease::Response>)> callback)
       override;
 
+  void createReleaseLeasesService(
+      const std::string& service_name,
+      std::function<void(const std::shared_ptr<Trigger::Request>, std::shared_ptr<Trigger::Response>)> callback)
+      override;
+
   std::shared_ptr<LeaseManager::MiddlewareHandle::Bond> createBond(const std::string& node_name,
                                                                    std::function<void()> break_callback) override;
 
+  void publishLeaseArray(std::unique_ptr<LeaseArray> message) override;
+
  private:
   std::shared_ptr<rclcpp::Node> node_;
+  std::shared_ptr<rclcpp::Publisher<LeaseArray>> lease_array_publisher_;
+  std::shared_ptr<rclcpp::Service<Trigger>> claim_leases_service_;
   std::shared_ptr<rclcpp::Service<AcquireLease>> acquire_lease_service_;
   std::shared_ptr<rclcpp::Service<ReturnLease>> return_lease_service_;
+  std::shared_ptr<rclcpp::Service<Trigger>> release_leases_service_;
 };
 }  // namespace spot_ros2::lease

spot_driver/src/api/default_lease_client.cpp

@@ -72,6 +72,18 @@ tl::expected<bool, std::string> DefaultLeaseClient::returnLease(const ::bosdyn::
   }
 }
 
+tl::expected<std::vector<::bosdyn::api::LeaseResource>, std::string> DefaultLeaseClient::listLeases() {
+  try {
+    auto result = lease_client_->ListLeases();
+    if (!result) {
+      return tl::make_unexpected(result.status.Chain("Could not list leases").message());
+    }
+    return std::vector(result.response.resources().begin(), result.response.resources().end());
+  } 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();
 }

spot_driver/src/interfaces/rclcpp_parameter_interface.cpp

@@ -30,6 +30,7 @@ constexpr auto kParameterPreferredOdomFrame = "preferred_odom_frame";
 constexpr auto kParameterTFRoot = "tf_root";
 constexpr auto kParameterNameGripperless = "gripperless";
 constexpr auto kParameterTimeSyncTimeout = "timesync_timeout";
+constexpr auto kParameterNameLeaseRate = "lease_rate";
 
 /**
  * @brief Get a rclcpp parameter. If the parameter has not been declared, declare it with the provided default value and
@@ -251,4 +252,10 @@ std::string RclcppParameterInterface::getSpotName() const {
     return "";
   }
 }
+
+std::optional<double> RclcppParameterInterface::getLeaseRate() const {
+  const double lease_rate = declareAndGetParameter<double>(node_, kParameterNameLeaseRate, kDefaultLeaseRate);
+  return lease_rate > 0.0 ? std::make_optional(lease_rate) : std::nullopt;
+}
+
 }  // namespace spot_ros2