Adds timesync timeout to params (#552)

Co-authored-by: Brian Jin <[email protected]>

spot_driver/include/spot_driver/api/default_spot_api.hpp

@@ -11,6 +11,7 @@
 #include <spot_driver/api/time_sync_api.hpp>
 #include <spot_driver/api/world_object_client_interface.hpp>
 
+#include <chrono>
 #include <memory>
 #include <optional>
 #include <string>
@@ -19,7 +20,7 @@ namespace spot_ros2 {
 
 class DefaultSpotApi : public SpotApi {
  public:
-  explicit DefaultSpotApi(const std::string& sdk_client_name,
+  explicit DefaultSpotApi(const std::string& sdk_client_name, const std::chrono::seconds timesync_timeout,
                           const std::optional<std::string>& certificate = std::nullopt);
 
   [[nodiscard]] tl::expected<void, std::string> createRobot(const std::string& robot_name,
@@ -43,5 +44,6 @@ class DefaultSpotApi : public SpotApi {
   std::shared_ptr<TimeSyncApi> time_sync_api_;
   std::shared_ptr<WorldObjectClientInterface> world_object_client_interface_;
   std::string robot_name_;
+  const std::chrono::seconds timesync_timeout_;
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/api/default_time_sync_api.hpp

@@ -9,14 +9,16 @@
 #include <spot_driver/api/time_sync_api.hpp>
 #include <tl_expected/expected.hpp>
 
+#include <chrono>
 #include <memory>
 #include <string>
 
 namespace spot_ros2 {
 
 class DefaultTimeSyncApi : public TimeSyncApi {
  public:
-  explicit DefaultTimeSyncApi(std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread);
+  explicit DefaultTimeSyncApi(std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread,
+                              const std::chrono::seconds timesync_timeout);
 
   /**
   * @brief Get the current clock skew from the Spot SDK's time sync endpoint.
@@ -36,5 +38,6 @@ class DefaultTimeSyncApi : public TimeSyncApi {
 
  private:
   std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread_;
+  const std::chrono::seconds timesync_timeout_;
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <chrono>
 #include <optional>
 #include <set>
 #include <string>
@@ -46,6 +47,7 @@ class ParameterInterfaceBase {
   virtual std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(bool has_arm, bool gripperless) const = 0;
   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;
 
  protected:
   // These are the definitions of the default values for optional parameters.
@@ -64,5 +66,6 @@ class ParameterInterfaceBase {
   static constexpr bool kDefaultGripperless{false};
   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};
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp

@@ -5,6 +5,7 @@
 #include <rclcpp/node.hpp>
 #include <spot_driver/interfaces/parameter_interface_base.hpp>
 
+#include <chrono>
 #include <memory>
 #include <optional>
 #include <set>
@@ -42,6 +43,7 @@ class RclcppParameterInterface : public ParameterInterfaceBase {
                                                                       const bool gripperless) const override;
   [[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;
 
  private:
   std::shared_ptr<rclcpp::Node> node_;

spot_driver/src/api/default_spot_api.cpp

@@ -14,7 +14,9 @@
 
 namespace spot_ros2 {
 
-DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::optional<std::string>& certificate) {
+DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::chrono::seconds timesync_timeout,
+                               const std::optional<std::string>& certificate)
+    : timesync_timeout_(timesync_timeout) {
   if (certificate.has_value()) {
     client_sdk_ = std::make_unique<::bosdyn::client::ClientSdk>();
     client_sdk_->SetClientName(sdk_client_name);
@@ -64,7 +66,7 @@ tl::expected<void, std::string> DefaultSpotApi::authenticate(const std::string&
   if (!get_time_sync_thread_response) {
     return tl::make_unexpected("Failed to get the time synchronization thread.");
   }
-  time_sync_api_ = std::make_shared<DefaultTimeSyncApi>(get_time_sync_thread_response.response);
+  time_sync_api_ = std::make_shared<DefaultTimeSyncApi>(get_time_sync_thread_response.response, timesync_timeout_);
 
   // Image API.
   const auto image_client_result = robot_->EnsureServiceClient<::bosdyn::client::ImageClient>(

spot_driver/src/api/default_time_sync_api.cpp

@@ -6,14 +6,15 @@
 
 namespace spot_ros2 {
 
-DefaultTimeSyncApi::DefaultTimeSyncApi(std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread)
-    : time_sync_thread_{time_sync_thread} {}
+DefaultTimeSyncApi::DefaultTimeSyncApi(std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread,
+                                       std::chrono::seconds timesync_timeout)
+    : time_sync_thread_{time_sync_thread}, timesync_timeout_{timesync_timeout} {}
 
 tl::expected<google::protobuf::Duration, std::string> DefaultTimeSyncApi::getClockSkew() {
   if (!time_sync_thread_) {
     return tl::make_unexpected("Time sync thread was not initialized.");
   }
-  if (!time_sync_thread_->WaitForSync(std::chrono::seconds(5))) {
+  if (!time_sync_thread_->WaitForSync(std::chrono::seconds(timesync_timeout_))) {
     return tl::make_unexpected("Failed to establish time sync before timing out.");
   }
   if (!time_sync_thread_->HasEstablishedTimeSync()) {

spot_driver/src/images/spot_image_publisher_node.cpp

@@ -39,7 +39,8 @@ SpotImagePublisherNode::SpotImagePublisherNode(const rclcpp::NodeOptions& node_o
   auto tf_broadcaster = std::make_unique<RclcppTfBroadcasterInterface>(node);
   auto timer = std::make_unique<RclcppWallTimerInterface>(node);
 
-  auto spot_api = std::make_unique<DefaultSpotApi>(kSDKClientName, parameters->getCertificate());
+  const auto timesync_timeout = parameters->getTimeSyncTimeout();
+  auto spot_api = std::make_unique<DefaultSpotApi>(kSDKClientName, timesync_timeout, parameters->getCertificate());
 
   initialize(std::move(spot_api), std::move(mw_handle), std::move(parameters), std::move(logger),
              std::move(tf_broadcaster), std::move(timer));

spot_driver/src/interfaces/rclcpp_parameter_interface.cpp

@@ -2,6 +2,7 @@
 
 #include <spot_driver/interfaces/rclcpp_parameter_interface.hpp>
 
+#include <chrono>
 #include <cstdlib>
 #include <vector>
 
@@ -28,6 +29,7 @@ constexpr auto kParameterNamePublishDepthRegisteredImages = "publish_depth_regis
 constexpr auto kParameterPreferredOdomFrame = "preferred_odom_frame";
 constexpr auto kParameterTFRoot = "tf_root";
 constexpr auto kParameterNameGripperless = "gripperless";
+constexpr auto kParameterTimeSyncTimeout = "timesync_timeout";
 
 /**
  * @brief Get a rclcpp parameter. If the parameter has not been declared, declare it with the provided default value and
@@ -196,6 +198,13 @@ bool RclcppParameterInterface::getGripperless() const {
   return declareAndGetParameter<bool>(node_, kParameterNameGripperless, kDefaultGripperless);
 }
 
+std::chrono::seconds RclcppParameterInterface::getTimeSyncTimeout() const {
+  int timeout_seconds =
+      declareAndGetParameter<int>(node_, kParameterTimeSyncTimeout,
+                                  std::chrono::duration_cast<std::chrono::seconds>(kDefaultTimeSyncTimeout).count());
+  return std::chrono::seconds(timeout_seconds);
+}
+
 std::set<spot_ros2::SpotCamera> RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm,
                                                                                 const bool gripperless) const {
   const bool has_hand_camera = has_arm && (!gripperless);

spot_driver/src/kinematic/kinematic_node.cpp

@@ -26,7 +26,9 @@ KinematicNode::KinematicNode(const rclcpp::NodeOptions& node_options) {
   auto node = std::make_shared<rclcpp::Node>("kinematic_service", node_options);
   auto parameter_interface = std::make_shared<RclcppParameterInterface>(node);
   auto logger_interface = std::make_shared<RclcppLoggerInterface>(node->get_logger());
-  auto spot_api = std::make_unique<DefaultSpotApi>(kSDKClientName, parameter_interface->getCertificate());
+  const auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
+  auto spot_api =
+      std::make_unique<DefaultSpotApi>(kSDKClientName, timesync_timeout, parameter_interface->getCertificate());
   initialize(node, std::move(spot_api), parameter_interface, logger_interface);
 }
 

spot_driver/src/object_sync/object_synchronizer_node.cpp

@@ -48,7 +48,9 @@ ObjectSynchronizerNode::ObjectSynchronizerNode(const rclcpp::NodeOptions& node_o
   auto tf_broadcaster_timer = std::make_unique<RclcppWallTimerInterface>(node);
   auto clock_interface = std::make_unique<RclcppClockInterface>(node->get_node_clock_interface());
 
-  auto spot_api = std::make_unique<DefaultSpotApi>(kDefaultSDKName, parameter_interface->getCertificate());
+  const auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
+  auto spot_api =
+      std::make_unique<DefaultSpotApi>(kDefaultSDKName, timesync_timeout, parameter_interface->getCertificate());
 
   initialize(std::move(spot_api), std::move(parameter_interface), std::move(logger_interface),
              std::move(tf_broadcaster_interface), std::move(tf_listener_interface),

spot_driver/src/robot_state/state_publisher_node.cpp

@@ -42,7 +42,9 @@ StatePublisherNode::StatePublisherNode(const rclcpp::NodeOptions& node_options)
   auto tf_broadcaster_interface = std::make_unique<RclcppTfBroadcasterInterface>(node);
   auto timer_interface = std::make_unique<RclcppWallTimerInterface>(node);
 
-  auto spot_api = std::make_unique<DefaultSpotApi>(kDefaultSDKName, parameter_interface->getCertificate());
+  const auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
+  auto spot_api =
+      std::make_unique<DefaultSpotApi>(kDefaultSDKName, timesync_timeout, parameter_interface->getCertificate());
 
   initialize(std::move(spot_api), std::move(mw_handle), std::move(parameter_interface), std::move(logger_interface),
              std::move(tf_broadcaster_interface), std::move(timer_interface));

spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp

@@ -4,6 +4,7 @@
 
 #include <spot_driver/interfaces/parameter_interface_base.hpp>
 
+#include <chrono>
 #include <optional>
 #include <set>
 #include <string>
@@ -58,6 +59,8 @@ class FakeParameterInterface : public ParameterInterfaceBase {
     return getDefaultCamerasUsed(has_arm, gripperless);
   }
 
+  std::chrono::seconds getTimeSyncTimeout() const override { return kDefaultTimeSyncTimeout; }
+
   static constexpr auto kExampleHostname{"192.168.0.10"};
   static constexpr auto kExampleUsername{"spot_user"};
   static constexpr auto kExamplePassword{"hunter2"};

spot_driver/test/src/test_parameter_interface.cpp

@@ -8,6 +8,7 @@
 #include <spot_driver/interfaces/rclcpp_parameter_interface.hpp>
 #include <spot_driver/rclcpp_test.hpp>
 
+#include <chrono>
 #include <memory>
 
 namespace {
@@ -197,6 +198,8 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigFromParameters) {
   node_->declare_parameter("publish_depth_registered", publish_depth_registered_images_parameter);
   constexpr auto tf_root_parameter = "body";
   node_->declare_parameter("tf_root", tf_root_parameter);
+  constexpr auto timesync_timeout_parameter = 42;
+  node_->declare_parameter("timesync_timeout", timesync_timeout_parameter);
 
   // GIVEN we create a RclcppParameterInterface using the node
   RclcppParameterInterface parameter_interface{node_};
@@ -216,6 +219,7 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigFromParameters) {
   EXPECT_THAT(parameter_interface.getPublishDepthImages(), Eq(publish_depth_images_parameter));
   EXPECT_THAT(parameter_interface.getPublishDepthRegisteredImages(), Eq(publish_depth_registered_images_parameter));
   EXPECT_THAT(parameter_interface.getTFRoot(), Eq(tf_root_parameter));
+  EXPECT_THAT(parameter_interface.getTimeSyncTimeout(), Eq(std::chrono::seconds(timesync_timeout_parameter)));
 }
 
 TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigEnvVarsOverruleParameters) {
@@ -276,6 +280,7 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetConfigDefaults) {
   EXPECT_THAT(parameter_interface.getPublishDepthImages(), IsTrue());
   EXPECT_THAT(parameter_interface.getPublishDepthRegisteredImages(), IsTrue());
   EXPECT_THAT(parameter_interface.getTFRoot(), StrEq("odom"));
+  EXPECT_THAT(parameter_interface.getTimeSyncTimeout(), Eq(std::chrono::seconds(5)));
 }
 
 TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithArm) {