Skip to content

Commit

Permalink
chore: implemented requested changes
Browse files Browse the repository at this point in the history
  • Loading branch information
mschweig committed Jan 14, 2025
1 parent 5856332 commit 78d5962
Show file tree
Hide file tree
Showing 13 changed files with 35 additions and 27 deletions.
8 changes: 5 additions & 3 deletions spot_driver/include/spot_driver/api/default_spot_api.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#include <spot_driver/api/time_sync_api.hpp>
#include <spot_driver/api/world_object_client_interface.hpp>

#include <chrono>
#include <memory>
#include <optional>
#include <string>
Expand All @@ -20,8 +21,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<int8_t> timesync_timeout = std::nullopt);
const std::chrono::seconds& timesync_timeout,
const std::optional<std::string>& certificate = std::nullopt);

[[nodiscard]] tl::expected<void, std::string> createRobot(const std::string& robot_name,
const std::string& ip_address,
Expand All @@ -44,6 +45,7 @@ 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_;
const std::chrono::seconds timesync_timeout_;

};
} // namespace spot_ros2
5 changes: 3 additions & 2 deletions spot_driver/include/spot_driver/api/default_time_sync_api.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <spot_driver/api/time_sync_api.hpp>
#include <tl_expected/expected.hpp>

#include <chrono>
#include <memory>
#include <string>

Expand All @@ -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.
Expand All @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <optional>
#include <set>
#include <string>
#include <chrono>
#include <tl_expected/expected.hpp>

#include <spot_driver/types.hpp>
Expand Down Expand Up @@ -46,7 +47,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;
virtual std::chrono::seconds getTimeSyncTimeout() const = 0;

protected:
// These are the definitions of the default values for optional parameters.
Expand All @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <set>
#include <string>
#include <vector>
#include <chrono>

namespace spot_ros2 {
/**
Expand Down Expand Up @@ -42,7 +43,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;
[[nodiscard]] std::chrono::seconds getTimeSyncTimeout() const override;

private:
std::shared_ptr<rclcpp::Node> node_;
Expand Down
9 changes: 4 additions & 5 deletions spot_driver/src/api/default_spot_api.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,10 @@

namespace spot_ros2 {

DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::optional<std::string>& certificate,
const std::optional<int8_t> timesync_timeout) {
DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name,
const std::chrono::seconds& timesync_timeout,
const std::optional<std::string>& certificate)
: timesync_timeout_(timesync_timeout) {
if (certificate.has_value()) {
client_sdk_ = std::make_unique<::bosdyn::client::ClientSdk>();
client_sdk_->SetClientName(sdk_client_name);
Expand All @@ -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<void, std::string> DefaultSpotApi::createRobot(const std::string& robot_name,
Expand Down
2 changes: 1 addition & 1 deletion spot_driver/src/api/default_time_sync_api.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<google::protobuf::Duration, std::string> DefaultTimeSyncApi::getClockSkew() {
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/src/images/spot_image_publisher_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +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 timesync_timeout = parameters->getTimeSyncTimeout();
auto spot_api = std::make_unique<DefaultSpotApi>(kSDKClientName, parameters->getCertificate(), timesync_timeout);
const auto timesync_timeout = parameters->getTimeSyncTimeout();
auto spot_api = std::make_unique<DefaultSpotApi>(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));
Expand Down
6 changes: 4 additions & 2 deletions spot_driver/src/interfaces/rclcpp_parameter_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

#include <cstdlib>
#include <vector>
#include <chrono>

namespace {
constexpr auto kEnvVarNameHostname = "SPOT_IP";
Expand Down Expand Up @@ -197,8 +198,9 @@ bool RclcppParameterInterface::getGripperless() const {
return declareAndGetParameter<bool>(node_, kParameterNameGripperless, kDefaultGripperless);
}

int8_t RclcppParameterInterface::getTimeSyncTimeout() const {
return declareAndGetParameter<int8_t>(node_, kParameterTimeSyncTimeout, kDefaultTimeSyncTimeout);
std::chrono::seconds RclcppParameterInterface::getTimeSyncTimeout() const {
int timeout_seconds = declareAndGetParameter<int>(node_, kParameterTimeSyncTimeout, kDefaultTimeSyncTimeout.count());
return std::chrono::seconds(timeout_seconds);
}

std::set<spot_ros2::SpotCamera> RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm,
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/src/kinematic/kinematic_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,9 +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 timesync_timeout = parameter_interface->getTimeSyncTimeout();
const auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
auto spot_api =
std::make_unique<DefaultSpotApi>(kSDKClientName, parameter_interface->getCertificate(), timesync_timeout);
std::make_unique<DefaultSpotApi>(kSDKClientName, timesync_timeout, parameter_interface->getCertificate());
initialize(node, std::move(spot_api), parameter_interface, logger_interface);
}

Expand Down
4 changes: 2 additions & 2 deletions spot_driver/src/object_sync/object_synchronizer_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +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 timesync_timeout = parameter_interface->getTimeSyncTimeout();
const auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
auto spot_api =
std::make_unique<DefaultSpotApi>(kDefaultSDKName, parameter_interface->getCertificate(), timesync_timeout);
std::make_unique<DefaultSpotApi>(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),
Expand Down
4 changes: 2 additions & 2 deletions spot_driver/src/robot_state/state_publisher_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +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 timesync_timeout = parameter_interface->getTimeSyncTimeout();
const auto timesync_timeout = parameter_interface->getTimeSyncTimeout();
auto spot_api =
std::make_unique<DefaultSpotApi>(kDefaultSDKName, parameter_interface->getCertificate(), timesync_timeout);
std::make_unique<DefaultSpotApi>(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));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <set>
#include <string>
#include <vector>
#include <chrono>

namespace spot_ros2::test {
class FakeParameterInterface : public ParameterInterfaceBase {
Expand Down Expand Up @@ -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"};
Expand Down
5 changes: 3 additions & 2 deletions spot_driver/test/src/test_parameter_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <spot_driver/rclcpp_test.hpp>

#include <memory>
#include <chrono>

namespace {
using ::testing::Eq;
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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) {
Expand Down

0 comments on commit 78d5962

Please sign in to comment.