[PN-97] Implement Spot's Robot State Publisher in C++

spot_driver/CMakeLists.txt

@@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_ROS_DEPENDS
   tf2_ros
   tl_expected
   spot_msgs
+  nav_msgs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -28,33 +29,50 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_ROS_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+###
+# Spot API
+###
+
 add_library(spot_api
   src/api/default_kinematic_api.cpp
   src/api/default_image_client.cpp
   src/api/default_spot_api.cpp
+  src/api/default_state_client.cpp
   src/api/default_time_sync_api.cpp
   src/api/spot_image_sources.cpp
+  src/conversions/common_conversions.cpp
+  src/conversions/geometry.cpp
+  src/conversions/kinematic_conversions.cpp
+  src/conversions/robot_state.cpp
+  src/images/spot_image_publisher.cpp
+  src/images/images_middleware_handle.cpp
+  src/images/spot_image_publisher_node.cpp
   src/interfaces/rclcpp_logger_interface.cpp
+  src/interfaces/rclcpp_node_interface.cpp
   src/interfaces/rclcpp_parameter_interface.cpp
   src/interfaces/rclcpp_tf_interface.cpp
   src/interfaces/rclcpp_wall_timer_interface.cpp
-  src/conversions/common_conversions.cpp
-  src/conversions/kinematic_conversions.cpp
   src/kinematic/kinematic_node.cpp
   src/kinematic/kinematic_service.cpp
   src/kinematic/kinematic_middleware_handle.cpp
-  src/images/spot_image_publisher.cpp
-  src/images/images_middleware_handle.cpp
-  src/images/spot_image_publisher_node.cpp
+  src/robot_state/state_middleware_handle.cpp
+  src/robot_state/state_publisher.cpp
+  src/robot_state/state_publisher_node.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})
 
+###
+# Spot image publisher
+###
+
 # Create executable to allow running SpotImagePublisherNode directly as a ROS 2 node
 add_executable(spot_image_publisher_node src/images/spot_image_publisher_node_main.cpp)
 target_include_directories(spot_image_publisher_node
@@ -78,28 +96,60 @@ rclcpp_components_register_node(
   PLUGIN "spot_ros2::images::SpotImagePublisherNode"
   EXECUTABLE spot_image_publisher_node_component)
 
+###
+# Spot state publisher
+###
+
+# Create executable to allow running StatePublisherNode directly as a ROS 2 node
+add_executable(state_publisher_node src/robot_state/state_publisher_node_main.cpp)
+target_link_libraries(state_publisher_node PUBLIC spot_api)
+target_include_directories(state_publisher_node
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+# Register a composable node to allow loading StatePublisherNode in a component container
+add_library(state_publisher_component SHARED src/robot_state/state_publisher_component.cpp)
+target_include_directories(state_publisher_component
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(state_publisher_component PUBLIC spot_api)
+ament_target_dependencies(state_publisher_component PUBLIC rclcpp_components)
+
+rclcpp_components_register_node(
+  state_publisher_component
+  PLUGIN "spot_ros2::StatePublisherNode"
+  EXECUTABLE state_publisher_node_component)
+
+###
+# Spot IK
+###
+
 # Create executable to allow running KinematicNode directly as a ROS 2 node.
-add_executable(kinematic_node src/kinematic/kinematic_node_main.cpp)
-target_include_directories(kinematic_node
+add_executable(spot_inverse_kinematics_node src/kinematic/kinematic_node_main.cpp)
+target_include_directories(spot_inverse_kinematics_node
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(kinematic_node PUBLIC spot_api)
+target_link_libraries(spot_inverse_kinematics_node PUBLIC spot_api)
 
 # Register a composable node to allow loading KinematicNode in a component container
-add_library(kinematic_component SHARED src/kinematic/kinematic_component.cpp)
-target_include_directories(kinematic_component
+add_library(spot_inverse_kinematics_component SHARED src/kinematic/kinematic_component.cpp)
+target_include_directories(spot_inverse_kinematics_component
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(kinematic_component PUBLIC spot_api)
+target_link_libraries(spot_inverse_kinematics_component PUBLIC spot_api)
 
 rclcpp_components_register_node(
-  kinematic_component
+  spot_inverse_kinematics_component
   PLUGIN "spot_ros2::kinematic::KinematicNode"
-  EXECUTABLE kinematic_node_component)
+  EXECUTABLE spot_inverse_kinematics_node_component)
 
 ament_python_install_package(${PROJECT_NAME})
 install(
@@ -131,23 +181,24 @@ install(
   TARGETS
     spot_api
     spot_image_publisher_component
-    kinematic_component
+    spot_inverse_kinematics_component
+    state_publisher_component
   EXPORT ${PROJECT_NAME}Targets
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin
   INCLUDES DESTINATION include
 )
 
-# Install Binaries
+# Install Executables
 install(
   TARGETS 
     spot_image_publisher_node
     spot_image_publisher_node_component
-    kinematic_node
-    kinematic_node_component
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
+    spot_inverse_kinematics_node
+    spot_inverse_kinematics_node_component
+    state_publisher_node
+    state_publisher_node_component
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 

spot_driver/include/spot_driver/api/default_spot_api.hpp

@@ -6,6 +6,8 @@
 
 #include <bosdyn/client/robot/robot.h>
 #include <bosdyn/client/sdk/client_sdk.h>
+
+#include <spot_driver/api/state_client_interface.hpp>
 #include <spot_driver/api/time_sync_api.hpp>
 
 #include <memory>
@@ -22,12 +24,15 @@ class DefaultSpotApi : public SpotApi {
   tl::expected<bool, std::string> hasArm() const override;
   std::shared_ptr<KinematicApi> kinematicApi() const override;
   std::shared_ptr<ImageClientInterface> image_client_interface() const override;
+  std::shared_ptr<StateClientInterface> stateClientInterface() const override;
+  std::shared_ptr<TimeSyncApi> timeSyncInterface() const override;
 
  private:
   std::unique_ptr<::bosdyn::client::ClientSdk> client_sdk_;
   std::unique_ptr<::bosdyn::client::Robot> robot_;
   std::shared_ptr<KinematicApi> kinematicApi_;
   std::shared_ptr<ImageClientInterface> image_client_interface_;
+  std::shared_ptr<StateClientInterface> state_client_interface_;
   std::shared_ptr<TimeSyncApi> time_sync_api_;
   std::string robot_name_;
 };

spot_driver/include/spot_driver/api/default_state_client.hpp

@@ -0,0 +1,34 @@
+// Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <bosdyn/client/robot_state/robot_state_client.h>
+#include <spot_driver/api/state_client_interface.hpp>
+
+#include <string>
+
+namespace spot_ros2 {
+
+class DefaultStateClient final : public StateClientInterface {
+ public:
+  /**
+   * @brief constructor for DefaultStateClient.
+   *
+   * @param client A pointer to Spot's RobotStateClient. A DefaultStateClient SHOULD NOT delete this pointer since it
+   * does not take ownership.
+   */
+  explicit DefaultStateClient(::bosdyn::client::RobotStateClient* client);
+
+  /**
+   * @brief Retrieve Spot's most recent robot state data.
+   * @return Returns an expected which contains a RobotState message if the request was completed successfully or an
+   * error message if the request could not be completed.
+   */
+  tl::expected<bosdyn::api::RobotState, std::string> getRobotState() override;
+
+ private:
+  /** @brief A pointer to a RobotStateClient provided to this class during construction. */
+  ::bosdyn::client::RobotStateClient* client_;
+};
+
+}  // namespace spot_ros2

spot_driver/include/spot_driver/api/default_time_sync_api.hpp

@@ -17,7 +17,6 @@ namespace spot_ros2 {
 class DefaultTimeSyncApi : public TimeSyncApi {
  public:
   explicit DefaultTimeSyncApi(std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread);
-  ~DefaultTimeSyncApi() = default;
 
   tl::expected<builtin_interfaces::msg::Time, std::string> convertRobotTimeToLocalTime(
       const google::protobuf::Timestamp& robot_timestamp) override;

spot_driver/include/spot_driver/api/spot_api.hpp

@@ -3,6 +3,8 @@
 #pragma once
 
 #include <spot_driver/api/kinematic_api.hpp>
+#include <spot_driver/api/state_client_interface.hpp>
+#include <spot_driver/api/time_sync_api.hpp>
 #include <spot_driver/interfaces/image_client_interface.hpp>
 #include <tl_expected/expected.hpp>
 
@@ -20,5 +22,12 @@ class SpotApi {
   virtual tl::expected<bool, std::string> hasArm() const = 0;
   virtual std::shared_ptr<KinematicApi> kinematicApi() const = 0;
   virtual std::shared_ptr<ImageClientInterface> image_client_interface() const = 0;
+
+  /**
+   * @brief Get a StateClientInterface that communicates with Spot's robot state server.
+   * @return A shared_ptr to an instance of StateClientInterface which is owned by this object.
+   */
+  virtual std::shared_ptr<StateClientInterface> stateClientInterface() const = 0;
+  virtual std::shared_ptr<TimeSyncApi> timeSyncInterface() const = 0;
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/api/state_client_interface.hpp

@@ -0,0 +1,27 @@
+// Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <bosdyn/api/robot_state.pb.h>
+#include <tl_expected/expected.hpp>
+
+#include <string>
+
+namespace spot_ros2 {
+
+/**
+ * @brief Interface class to interact with Spot SDK's Robot State Client
+ */
+class StateClientInterface {
+ public:
+  virtual ~StateClientInterface() = default;
+
+  /**
+   * @brief Retrieve Spot's most recent robot state data.
+   * @return Returns an expected which contains a RobotState message if the request was completed successfully. If the
+   * request could not be completed, or if the response does not contain a RobotState message, return an error message
+   * describing the failure.
+   */
+  virtual tl::expected<bosdyn::api::RobotState, std::string> getRobotState() = 0;
+};
+}  // namespace spot_ros2

spot_driver/include/spot_driver/api/time_sync_api.hpp

@@ -38,6 +38,8 @@ inline builtin_interfaces::msg::Time applyClockSkew(const google::protobuf::Time
 
 class TimeSyncApi {
  public:
+  virtual ~TimeSyncApi() = default;
+
   virtual tl::expected<builtin_interfaces::msg::Time, std::string> convertRobotTimeToLocalTime(
       const google::protobuf::Timestamp& robot_timestamp) = 0;
 

spot_driver/include/spot_driver/conversions/geometry.hpp

@@ -0,0 +1,27 @@
+// Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved.
+
+#pragma once
+
+#include <bosdyn/api/geometry.pb.h>
+#include <bosdyn/math/frame_helpers.h>
+#include <builtin_interfaces/msg/time.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <string>
+
+namespace spot_ros2::conversions {
+/**
+ * @brief Convert SE3Pose messages to ROS TransformStamped messages
+ *
+ * @param transform Spot SE3Pose proto message
+ * @param parent_frame transform's parent frame
+ * @param child_frame transform's child frame
+ * @param tf_time transform's timestamp
+ */
+
+geometry_msgs::msg::TransformStamped toTransformStamped(const ::bosdyn::api::SE3Pose& transform,
+                                                        const std::string& parent_frame, const std::string& child_frame,
+                                                        const builtin_interfaces::msg::Time& tf_time);
+
+}  // namespace spot_ros2::conversions