diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp index 1594d5f8e5..9f76bd810d 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp @@ -52,6 +52,9 @@ class NodeTimeSource : public NodeTimeSourceInterface bool use_clock_thread = true ); + RCLCPP_PUBLIC + void attachClock(rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock); + RCLCPP_PUBLIC virtual ~NodeTimeSource(); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp index 3783e5d83a..8b9ee9e0cb 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -30,6 +31,10 @@ class NodeTimeSourceInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface) + RCLCPP_PUBLIC + virtual + void attachClock(rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock) = 0; + RCLCPP_PUBLIC virtual ~NodeTimeSourceInterface() = default; diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 71b0d997cf..f3733e1017 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -27,6 +27,7 @@ #include "rclcpp/publisher_options.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" namespace rclcpp { @@ -408,6 +409,16 @@ class NodeOptions NodeOptions & allocator(rcl_allocator_t allocator); + /// Return the time source to be used. + RCLCPP_PUBLIC + const rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + time_source() const; + + /// Set the time source to be used. + RCLCPP_PUBLIC + NodeOptions & + time_source(rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr time_source); + private: // This is mutable to allow for a const accessor which lazily creates the node options instance. /// Underlying rcl_node_options structure. @@ -456,6 +467,8 @@ class NodeOptions bool automatically_declare_parameters_from_overrides_ {false}; rcl_allocator_t allocator_ {rcl_get_default_allocator()}; + + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr time_source_ {nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 1a68c7f108..6bd1e07007 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -213,7 +213,9 @@ Node::Node( options.allow_undeclared_parameters(), options.automatically_declare_parameters_from_overrides() )), - node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( + node_time_source_( + options.time_source() ? options.time_source() : + std::make_shared( node_base_, node_topics_, node_graph_, @@ -235,6 +237,7 @@ Node::Node( sub_namespace_(""), effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_)) { + node_time_source_->attachClock(node_clock_); // we have got what we wanted directly from the overrides, // but declare the parameters anyway so they are visible. rclcpp::detail::declare_qos_parameters( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp index 2bd3a098b6..7aa445da03 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp @@ -46,7 +46,11 @@ NodeTimeSource::NodeTimeSource( node_logging_, node_clock_, node_parameters_); - time_source_.attachClock(node_clock_->get_clock()); +} + +void NodeTimeSource::attachClock(rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock) +{ + time_source_.attachClock(clock->get_clock()); } NodeTimeSource::~NodeTimeSource() diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index ca58154eb4..22e5dc10e2 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -88,6 +88,7 @@ NodeOptions::operator=(const NodeOptions & other) this->automatically_declare_parameters_from_overrides_ = other.automatically_declare_parameters_from_overrides_; this->allocator_ = other.allocator_; + this->time_source_ = other.time_source_; } return *this; } @@ -397,4 +398,17 @@ NodeOptions::allocator(rcl_allocator_t allocator) return *this; } +const rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr +NodeOptions::time_source() const +{ + return this->time_source_; +} + +NodeOptions & +NodeOptions::time_source(rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr time_source) +{ + this->time_source_ = time_source; + return *this; +} + } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 7abc36f38e..41159b3a25 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -22,6 +22,7 @@ #include "rcl/arguments.h" #include "rcl/remap.h" +#include "rclcpp/node.hpp" #include "rclcpp/node_options.hpp" #include "../mocking_utils/patch.hpp" @@ -210,6 +211,8 @@ TEST(TestNodeOptions, copy) { // We separate attribute modification from variable initialisation (copy assignment operator) // to be sure the "non_default_options"'s properties are correctly set before testing the // assignment operator. + rclcpp::init(0, nullptr); + rclcpp::Node node("time_sink__test_node"); auto non_default_options = rclcpp::NodeOptions(); non_default_options .parameter_overrides({rclcpp::Parameter("foo", 0), rclcpp::Parameter("bar", "1")}) @@ -226,7 +229,8 @@ TEST(TestNodeOptions, copy) { .parameter_event_qos(rclcpp::ClockQoS()) .rosout_qos(rclcpp::ParameterEventsQoS()) .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true); + .automatically_declare_parameters_from_overrides(true) + .time_source(node.get_node_time_source_interface()); auto copied_options = non_default_options; EXPECT_EQ(non_default_options.parameter_overrides(), copied_options.parameter_overrides()); @@ -250,6 +254,8 @@ TEST(TestNodeOptions, copy) { copied_options.allow_undeclared_parameters()); EXPECT_EQ(non_default_options.automatically_declare_parameters_from_overrides(), copied_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(non_default_options.time_source(), copied_options.time_source()); + rclcpp::shutdown(); } }