Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adds timesync timeout to params #552

Merged
merged 9 commits into from
Jan 14, 2025
Merged

Adds timesync timeout to params #552

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
803f75a
feat: add timesync timeout to params
mschweig Jan 10, 2025
ba0adbc
fix: use existing nodes and add testcase
mschweig Jan 12, 2025
402e425
Merge branch 'main' into feat/add-timesync-param-to-ParameterInterface
mschweig Jan 14, 2025
5856332
Merge branch 'bdaiinstitute:main' into feat/add-timesync-param-to-Par…
mschweig Jan 14, 2025
78d5962
chore: implemented requested changes
mschweig Jan 14, 2025
aa7f8ed
chore: ran formatter
mschweig Jan 14, 2025
b0c0ac0
Update spot_driver/src/interfaces/rclcpp_parameter_interface.cpp
mschweig Jan 14, 2025
e61394e
chore: removed reference inconsistency
mschweig Jan 14, 2025
a1dd17b
chore: run formatter
mschweig Jan 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion spot_driver/include/spot_driver/api/default_spot_api.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,8 @@ namespace spot_ros2 {
class DefaultSpotApi : public SpotApi {
public:
explicit DefaultSpotApi(const std::string& sdk_client_name,
const std::optional<std::string>& certificate = std::nullopt);
const std::optional<std::string>& certificate = std::nullopt,
const std::optional<int8_t> timesync_timeout = std::nullopt);
mschweig marked this conversation as resolved.
Show resolved Hide resolved

[[nodiscard]] tl::expected<void, std::string> createRobot(const std::string& robot_name,
const std::string& ip_address,
Expand All @@ -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_;
int8_t timesync_timeout_;
};
} // namespace spot_ros2
4 changes: 3 additions & 1 deletion spot_driver/include/spot_driver/api/default_time_sync_api.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -36,5 +37,6 @@ class DefaultTimeSyncApi : public TimeSyncApi {

private:
std::shared_ptr<::bosdyn::client::TimeSyncThread> time_sync_thread_;
int8_t timesync_timeout_;
mschweig marked this conversation as resolved.
Show resolved Hide resolved
};
} // namespace spot_ros2
2 changes: 2 additions & 0 deletions spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
1 change: 1 addition & 0 deletions spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
8 changes: 6 additions & 2 deletions spot_driver/src/api/default_spot_api.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,8 @@

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::optional<std::string>& certificate,
const std::optional<int8_t> timesync_timeout) {
if (certificate.has_value()) {
client_sdk_ = std::make_unique<::bosdyn::client::ClientSdk>();
client_sdk_->SetClientName(sdk_client_name);
Expand All @@ -25,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();
}
mschweig marked this conversation as resolved.
Show resolved Hide resolved
}

tl::expected<void, std::string> DefaultSpotApi::createRobot(const std::string& robot_name,
Expand Down Expand Up @@ -64,7 +68,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>(
Expand Down
7 changes: 4 additions & 3 deletions spot_driver/src/api/default_time_sync_api.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
mschweig marked this conversation as resolved.
Show resolved Hide resolved
: 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_))) {
mschweig marked this conversation as resolved.
Show resolved Hide resolved
return tl::make_unexpected("Failed to establish time sync before timing out.");
}
if (!time_sync_thread_->HasEstablishedTimeSync()) {
Expand Down
3 changes: 2 additions & 1 deletion spot_driver/src/images/spot_image_publisher_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
auto timesync_timeout = parameters->getTimeSyncTimeout();
mschweig marked this conversation as resolved.
Show resolved Hide resolved
auto spot_api = std::make_unique<DefaultSpotApi>(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));
Expand Down
5 changes: 5 additions & 0 deletions spot_driver/src/interfaces/rclcpp_parameter_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 = "timesync_timeout";

/**
* @brief Get a rclcpp parameter. If the parameter has not been declared, declare it with the provided default value and
Expand Down Expand Up @@ -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);
Expand Down
4 changes: 3 additions & 1 deletion spot_driver/src/kinematic/kinematic_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
auto spot_api =
std::make_unique<DefaultSpotApi>(kSDKClientName, parameter_interface->getCertificate(), timesync_timeout);
initialize(node, std::move(spot_api), parameter_interface, logger_interface);
}

Expand Down
4 changes: 3 additions & 1 deletion spot_driver/src/object_sync/object_synchronizer_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
auto spot_api =
std::make_unique<DefaultSpotApi>(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),
Expand Down
4 changes: 3 additions & 1 deletion spot_driver/src/robot_state/state_publisher_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
auto spot_api =
std::make_unique<DefaultSpotApi>(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));
Expand Down
2 changes: 2 additions & 0 deletions spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"};
Expand Down
4 changes: 4 additions & 0 deletions spot_driver/test/src/test_parameter_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
mschweig marked this conversation as resolved.
Show resolved Hide resolved

// GIVEN we create a RclcppParameterInterface using the node
RclcppParameterInterface parameter_interface{node_};
Expand All @@ -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) {
Expand Down Expand Up @@ -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) {
Expand Down
Loading