WIP: Allow to pass existing TimeSource via NodeOptions

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();

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;

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

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<rclcpp::node_interfaces::NodeTimeSource>(
       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(

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()

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

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();
   }
 }