WIP

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

Author: mhidalgo-bdai

spot_driver/src/lease/lease_service.cpp

@@ -1 +1,52 @@
-#include "lease_service.h"
+// Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#include <spot_driver/lease/lease_service.hpp>
+
+#include <spot_driver/interfaces/rclcpp_logger_interface.hpp>
+#include <spot_driver/lease/lease_middleware_handle.hpp>
+
+namespace {
+constexpr auto kAcquireServiceName = "acquire_lease";
+constexpr auto kReleaseServiceName = "release_lease";
+}
+
+namespace spot_ros2::lease {
+LeaseService::LeaseService(std::shared_ptr<LeaseApi> kinematic_api,
+                                   std::shared_ptr<LoggerInterfaceBase> logger,
+                                   std::unique_ptr<MiddlewareHandle> middleware_handle)
+    : lease_api_{lease_api}, logger_{std::move(logger)}, middleware_handle_{std::move(middleware_handle)} {}
+
+void LeaseService::initialize() {
+  middleware_handle_->createAcquireService(kAcquireServiceName,
+                                           [this](const std::shared_ptr<AcquireLease::Request> request,
+                                                  std::shared_ptr<AcquireLease::Response> response) {
+                                               this->acquireLease(request, response);
+                                           });
+  middleware_handle_->createReleaseService(kReleaseServiceName,
+                                           [this](const std::shared_ptr<ReleaseLease::Request> request,
+                                                  std::shared_ptr<ReleaseLease::Response> response) {
+                                               this->releaseLease(request, response);
+                                           });
+}
+
+void LeaseService::acquireLease(const std::shared_ptr<AcquireLease::Request> request,
+                                std::shared_ptr<AcquireLease::Response> response) {
+  auto ros_request = request->request;
+
+  bosdyn::api::spot::InverseLeasesRequest proto_request;
+  convertToProto(ros_request, proto_request);
+
+  auto expected = kinematic_api_->getSolutions(proto_request);
+  if (!expected) {
+    logger_->logError(std::string{"Error querying the Inverse Leases service: "}.append(expected.error()));
+    response->response.status.value = bosdyn_spot_api_msgs::msg::InverseLeasesResponseStatus::STATUS_UNKNOWN;
+  } else {
+    convertToRos(expected.value(), response->response);
+  }
+}
+
+void LeaseService::releaseLease(const std::shared_ptr<ReleaseLease::Request> request,
+                                std::shared_ptr<ReleaseLease::Response> response) {
+
+}
+}  // namespace spot_ros2::lease