From 803f75ab23968d3e94ac968ab3b7011618c72697 Mon Sep 17 00:00:00 2001 From: Manuel Schweiger Date: Fri, 10 Jan 2025 21:44:06 +0100 Subject: [PATCH 1/7] feat: add timesync timeout to params --- .../spot_driver/api/default_time_sync_api.hpp | 4 +++- .../interfaces/parameter_interface_base.hpp | 2 ++ .../interfaces/rclcpp_parameter_interface.hpp | 1 + spot_driver/src/api/default_spot_api.cpp | 13 ++++++++++++- spot_driver/src/api/default_time_sync_api.cpp | 7 ++++--- .../src/interfaces/rclcpp_parameter_interface.cpp | 5 +++++ .../spot_driver/fake/fake_parameter_interface.hpp | 2 ++ 7 files changed, 29 insertions(+), 5 deletions(-) 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..a61bec7e6 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 @@ -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 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..97982a6b5 100644 --- a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp +++ b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp @@ -46,6 +46,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; 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 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..7412b5e5a 100644 --- a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp +++ b/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::string> getCamerasUsed( const bool has_arm, const bool gripperless) const override; + [[nodiscard]] int8_t 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..1b967c6e6 100644 --- a/spot_driver/src/api/default_spot_api.cpp +++ b/spot_driver/src/api/default_spot_api.cpp @@ -8,10 +8,17 @@ #include #include #include +#include #include #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& certificate) { @@ -55,6 +62,10 @@ tl::expected 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(kNodeName, kNodeNameSpace); + auto parameter_interface = std::make_unique(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 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..4abe00f40 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, + int8_t 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/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index a2d6b0c2e..2424adefd 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/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(node_, kParameterNameGripperless, kDefaultGripperless); } +int8_t RclcppParameterInterface::getTimeSyncTimeout() const { + return declareAndGetParameter(node_, kParameterTimeSyncTimeout, kDefaultTimeSyncTimeout); +} + 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/test/include/spot_driver/fake/fake_parameter_interface.hpp b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp index 122ad8cb1..94aa74b5f 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 @@ -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"}; From ba0adbc744c1d50850da344302c8aac630c83fad Mon Sep 17 00:00:00 2001 From: Manuel Schweiger Date: Sun, 12 Jan 2025 15:24:45 +0100 Subject: [PATCH 2/7] fix: use existing nodes and add testcase --- .../spot_driver/api/default_spot_api.hpp | 4 +++- spot_driver/src/api/default_spot_api.cpp | 19 ++++++------------- .../src/images/spot_image_publisher_node.cpp | 3 ++- .../interfaces/rclcpp_parameter_interface.cpp | 2 +- spot_driver/src/kinematic/kinematic_node.cpp | 4 +++- .../object_sync/object_synchronizer_node.cpp | 4 +++- .../src/robot_state/state_publisher_node.cpp | 4 +++- .../test/src/test_parameter_interface.cpp | 4 ++++ 8 files changed, 25 insertions(+), 19 deletions(-) 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..ac6d2bfa9 100644 --- a/spot_driver/include/spot_driver/api/default_spot_api.hpp +++ b/spot_driver/include/spot_driver/api/default_spot_api.hpp @@ -20,7 +20,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& certificate = std::nullopt, + const std::optional timesync_timeout = std::nullopt); [[nodiscard]] tl::expected createRobot(const std::string& robot_name, const std::string& ip_address, @@ -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_; + int8_t timesync_timeout_; }; } // namespace spot_ros2 diff --git a/spot_driver/src/api/default_spot_api.cpp b/spot_driver/src/api/default_spot_api.cpp index 1b967c6e6..aa59f3f8c 100644 --- a/spot_driver/src/api/default_spot_api.cpp +++ b/spot_driver/src/api/default_spot_api.cpp @@ -8,20 +8,14 @@ #include #include #include -#include #include #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& certificate) { +DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::optional& certificate, + const std::optional timesync_timeout) { if (certificate.has_value()) { client_sdk_ = std::make_unique<::bosdyn::client::ClientSdk>(); client_sdk_->SetClientName(sdk_client_name); @@ -32,6 +26,9 @@ 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, @@ -62,10 +59,6 @@ tl::expected 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(kNodeName, kNodeNameSpace); - auto parameter_interface = std::make_unique(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."); @@ -75,7 +68,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, timesync_timeout); + 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/images/spot_image_publisher_node.cpp b/spot_driver/src/images/spot_image_publisher_node.cpp index 3d6fbbef2..4ded679fe 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()); + auto timesync_timeout = parameters->getTimeSyncTimeout(); + auto spot_api = std::make_unique(kSDKClientName, parameters->getCertificate(), timesync_timeout); 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 2424adefd..3cb84bac0 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -28,7 +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"; +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 diff --git a/spot_driver/src/kinematic/kinematic_node.cpp b/spot_driver/src/kinematic/kinematic_node.cpp index 56f852881..843cff0d1 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()); + auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); + auto spot_api = + std::make_unique(kSDKClientName, parameter_interface->getCertificate(), timesync_timeout); 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..bf6b2f0c8 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()); + auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); + auto spot_api = + std::make_unique(kDefaultSDKName, parameter_interface->getCertificate(), timesync_timeout); 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..21eb2d847 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()); + auto timesync_timeout = parameter_interface->getTimeSyncTimeout(); + auto spot_api = + std::make_unique(kDefaultSDKName, parameter_interface->getCertificate(), timesync_timeout); 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/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 4ad4be3a2..09ff3df2a 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -197,6 +197,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 +218,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)); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigEnvVarsOverruleParameters) { @@ -276,6 +279,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)); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithArm) { From 78d5962ebd8246cec73079c85c0925dbf53aecda Mon Sep 17 00:00:00 2001 From: Manuel Schweiger Date: Tue, 14 Jan 2025 17:28:18 +0100 Subject: [PATCH 3/7] chore: implemented requested changes --- spot_driver/include/spot_driver/api/default_spot_api.hpp | 8 +++++--- .../include/spot_driver/api/default_time_sync_api.hpp | 5 +++-- .../spot_driver/interfaces/parameter_interface_base.hpp | 5 +++-- .../interfaces/rclcpp_parameter_interface.hpp | 3 ++- spot_driver/src/api/default_spot_api.cpp | 9 ++++----- spot_driver/src/api/default_time_sync_api.cpp | 2 +- spot_driver/src/images/spot_image_publisher_node.cpp | 4 ++-- .../src/interfaces/rclcpp_parameter_interface.cpp | 6 ++++-- spot_driver/src/kinematic/kinematic_node.cpp | 4 ++-- spot_driver/src/object_sync/object_synchronizer_node.cpp | 4 ++-- spot_driver/src/robot_state/state_publisher_node.cpp | 4 ++-- .../spot_driver/fake/fake_parameter_interface.hpp | 3 ++- spot_driver/test/src/test_parameter_interface.cpp | 5 +++-- 13 files changed, 35 insertions(+), 27 deletions(-) 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 ac6d2bfa9..7a72143c6 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 a61bec7e6..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 @@ -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 97982a6b5..ad9d7e452 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 7412b5e5a..3a9ad02d9 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 aa59f3f8c..cc46a6394 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 4abe00f40..8d7550abb 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 4ded679fe..8adade368 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 3cb84bac0..0bf7bc884 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 843cff0d1..713c27090 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 bf6b2f0c8..2e4c8c536 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 21eb2d847..2a61e0b2c 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 94aa74b5f..615a9ca82 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 09ff3df2a..ac4c0c17c 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) { From aa7f8edd7018838aa37cb57caac90a4a14b5b476 Mon Sep 17 00:00:00 2001 From: Manuel Schweiger Date: Tue, 14 Jan 2025 17:35:34 +0100 Subject: [PATCH 4/7] chore: ran formatter --- spot_driver/include/spot_driver/api/default_spot_api.hpp | 4 +--- .../spot_driver/interfaces/parameter_interface_base.hpp | 2 +- .../spot_driver/interfaces/rclcpp_parameter_interface.hpp | 2 +- spot_driver/src/api/default_spot_api.cpp | 5 ++--- spot_driver/src/interfaces/rclcpp_parameter_interface.cpp | 2 +- .../include/spot_driver/fake/fake_parameter_interface.hpp | 2 +- spot_driver/test/src/test_parameter_interface.cpp | 2 +- 7 files changed, 8 insertions(+), 11 deletions(-) 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 7a72143c6..da2122627 100644 --- a/spot_driver/include/spot_driver/api/default_spot_api.hpp +++ b/spot_driver/include/spot_driver/api/default_spot_api.hpp @@ -20,8 +20,7 @@ namespace spot_ros2 { class DefaultSpotApi : public SpotApi { public: - explicit DefaultSpotApi(const std::string& sdk_client_name, - const std::chrono::seconds& timesync_timeout, + 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, @@ -46,6 +45,5 @@ class DefaultSpotApi : public SpotApi { 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/interfaces/parameter_interface_base.hpp b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp index ad9d7e452..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,10 +2,10 @@ #pragma once +#include #include #include #include -#include #include #include 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 3a9ad02d9..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,12 +5,12 @@ #include #include +#include #include #include #include #include #include -#include namespace spot_ros2 { /** diff --git a/spot_driver/src/api/default_spot_api.cpp b/spot_driver/src/api/default_spot_api.cpp index cc46a6394..af25eab82 100644 --- a/spot_driver/src/api/default_spot_api.cpp +++ b/spot_driver/src/api/default_spot_api.cpp @@ -14,10 +14,9 @@ namespace spot_ros2 { -DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, - const std::chrono::seconds& timesync_timeout, +DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::chrono::seconds& timesync_timeout, const std::optional& certificate) - : timesync_timeout_(timesync_timeout) { + : timesync_timeout_(timesync_timeout) { if (certificate.has_value()) { client_sdk_ = std::make_unique<::bosdyn::client::ClientSdk>(); client_sdk_->SetClientName(sdk_client_name); diff --git a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index 0bf7bc884..38bc37fcd 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -2,9 +2,9 @@ #include +#include #include #include -#include namespace { constexpr auto kEnvVarNameHostname = "SPOT_IP"; 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 615a9ca82..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,11 +4,11 @@ #include +#include #include #include #include #include -#include namespace spot_ros2::test { class FakeParameterInterface : public ParameterInterfaceBase { diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index ac4c0c17c..c7b8a3eff 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -8,8 +8,8 @@ #include #include -#include #include +#include namespace { using ::testing::Eq; From b0c0ac070c34a1c451df12db4d58de9e8d5e46d1 Mon Sep 17 00:00:00 2001 From: Manuel Schweiger <58733365+mschweig@users.noreply.github.com> Date: Tue, 14 Jan 2025 21:09:26 +0100 Subject: [PATCH 5/7] Update spot_driver/src/interfaces/rclcpp_parameter_interface.cpp Co-authored-by: Brian Jin <163037159+bjin-bdai@users.noreply.github.com> --- spot_driver/src/interfaces/rclcpp_parameter_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index 38bc37fcd..babb9b500 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -199,7 +199,7 @@ bool RclcppParameterInterface::getGripperless() const { } std::chrono::seconds RclcppParameterInterface::getTimeSyncTimeout() const { - int timeout_seconds = declareAndGetParameter(node_, kParameterTimeSyncTimeout, kDefaultTimeSyncTimeout.count()); + int timeout_seconds = declareAndGetParameter(node_, kParameterTimeSyncTimeout, std::chrono::duration_cast(kDefaultTimeSyncTimeout).count()); return std::chrono::seconds(timeout_seconds); } From e61394efec5cc18792bf8252a53cdd134816a6f7 Mon Sep 17 00:00:00 2001 From: Manuel Schweiger Date: Tue, 14 Jan 2025 21:13:44 +0100 Subject: [PATCH 6/7] chore: removed reference inconsistency --- spot_driver/include/spot_driver/api/default_spot_api.hpp | 2 +- spot_driver/src/api/default_spot_api.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 da2122627..583427ef2 100644 --- a/spot_driver/include/spot_driver/api/default_spot_api.hpp +++ b/spot_driver/include/spot_driver/api/default_spot_api.hpp @@ -20,7 +20,7 @@ namespace spot_ros2 { class DefaultSpotApi : public SpotApi { public: - explicit DefaultSpotApi(const std::string& sdk_client_name, const std::chrono::seconds& timesync_timeout, + 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, diff --git a/spot_driver/src/api/default_spot_api.cpp b/spot_driver/src/api/default_spot_api.cpp index af25eab82..c19cdbaae 100644 --- a/spot_driver/src/api/default_spot_api.cpp +++ b/spot_driver/src/api/default_spot_api.cpp @@ -14,7 +14,7 @@ namespace spot_ros2 { -DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::chrono::seconds& 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()) { From a1dd17b4f4ce7579dab504be21c2b92358c083cb Mon Sep 17 00:00:00 2001 From: Manuel Schweiger Date: Tue, 14 Jan 2025 21:20:46 +0100 Subject: [PATCH 7/7] chore: run formatter --- spot_driver/src/interfaces/rclcpp_parameter_interface.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index babb9b500..b4f7929af 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -199,7 +199,9 @@ bool RclcppParameterInterface::getGripperless() const { } std::chrono::seconds RclcppParameterInterface::getTimeSyncTimeout() const { - int timeout_seconds = declareAndGetParameter(node_, kParameterTimeSyncTimeout, std::chrono::duration_cast(kDefaultTimeSyncTimeout).count()); + int timeout_seconds = + declareAndGetParameter(node_, kParameterTimeSyncTimeout, + std::chrono::duration_cast(kDefaultTimeSyncTimeout).count()); return std::chrono::seconds(timeout_seconds); }