diff --git a/spot_driver/include/spot_driver/api/default_spot_api.hpp b/spot_driver/include/spot_driver/api/default_spot_api.hpp index bd60b0a86..583427ef2 100644 --- a/spot_driver/include/spot_driver/api/default_spot_api.hpp +++ b/spot_driver/include/spot_driver/api/default_spot_api.hpp @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -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& certificate = std::nullopt); [[nodiscard]] tl::expected createRobot(const std::string& robot_name, @@ -43,5 +44,6 @@ class DefaultSpotApi : public SpotApi { std::shared_ptr time_sync_api_; std::shared_ptr world_object_client_interface_; std::string robot_name_; + const std::chrono::seconds timesync_timeout_; }; } // namespace spot_ros2 diff --git a/spot_driver/include/spot_driver/api/default_time_sync_api.hpp b/spot_driver/include/spot_driver/api/default_time_sync_api.hpp index 92155723d..1492d43cd 100644 --- a/spot_driver/include/spot_driver/api/default_time_sync_api.hpp +++ b/spot_driver/include/spot_driver/api/default_time_sync_api.hpp @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -16,7 +17,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, + 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 diff --git a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp index aa1751c6b..74cb7fc38 100644 --- a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp +++ b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp @@ -2,6 +2,7 @@ #pragma once +#include #include #include #include @@ -46,6 +47,7 @@ class ParameterInterfaceBase { virtual std::set getDefaultCamerasUsed(bool has_arm, bool gripperless) const = 0; virtual tl::expected, 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 diff --git a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp b/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp index 0a98a5905..45fb936aa 100644 --- a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp +++ b/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -42,6 +43,7 @@ class RclcppParameterInterface : public ParameterInterfaceBase { const bool gripperless) const override; [[nodiscard]] tl::expected, std::string> getCamerasUsed( const bool has_arm, const bool gripperless) const override; + [[nodiscard]] std::chrono::seconds getTimeSyncTimeout() const override; private: std::shared_ptr node_; diff --git a/spot_driver/src/api/default_spot_api.cpp b/spot_driver/src/api/default_spot_api.cpp index 173fc222f..c19cdbaae 100644 --- a/spot_driver/src/api/default_spot_api.cpp +++ b/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& certificate) { +DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::chrono::seconds timesync_timeout, + const std::optional& 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 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(get_time_sync_thread_response.response); + time_sync_api_ = std::make_shared(get_time_sync_thread_response.response, timesync_timeout_); // Image API. const auto image_client_result = robot_->EnsureServiceClient<::bosdyn::client::ImageClient>( diff --git a/spot_driver/src/api/default_time_sync_api.cpp b/spot_driver/src/api/default_time_sync_api.cpp index c52f35cce..8d7550abb 100644 --- a/spot_driver/src/api/default_time_sync_api.cpp +++ b/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 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()) { diff --git a/spot_driver/src/images/spot_image_publisher_node.cpp b/spot_driver/src/images/spot_image_publisher_node.cpp index 3d6fbbef2..8adade368 100644 --- a/spot_driver/src/images/spot_image_publisher_node.cpp +++ b/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(node); auto timer = std::make_unique(node); - auto spot_api = std::make_unique(kSDKClientName, parameters->getCertificate()); + const auto timesync_timeout = parameters->getTimeSyncTimeout(); + auto spot_api = std::make_unique(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)); diff --git a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index a2d6b0c2e..b4f7929af 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -2,6 +2,7 @@ #include +#include #include #include @@ -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(node_, kParameterNameGripperless, kDefaultGripperless); } +std::chrono::seconds RclcppParameterInterface::getTimeSyncTimeout() const { + int timeout_seconds = + declareAndGetParameter(node_, kParameterTimeSyncTimeout, + std::chrono::duration_cast(kDefaultTimeSyncTimeout).count()); + return std::chrono::seconds(timeout_seconds); +} + std::set RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm, const bool gripperless) const { const bool has_hand_camera = has_arm && (!gripperless); diff --git a/spot_driver/src/kinematic/kinematic_node.cpp b/spot_driver/src/kinematic/kinematic_node.cpp index 56f852881..713c27090 100644 --- a/spot_driver/src/kinematic/kinematic_node.cpp +++ b/spot_driver/src/kinematic/kinematic_node.cpp @@ -26,7 +26,9 @@ KinematicNode::KinematicNode(const rclcpp::NodeOptions& node_options) { auto node = std::make_shared("kinematic_service", node_options); auto parameter_interface = std::make_shared(node); auto logger_interface = std::make_shared(node->get_logger()); - auto spot_api = std::make_unique(kSDKClientName, parameter_interface->getCertificate()); + const auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); + auto spot_api = + std::make_unique(kSDKClientName, timesync_timeout, parameter_interface->getCertificate()); initialize(node, std::move(spot_api), parameter_interface, logger_interface); } diff --git a/spot_driver/src/object_sync/object_synchronizer_node.cpp b/spot_driver/src/object_sync/object_synchronizer_node.cpp index 3c9a67be1..2e4c8c536 100644 --- a/spot_driver/src/object_sync/object_synchronizer_node.cpp +++ b/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(node); auto clock_interface = std::make_unique(node->get_node_clock_interface()); - auto spot_api = std::make_unique(kDefaultSDKName, parameter_interface->getCertificate()); + const auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); + auto spot_api = + std::make_unique(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), diff --git a/spot_driver/src/robot_state/state_publisher_node.cpp b/spot_driver/src/robot_state/state_publisher_node.cpp index 6ecce6b70..2a61e0b2c 100644 --- a/spot_driver/src/robot_state/state_publisher_node.cpp +++ b/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(node); auto timer_interface = std::make_unique(node); - auto spot_api = std::make_unique(kDefaultSDKName, parameter_interface->getCertificate()); + const auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); + auto spot_api = + std::make_unique(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)); diff --git a/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp index 122ad8cb1..3bed1065f 100644 --- a/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp +++ b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp @@ -4,6 +4,7 @@ #include +#include #include #include #include @@ -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"}; diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 4ad4be3a2..c7b8a3eff 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -8,6 +8,7 @@ #include #include +#include #include 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) {