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 ac6d2bfa..7a72143c 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 @@ -20,8 +21,8 @@ namespace spot_ros2 { class DefaultSpotApi : public SpotApi { public: explicit DefaultSpotApi(const std::string& sdk_client_name, - const std::optional& certificate = std::nullopt, - const std::optional timesync_timeout = std::nullopt); + const std::chrono::seconds& timesync_timeout, + const std::optional& certificate = std::nullopt); [[nodiscard]] tl::expected createRobot(const std::string& robot_name, const std::string& ip_address, @@ -44,6 +45,7 @@ class DefaultSpotApi : public SpotApi { std::shared_ptr time_sync_api_; std::shared_ptr world_object_client_interface_; std::string robot_name_; - int8_t timesync_timeout_; + 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 a61bec7e..1492d43c 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 @@ -17,7 +18,7 @@ namespace spot_ros2 { class DefaultTimeSyncApi : public TimeSyncApi { public: explicit DefaultTimeSyncApi(std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread, - int8_t timesync_timeout); + const std::chrono::seconds timesync_timeout); /** * @brief Get the current clock skew from the Spot SDK's time sync endpoint. @@ -37,6 +38,6 @@ class DefaultTimeSyncApi : public TimeSyncApi { private: std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread_; - int8_t timesync_timeout_; + 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 97982a6b..ad9d7e45 100644 --- a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp +++ b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -46,7 +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 int8_t getTimeSyncTimeout() const = 0; + virtual std::chrono::seconds getTimeSyncTimeout() const = 0; protected: // These are the definitions of the default values for optional parameters. @@ -65,6 +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 int8_t kDefaultTimeSyncTimeout{5}; + 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 7412b5e5..3a9ad02d 100644 --- a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp +++ b/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp @@ -10,6 +10,7 @@ #include #include #include +#include namespace spot_ros2 { /** @@ -42,7 +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]] int8_t getTimeSyncTimeout() 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 aa59f3f8..cc46a639 100644 --- a/spot_driver/src/api/default_spot_api.cpp +++ b/spot_driver/src/api/default_spot_api.cpp @@ -14,8 +14,10 @@ namespace spot_ros2 { -DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::optional& certificate, - const std::optional timesync_timeout) { +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); @@ -26,9 +28,6 @@ DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::op } else { client_sdk_ = ::bosdyn::client::CreateStandardSDK(sdk_client_name); } - if (timesync_timeout.has_value()) { - timesync_timeout_ = timesync_timeout.value(); - } } tl::expected DefaultSpotApi::createRobot(const std::string& robot_name, diff --git a/spot_driver/src/api/default_time_sync_api.cpp b/spot_driver/src/api/default_time_sync_api.cpp index 4abe00f4..8d7550ab 100644 --- a/spot_driver/src/api/default_time_sync_api.cpp +++ b/spot_driver/src/api/default_time_sync_api.cpp @@ -7,7 +7,7 @@ namespace spot_ros2 { DefaultTimeSyncApi::DefaultTimeSyncApi(std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread, - int8_t timesync_timeout) + std::chrono::seconds timesync_timeout) : time_sync_thread_{time_sync_thread}, timesync_timeout_{timesync_timeout} {} tl::expected DefaultTimeSyncApi::getClockSkew() { diff --git a/spot_driver/src/images/spot_image_publisher_node.cpp b/spot_driver/src/images/spot_image_publisher_node.cpp index 4ded679f..8adade36 100644 --- a/spot_driver/src/images/spot_image_publisher_node.cpp +++ b/spot_driver/src/images/spot_image_publisher_node.cpp @@ -39,8 +39,8 @@ SpotImagePublisherNode::SpotImagePublisherNode(const rclcpp::NodeOptions& node_o auto tf_broadcaster = std::make_unique(node); auto timer = std::make_unique(node); - auto timesync_timeout = parameters->getTimeSyncTimeout(); - auto spot_api = std::make_unique(kSDKClientName, parameters->getCertificate(), timesync_timeout); + 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 3cb84bac..0bf7bc88 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -4,6 +4,7 @@ #include #include +#include namespace { constexpr auto kEnvVarNameHostname = "SPOT_IP"; @@ -197,8 +198,9 @@ bool RclcppParameterInterface::getGripperless() const { return declareAndGetParameter(node_, kParameterNameGripperless, kDefaultGripperless); } -int8_t RclcppParameterInterface::getTimeSyncTimeout() const { - return declareAndGetParameter(node_, kParameterTimeSyncTimeout, kDefaultTimeSyncTimeout); +std::chrono::seconds RclcppParameterInterface::getTimeSyncTimeout() const { + int timeout_seconds = declareAndGetParameter(node_, kParameterTimeSyncTimeout, kDefaultTimeSyncTimeout.count()); + return std::chrono::seconds(timeout_seconds); } std::set RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm, diff --git a/spot_driver/src/kinematic/kinematic_node.cpp b/spot_driver/src/kinematic/kinematic_node.cpp index 843cff0d..713c2709 100644 --- a/spot_driver/src/kinematic/kinematic_node.cpp +++ b/spot_driver/src/kinematic/kinematic_node.cpp @@ -26,9 +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 timesync_timeout = parameter_interface->getTimeSyncTimeout(); + const auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); auto spot_api = - std::make_unique(kSDKClientName, parameter_interface->getCertificate(), timesync_timeout); + 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 bf6b2f0c..2e4c8c53 100644 --- a/spot_driver/src/object_sync/object_synchronizer_node.cpp +++ b/spot_driver/src/object_sync/object_synchronizer_node.cpp @@ -48,9 +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 timesync_timeout = parameter_interface->getTimeSyncTimeout(); + const auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); auto spot_api = - std::make_unique(kDefaultSDKName, parameter_interface->getCertificate(), timesync_timeout); + 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 21eb2d84..2a61e0b2 100644 --- a/spot_driver/src/robot_state/state_publisher_node.cpp +++ b/spot_driver/src/robot_state/state_publisher_node.cpp @@ -42,9 +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 timesync_timeout = parameter_interface->getTimeSyncTimeout(); + const auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); auto spot_api = - std::make_unique(kDefaultSDKName, parameter_interface->getCertificate(), timesync_timeout); + 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 94aa74b5..615a9ca8 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 @@ -8,6 +8,7 @@ #include #include #include +#include namespace spot_ros2::test { class FakeParameterInterface : public ParameterInterfaceBase { @@ -58,7 +59,7 @@ class FakeParameterInterface : public ParameterInterfaceBase { return getDefaultCamerasUsed(has_arm, gripperless); } - int8_t getTimeSyncTimeout() const override { return kDefaultTimeSyncTimeout; } + std::chrono::seconds getTimeSyncTimeout() const override { return kDefaultTimeSyncTimeout; } static constexpr auto kExampleHostname{"192.168.0.10"}; static constexpr auto kExampleUsername{"spot_user"}; diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 09ff3df2..ac4c0c17 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -9,6 +9,7 @@ #include #include +#include namespace { using ::testing::Eq; @@ -218,7 +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(timesync_timeout_parameter)); + EXPECT_THAT(parameter_interface.getTimeSyncTimeout(), Eq(std::chrono::seconds(timesync_timeout_parameter))); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigEnvVarsOverruleParameters) { @@ -279,7 +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(5)); + EXPECT_THAT(parameter_interface.getTimeSyncTimeout(), Eq(std::chrono::seconds(5))); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithArm) {