Extension of Spot_Driver_CPP refactor (#196)

spot_driver_cpp/CMakeLists.txt

@@ -26,30 +26,41 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_ROS_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+add_library(spot_api
+  src/api/default_image_client_api.cpp
+  src/api/default_spot_api.cpp
+  src/api/default_time_sync_api.cpp)
+
+target_include_directories(spot_api PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+target_link_libraries(spot_api PUBLIC bosdyn::bosdyn_client_static)
+set_property(TARGET spot_api PROPERTY POSITION_INDEPENDENT_CODE ON)
+ament_target_dependencies(spot_api PUBLIC ${THIS_PACKAGE_INCLUDE_ROS_DEPENDS})
+
 # Base library containing the SpotImagePublisherNode class
 add_library(spot_image_publisher
   src/interfaces/rclcpp_logger_interface.cpp
   src/interfaces/rclcpp_parameter_interface.cpp
-  src/interfaces/rclcpp_publisher_interface.cpp
   src/interfaces/rclcpp_tf_interface.cpp
   src/interfaces/rclcpp_wall_timer_interface.cpp
-  src/api/default_image_api.cpp
-  src/api/default_robot_api.cpp
-  src/api/robot.cpp
-  src/spot_image_publisher.cpp
-  src/spot_image_publisher_node.cpp
-  src/spot_image_sources.cpp)
+  src/images/spot_image_publisher.cpp
+  src/images/images_middleware_handle.cpp
+  src/images/spot_image_publisher_node.cpp
+  src/images/spot_image_sources.cpp)
 target_include_directories(spot_image_publisher
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(spot_image_publisher PUBLIC bosdyn::bosdyn_client_static)
+target_link_libraries(spot_image_publisher PUBLIC spot_api)
 set_property(TARGET spot_image_publisher PROPERTY POSITION_INDEPENDENT_CODE ON)
 ament_target_dependencies(spot_image_publisher PUBLIC ${THIS_PACKAGE_INCLUDE_ROS_DEPENDS})
 
 # Create executable to allow running SpotImagePublisherNode directly as a ROS 2 node
-add_executable(spot_image_publisher_node src/spot_image_publisher_node_main.cpp)
+add_executable(spot_image_publisher_node src/images/spot_image_publisher_node_main.cpp)
 target_link_libraries(spot_image_publisher_node PUBLIC spot_image_publisher)
 target_include_directories(spot_image_publisher_node
   PUBLIC
@@ -58,7 +69,7 @@ target_include_directories(spot_image_publisher_node
 )
 
 # Register a composable node to allow loading SpotImagePublisherNode in a component container
-add_library(spot_image_publisher_component SHARED src/spot_image_publisher_component.cpp)
+add_library(spot_image_publisher_component SHARED src/images/spot_image_publisher_component.cpp)
 target_include_directories(spot_image_publisher_component
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -69,12 +80,13 @@ ament_target_dependencies(spot_image_publisher PUBLIC rclcpp_components)
 
 rclcpp_components_register_node(
   spot_image_publisher_component
-  PLUGIN "spot_ros2::SpotImagePublisherNode"
+  PLUGIN "spot_ros2::images::SpotImagePublisherNode"
   EXECUTABLE spot_image_publisher_node_component)
 
 # Install Libraries
 install(
   TARGETS
+    spot_api
     spot_image_publisher
     spot_image_publisher_component
   EXPORT ${PROJECT_NAME}Targets

spot_driver_cpp/include/spot_driver_cpp/api/default_image_client_api.hpp

@@ -0,0 +1,31 @@
+// Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <bosdyn/client/image/image_client.h>
+#include <bosdyn/client/sdk/client_sdk.h>
+#include <spot_driver_cpp/api/image_client_api.hpp>
+#include <spot_driver_cpp/api/time_sync_api.hpp>
+#include <spot_driver_cpp/images/spot_image_sources.hpp>
+
+#include <memory>
+#include <string>
+
+namespace spot_ros2 {
+/**
+ * @brief Implements ImageClientApi to use the Spot C++ Image Client.
+ */
+class DefaultImageClientApi : public ImageClientApi {
+ public:
+  DefaultImageClientApi(::bosdyn::client::ImageClient* image_client, std::shared_ptr<TimeSyncApi> time_sync_api,
+                        const std::string& robot_name);
+  ~DefaultImageClientApi() = default;
+
+  tl::expected<GetImagesResult, std::string> getImages(::bosdyn::api::GetImageRequest request) override;
+
+ private:
+  ::bosdyn::client::ImageClient* image_client_;
+  std::shared_ptr<TimeSyncApi> time_sync_api_;
+  std::string robot_name_;
+};
+}  // namespace spot_ros2

spot_driver_cpp/include/spot_driver_cpp/api/default_spot_api.hpp

@@ -0,0 +1,32 @@
+// Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <spot_driver_cpp/api/spot_api.hpp>
+
+#include <bosdyn/client/robot/robot.h>
+#include <bosdyn/client/sdk/client_sdk.h>
+#include <spot_driver_cpp/api/time_sync_api.hpp>
+
+#include <memory>
+#include <string>
+
+namespace spot_ros2 {
+
+class DefaultSpotApi : public SpotApi {
+ public:
+  explicit DefaultSpotApi(const std::string& sdk_client_name);
+
+  tl::expected<void, std::string> createRobot(const std::string& ip_address, const std::string& robot_name) override;
+  tl::expected<void, std::string> authenticate(const std::string& username, const std::string& password) override;
+  tl::expected<bool, std::string> hasArm() const override;
+  std::shared_ptr<ImageClientApi> image_client_api() const override;
+
+ private:
+  std::unique_ptr<::bosdyn::client::ClientSdk> client_sdk_;
+  std::unique_ptr<::bosdyn::client::Robot> robot_;
+  std::shared_ptr<ImageClientApi> image_client_api_;
+  std::shared_ptr<TimeSyncApi> time_sync_api_;
+  std::string robot_name_;
+};
+}  // namespace spot_ros2

spot_driver_cpp/include/spot_driver_cpp/api/default_image_api.hpp → spot_driver_cpp/include/spot_driver_cpp/api/default_time_sync_api.hpp

@@ -2,29 +2,26 @@
 
 #pragma once
 
-#include <bosdyn/client/image/image_client.h>
-#include <bosdyn/client/sdk/client_sdk.h>
-#include <bosdyn/client/time_sync/time_sync_client.h>
-
-#include <spot_driver_cpp/api/robot.hpp>
-#include <spot_driver_cpp/api/image_api.hpp>
+#include <bosdyn/client/time_sync/time_sync_helpers.h>
+#include <google/protobuf/duration.pb.h>
+#include <google/protobuf/timestamp.pb.h>
+#include <builtin_interfaces/msg/time.hpp>
+#include <spot_driver_cpp/api/time_sync_api.hpp>
+#include <tl_expected/expected.hpp>
 
 #include <memory>
 #include <string>
 
 namespace spot_ros2 {
-/**
- * @brief Implements SpotInterfaceBase to use the Spot C++ SDK.
- */
-class DefaultImageApi : public ImageApi {
+
+class DefaultTimeSyncApi : public TimeSyncApi {
  public:
-  DefaultImageApi(std::shared_ptr<Robot> robot);
+  explicit DefaultTimeSyncApi(std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread);
+  ~DefaultTimeSyncApi() = default;
 
-  tl::expected<GetImagesResult, std::string> getImages(::bosdyn::api::GetImageRequest request) override;
   tl::expected<builtin_interfaces::msg::Time, std::string> convertRobotTimeToLocalTime(
       const google::protobuf::Timestamp& robot_timestamp) override;
 
- private:
   /**
   * @brief Get the current clock skew from the Spot SDK's time sync endpoint.
   * @details The clock skew is the difference between Spot's internal system clock and the host PC's system clock.
@@ -39,11 +36,9 @@ class DefaultImageApi : public ImageApi {
   * @return If the Spot SDK's time sync thread was not initialized, return an error message.
   * @return If the Spot SDK's time sync endpoint fails to handle the clock skew request, return an error message.
   */
-  tl::expected<google::protobuf::Duration, std::string> getClockSkew();
+  tl::expected<google::protobuf::Duration, std::string> getClockSkew() override;
 
-  std::unique_ptr<::bosdyn::client::ClientSdk> client_sdk_;
-  std::shared_ptr<Robot> robot_;
+ private:
   std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread_;
-  std::unique_ptr<::bosdyn::client::ImageClient> image_client_;
 };
 }  // namespace spot_ros2

spot_driver_cpp/include/spot_driver_cpp/api/image_api.hpp → spot_driver_cpp/include/spot_driver_cpp/api/image_client_api.hpp

@@ -4,7 +4,7 @@
 
 #include <builtin_interfaces/msg/time.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
-#include <spot_driver_cpp/spot_image_sources.hpp>
+#include <spot_driver_cpp/images/spot_image_sources.hpp>
 #include <tl_expected/expected.hpp>
 
 #include <map>
@@ -18,13 +18,10 @@ struct GetImagesResult {
 };
 
 /**
- * @brief Defines an interface for a class to connect to and interact with Spot.
+ * @brief Defines an interface for a class to connect to and interact with Spot's Image client.
  */
-class ImageApi {
+class ImageClientApi {
  public:
-  virtual ~ImageApi() {}
   virtual tl::expected<GetImagesResult, std::string> getImages(::bosdyn::api::GetImageRequest request) = 0;
-  virtual tl::expected<builtin_interfaces::msg::Time, std::string> convertRobotTimeToLocalTime(
-      const google::protobuf::Timestamp& robot_timestamp) = 0;
 };
 }  // namespace spot_ros2

spot_driver_cpp/include/spot_driver_cpp/api/spot_api.hpp

@@ -0,0 +1,22 @@
+// Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <spot_driver_cpp/api/image_client_api.hpp>
+#include <tl_expected/expected.hpp>
+
+#include <memory>
+#include <string>
+
+namespace spot_ros2 {
+
+class SpotApi {
+ public:
+  virtual ~SpotApi() = default;
+
+  virtual tl::expected<void, std::string> createRobot(const std::string& ip_address, const std::string& robot_name) = 0;
+  virtual tl::expected<void, std::string> authenticate(const std::string& username, const std::string& password) = 0;
+  virtual tl::expected<bool, std::string> hasArm() const = 0;
+  virtual std::shared_ptr<ImageClientApi> image_client_api() const = 0;
+};
+}  // namespace spot_ros2

spot_driver_cpp/include/spot_driver_cpp/api/time_sync_api.hpp

@@ -0,0 +1,60 @@
+// Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <google/protobuf/duration.pb.h>
+#include <google/protobuf/timestamp.pb.h>
+#include <builtin_interfaces/msg/time.hpp>
+#include <tl_expected/expected.hpp>
+
+#include <memory>
+#include <string>
+
+namespace spot_ros2 {
+
+inline builtin_interfaces::msg::Time applyClockSkew(const google::protobuf::Timestamp& timestamp,
+                                                    const google::protobuf::Duration& clock_skew) {
+  int64_t seconds_unskewed = timestamp.seconds() - clock_skew.seconds();
+  int32_t nanos_unskewed = timestamp.nanos() - clock_skew.nanos();
+
+  // Carry over a second if needed
+  // Note: Since ROS Time messages store the nanoseconds component as an unsigned integer, we need to do this before
+  // converting to ROS Time.
+  if (nanos_unskewed < 0) {
+    nanos_unskewed += 1e9;
+    seconds_unskewed -= 1;
+  } else if (nanos_unskewed >= 1e9) {
+    nanos_unskewed -= 1e9;
+    seconds_unskewed += 1;
+  }
+
+  // If the timestamp contains a negative time, create an all-zero ROS Time.
+  if (seconds_unskewed < 0) {
+    return builtin_interfaces::build<builtin_interfaces::msg::Time>().sec(0).nanosec(0);
+  } else {
+    return builtin_interfaces::build<builtin_interfaces::msg::Time>().sec(seconds_unskewed).nanosec(nanos_unskewed);
+  }
+}
+
+class TimeSyncApi {
+ public:
+  virtual tl::expected<builtin_interfaces::msg::Time, std::string> convertRobotTimeToLocalTime(
+      const google::protobuf::Timestamp& robot_timestamp) = 0;
+
+  /**
+  * @brief Get the current clock skew from the Spot SDK's time sync endpoint.
+  * @details The clock skew is the difference between Spot's internal system clock and the host PC's system clock.
+  * For example, a clock skew of 1.0 seconds means that a timestamp from Spot's clock that is 10.0 seconds after epoch
+  * would correspond to a timestmap from the system clock that is 9.0 seconds after epoch.
+
+  * The Spot SDK documentation provides a more detailed explanation of how Spot's time sync works here:
+  * https://dev.bostondynamics.com/docs/concepts/base_services#time-sync
+  *
+  * @return If the clock skew was successfully calculated, return a Duration containing the difference between Spot's
+  * internal clock and the host's system clock.
+  * @return If the Spot SDK's time sync thread was not initialized, return an error message.
+  * @return If the Spot SDK's time sync endpoint fails to handle the clock skew request, return an error message.
+  */
+  virtual tl::expected<google::protobuf::Duration, std::string> getClockSkew() = 0;
+};
+}  // namespace spot_ros2