Adds timesync timeout to params

spot_driver/include/spot_driver/api/default_time_sync_api.hpp

@@ -16,7 +16,8 @@ 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,
+                              int8_t timesync_timeout);
 
   /**
   * @brief Get the current clock skew from the Spot SDK's time sync endpoint.
@@ -36,5 +37,6 @@ class DefaultTimeSyncApi : public TimeSyncApi {
 
  private:
   std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread_;
+  int8_t timesync_timeout_;
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp

@@ -46,6 +46,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 int8_t getTimeSyncTimeout() const = 0;
 
  protected:
   // These are the definitions of the default values for optional parameters.
@@ -64,5 +65,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 int8_t kDefaultTimeSyncTimeout{5};
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp

@@ -42,6 +42,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]] int8_t getTimeSyncTimeout() const override;
 
  private:
   std::shared_ptr<rclcpp::Node> node_;

spot_driver/src/api/default_spot_api.cpp

@@ -8,10 +8,17 @@
 #include <spot_driver/api/default_spot_api.hpp>
 #include <spot_driver/api/default_state_client.hpp>
 #include <spot_driver/api/default_time_sync_api.hpp>
+#include <spot_driver/interfaces/rclcpp_parameter_interface.hpp>
 #include <tl_expected/expected.hpp>
 #include "spot_driver/api/default_world_object_client.hpp"
 #include "spot_driver/api/state_client_interface.hpp"
 
+namespace {
+constexpr auto kNodeNameSpace{"default_spot_api_namespace"};
+constexpr auto kNodeName{"default_spot_api"};
+
+}  // namespace
+
 namespace spot_ros2 {
 
 DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::optional<std::string>& certificate) {
@@ -55,6 +62,10 @@ tl::expected<void, std::string> DefaultSpotApi::authenticate(const std::string&
   }
   // Start time synchronization between the robot and the client system.
   // This must be done only after a successful authentication.
+  // Create a rclcpp node and parameter interface to retrieve ROS2 parameters, such as timesync_timeout.
+  const auto node_ = std::make_shared<rclcpp::Node>(kNodeName, kNodeNameSpace);
+  auto parameter_interface = std::make_unique<RclcppParameterInterface>(node_);
+  const auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
   const auto start_time_sync_response = robot_->StartTimeSync();
   if (!start_time_sync_response) {
     return tl::make_unexpected("Failed to start time synchronization.");
@@ -64,7 +75,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,
+                                       int8_t 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/interfaces/rclcpp_parameter_interface.cpp

@@ -28,6 +28,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 = "timeout";
 
 /**
  * @brief Get a rclcpp parameter. If the parameter has not been declared, declare it with the provided default value and
@@ -196,6 +197,10 @@ bool RclcppParameterInterface::getGripperless() const {
   return declareAndGetParameter<bool>(node_, kParameterNameGripperless, kDefaultGripperless);
 }
 
+int8_t RclcppParameterInterface::getTimeSyncTimeout() const {
+  return declareAndGetParameter<int8_t>(node_, kParameterTimeSyncTimeout, kDefaultTimeSyncTimeout);
+}
+
 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/test/include/spot_driver/fake/fake_parameter_interface.hpp

@@ -58,6 +58,8 @@ class FakeParameterInterface : public ParameterInterfaceBase {
     return getDefaultCamerasUsed(has_arm, gripperless);
   }
 
+  int8_t getTimeSyncTimeout() const override { return kDefaultTimeSyncTimeout; }
+
   static constexpr auto kExampleHostname{"192.168.0.10"};
   static constexpr auto kExampleUsername{"spot_user"};
   static constexpr auto kExamplePassword{"hunter2"};