Allow to pass existing TimeSource via NodeOptions

Signed-off-by: Patrick Roncagliolo <[email protected]>
ros2 · Nov 14, 2024 · 09cfbf2 · 09cfbf2
1 parent 88ebea9
commit 09cfbf2
Show file tree

Hide file tree

Showing 6 changed files with 44 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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(

diff --git 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
@@ -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