Added methods to the Robot wrapper.

spot_driver_cpp/include/spot_driver_cpp/api/robot.hpp

@@ -4,6 +4,8 @@
 
 #include <bosdyn/client/robot/robot.h>
 
+#include <tl_expected/expected.hpp>
+
 #include <memory>
 #include <string>
 
@@ -13,6 +15,10 @@ class Robot {
   Robot(std::unique_ptr<::bosdyn::client::Robot> robot, const std::string& robot_name);
 
   std::string name() const;
+
+  tl::expected<void, std::string> authenticate(const std::string& username, const std::string& password);
+  tl::expected<bool, std::string> hasArm() const;
+
   ::bosdyn::client::Robot& robot() const;
 
  private:

spot_driver_cpp/include/spot_driver_cpp/spot_image_publisher.hpp

@@ -5,6 +5,7 @@
 #include <rclcpp/node.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
+#include <spot_driver_cpp/api/robot.hpp>
 #include <spot_driver_cpp/interfaces/logger_interface_base.hpp>
 #include <spot_driver_cpp/interfaces/parameter_interface_base.hpp>
 #include <spot_driver_cpp/interfaces/publisher_interface_base.hpp>
@@ -54,7 +55,7 @@ class SpotImagePublisher {
    * @param parameter_interface  A unique_ptr to an instance of a class that implements ParameterInterfaceBase.
    * @param tf_interface  A unique_ptr to an instance of a class that implements TfInterfaceBase.
    */
-  SpotImagePublisher(std::unique_ptr<TimerInterfaceBase> timer_interface,
+  SpotImagePublisher(std::shared_ptr<Robot> robot, std::unique_ptr<TimerInterfaceBase> timer_interface,
                      std::unique_ptr<SpotInterfaceBase> spot_interface,
                      std::unique_ptr<PublisherInterfaceBase> publisher_interface,
                      std::unique_ptr<ParameterInterfaceBase> parameter_interface,
@@ -67,7 +68,7 @@ class SpotImagePublisher {
    *
    * @param node ROS 2 node to use when creating the interfaces.
    */
-  explicit SpotImagePublisher(const std::shared_ptr<rclcpp::Node>& node);
+  explicit SpotImagePublisher(std::shared_ptr<Robot> robot, const std::shared_ptr<rclcpp::Node>& node, );
 
   /**
    * @brief Connect to Spot and start publishing image data.
@@ -99,5 +100,7 @@ class SpotImagePublisher {
   std::unique_ptr<ParameterInterfaceBase> parameter_interface_;
   std::unique_ptr<TfInterfaceBase> tf_interface_;
   std::unique_ptr<LoggerInterfaceBase> logger_interface_;
+
+  std::shared_ptr<Robot> robot_;
 };
 }  // namespace spot_ros2

spot_driver_cpp/include/spot_driver_cpp/spot_image_publisher_node.hpp

@@ -4,6 +4,7 @@
 
 #include <rclcpp/node_interfaces/node_base_interface.hpp>
 #include <rclcpp/node_options.hpp>
+#include <spot_driver_cpp/api/robot.hpp>
 #include <spot_driver_cpp/spot_image_publisher.hpp>
 
 #include <memory>
@@ -27,6 +28,6 @@ class SpotImagePublisherNode {
 
  private:
   std::shared_ptr<rclcpp::Node> node_;
-  SpotImagePublisher internal_;
+  std::unique_ptr<SpotImagePublisher> internal_;
 };
 }  // namespace spot_ros2

spot_driver_cpp/src/api/robot.cpp

@@ -2,6 +2,8 @@
 
 #include <spot_driver_cpp/api/robot.hpp>
 
+#include <bosdyn/client/gripper_camera_param/gripper_camera_param_client.h>
+
 namespace spot_ros2 {
 Robot::Robot(std::unique_ptr<::bosdyn::client::Robot> robot, const std::string& robot_name)
     : robot_{std::move(robot)}, robot_name_{robot_name} {}
@@ -10,6 +12,28 @@ std::string Robot::name() const {
   return robot_name_;
 }
 
+tl::expected<bool, std::string> Robot::hasArm() const {
+  // Determine if Spot has an arm by checking if the client for gripper camera parameters exists, since Spots without
+  // arms do not have this client.
+  const auto list_result = robot_->ListServices();
+  if (!list_result.status) {
+    return tl::make_unexpected("Failed to retrieve list of Spot services.");
+  }
+
+  const auto& services = list_result.response;
+
+  return std::find_if(services.cbegin(), services.cend(), [](const ::bosdyn::api::ServiceEntry& entry) {
+           return entry.name() == ::bosdyn::client::GripperCameraParamClient::GetDefaultServiceName();
+         }) != services.cend();
+}
+
+tl::expected<void, std::string> Robot::authenticate(const std::string& username, const std::string& password) {
+  const auto authenticate_result = robot_->Authenticate(username, password);
+  if (!authenticate_result) {
+    return tl::make_unexpected("Authentication with provided username and password did not succeed.");
+  }
+}
+
 ::bosdyn::client::Robot& Robot::robot() const {
   return *robot_;
 }

spot_driver_cpp/src/spot_image_publisher.cpp

@@ -59,7 +59,8 @@ ::bosdyn::api::GetImageRequest createImageRequest(const std::set<ImageSource>& s
   return request_message;
 }
 
-SpotImagePublisher::SpotImagePublisher(std::unique_ptr<TimerInterfaceBase> timer_interface,
+SpotImagePublisher::SpotImagePublisher(std::shared_ptr<Robot> robot,
+                                       std::unique_ptr<TimerInterfaceBase> timer_interface,
                                        std::unique_ptr<SpotInterfaceBase> spot_interface,
                                        std::unique_ptr<PublisherInterfaceBase> publisher_interface,
                                        std::unique_ptr<ParameterInterfaceBase> parameter_interface,
@@ -70,41 +71,24 @@ SpotImagePublisher::SpotImagePublisher(std::unique_ptr<TimerInterfaceBase> timer
       publisher_interface_{std::move(publisher_interface)},
       parameter_interface_{std::move(parameter_interface)},
       tf_interface_{std::move(tf_interface)},
-      logger_interface_{std::move(logger_interface)} {}
+      logger_interface_{std::move(logger_interface)},
+      robot_{robot} {}
 
-SpotImagePublisher::SpotImagePublisher(const std::shared_ptr<rclcpp::Node>& node)
-    : SpotImagePublisher(std::make_unique<RclcppWallTimerInterface>(node), std::make_unique<SpotInterface>(),
+SpotImagePublisher::SpotImagePublisher(std::shared_ptr<Robot> robot, const std::shared_ptr<rclcpp::Node>& node)
+    : SpotImagePublisher(robot, std::make_unique<RclcppWallTimerInterface>(node), std::make_unique<SpotInterface>(),
                          std::make_unique<RclcppPublisherInterface>(node),
                          std::make_unique<RclcppParameterInterface>(node), std::make_unique<RclcppTfInterface>(node),
                          std::make_unique<RclcppLoggerInterface>(node->get_logger())) {}
 
 bool SpotImagePublisher::initialize() {
   // These parameters all fall back to default values if the user did not set them at runtime
-  const auto address = parameter_interface_->getAddress();
-  const auto username = parameter_interface_->getUsername();
-  const auto password = parameter_interface_->getPassword();
   const auto rgb_image_quality = parameter_interface_->getRGBImageQuality();
   const auto publish_rgb_images = parameter_interface_->getPublishRGBImages();
   const auto publish_depth_images = parameter_interface_->getPublishDepthImages();
   const auto publish_depth_registered_images = parameter_interface_->getPublishDepthRegisteredImages();
   const auto has_rgb_cameras = parameter_interface_->getHasRGBCameras();
-  const auto spot_name = parameter_interface_->getSpotName();
 
-  // Initialize the SDK client, and connect to the robot
-  const auto create_robot_result = spot_interface_->createRobot(address, spot_name);
-  if (!create_robot_result) {
-    logger_interface_->logError(
-        std::string{"Failed to create interface to robot: "}.append(create_robot_result.error()));
-    return false;
-  }
-
-  const auto authenticate_result = spot_interface_->authenticate(username, password);
-  if (!authenticate_result) {
-    logger_interface_->logError(std::string{"Failed to authenticate with robot: "}.append(authenticate_result.error()));
-    return false;
-  }
-
-  const auto has_arm_result = spot_interface_->hasArm();
+  const auto has_arm_result = robot_->hasArm();
   if (!has_arm_result) {
     logger_interface_->logError(
         std::string{"Failed to determine if Spot is equipped with an arm: "}.append(has_arm_result.error()));

spot_driver_cpp/src/spot_image_publisher_node.cpp

@@ -1,11 +1,40 @@
 // Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved.
 
+#include <spot_driver_cpp/api/default_robot_api.hpp>
+#include <spot_driver_cpp/interfaces/rclcpp_logger_interface.hpp>
+#include <spot_driver_cpp/interfaces/rclcpp_parameter_interface.hpp>
 #include <spot_driver_cpp/spot_image_publisher_node.hpp>
 
 namespace spot_ros2 {
-SpotImagePublisherNode::SpotImagePublisherNode(const rclcpp::NodeOptions& node_options)
-    : node_{std::make_shared<rclcpp::Node>("image_publisher", node_options)}, internal_{SpotImagePublisher{node_}} {
-  internal_.initialize();
+SpotImagePublisherNode::SpotImagePublisherNode(const rclcpp::NodeOptions& node_options) {
+  node_ = std::make_shared<rclcpp::Node>("image_publisher", node_options);
+
+  auto logger = std::make_unique<RclcppLoggerInterface>(node_->get_logger());
+
+  auto parameters = std::make_unique<RclcppParameterInterface>(node_);
+  const auto address = parameters->getAddress();
+  const auto robot_name = parameters->getSpotName();
+  const auto username = parameters->getUsername();
+  const auto password = parameters->getPassword();
+
+  // Get a robot.
+  auto robot_api = std::make_unique<DefaultRobotApi>();
+  auto expected_robot = robot_api->createRobot(address, robot_name);
+  if (!expected_robot) {
+    logger->logError(std::string{"Failed to create interface to robot: "}.append(expected_robot.error()));
+    exit(1);
+  }
+
+  // Authenticate.
+  std::shared_ptr<Robot> robot{std::move(expected_robot.value())};
+  auto auth = robot->authenticate(username, password);
+  if (!auth) {
+    logger->logError("Authentication with provided username and password did not succeed.");
+    exit(1);
+  }
+
+  internal_ = std::make_unique<SpotImagePublisher>(robot, node_);
+  internal_->initialize();
 }
 
 std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> SpotImagePublisherNode::get_node_base_interface() {