Resolve startup race condition for sim time (#608)

Resolves #595 

* Separate the Node Time Source from the Node Clock
* Implement initial value checking of use_sim_time parameter parameter
* Be sure to update all newly attached clocks
* Homogenizing the behavior to use the last received value otherwise zero time when enabling sim time.
* Add virtual destructors to interface classes

rclcpp/CMakeLists.txt

@@ -53,6 +53,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/node_interfaces/node_logging.cpp
   src/rclcpp/node_interfaces/node_parameters.cpp
   src/rclcpp/node_interfaces/node_services.cpp
+  src/rclcpp/node_interfaces/node_time_source.cpp
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.cpp
   src/rclcpp/node_interfaces/node_waitables.cpp

rclcpp/include/rclcpp/node.hpp

@@ -47,6 +47,7 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/node_interfaces/node_waitables_interface.hpp"
@@ -458,6 +459,11 @@ public:
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
   get_node_parameters_interface();
 
+  /// Return the Node's internal NodeParametersInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+  get_node_time_source_interface();
+
 private:
   RCLCPP_DISABLE_COPY(Node)
 
@@ -473,6 +479,7 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
   bool use_intra_process_comms_;

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -38,6 +38,10 @@ class NodeBaseInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeBaseInterface() = default;
+
   /// Return the name of the node.
   /** \return The name of the node. */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -64,7 +64,6 @@ private:
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
   rclcpp::Clock::SharedPtr ros_clock_;
-  rclcpp::TimeSource time_source_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -31,6 +31,10 @@ class NodeClockInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeClockInterface() = default;
+
   /// Get a ROS clock which will be kept up to date by the node.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -38,6 +38,10 @@ class NodeGraphInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeGraphInterface() = default;
+
   /// Return a map of existing topic names to list of topic types.
   /**
    * A topic is considered to exist when at least one publisher or subscriber

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -33,6 +33,10 @@ class NodeLoggingInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeLoggingInterface() = default;
+
   /// Return the logger of the node.
   /** \return The logger of the node. */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -37,6 +37,10 @@ class NodeParametersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParametersInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -32,6 +32,10 @@ class NodeServicesInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeServicesInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   void

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

@@ -0,0 +1,72 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/time_source.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTimeSource part of the Node API.
+class NodeTimeSource : public NodeTimeSourceInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSource)
+
+  RCLCPP_PUBLIC
+  explicit NodeTimeSource(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters
+  );
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimeSource();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTimeSource)
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+
+  rclcpp::TimeSource time_source_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -0,0 +1,42 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTimeSource part of the Node API.
+class NodeTimeSourceInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimeSourceInterface() = default;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -31,6 +31,10 @@ class NodeTimersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimersInterface() = default;
+
   /// Add a timer to the node.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -40,6 +40,10 @@ class NodeTopicsInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTopicsInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::PublisherBase::SharedPtr

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -31,6 +31,10 @@ class NodeWaitablesInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitablesInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   void

rclcpp/include/rclcpp/time_source.hpp

@@ -51,7 +51,9 @@ public:
     rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
     rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface);
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface);
 
   RCLCPP_PUBLIC
   void detachNode();
@@ -76,6 +78,8 @@ private:
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
 
   // Store (and update on node attach) logger for logging.
   Logger logger_;

rclcpp/src/rclcpp/node.cpp

@@ -29,6 +29,7 @@
 #include "rclcpp/node_interfaces/node_logging.hpp"
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 #include "rclcpp/node_interfaces/node_services.hpp"
+#include "rclcpp/node_interfaces/node_time_source.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
 #include "rclcpp/node_interfaces/node_waitables.hpp"
@@ -83,6 +84,15 @@ Node::Node(
       use_intra_process_comms,
       start_parameter_services
     )),
+  node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(
+      node_base_,
+      node_topics_,
+      node_graph_,
+      node_services_,
+      node_logging_,
+      node_clock_,
+      node_parameters_
+    )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   use_intra_process_comms_(use_intra_process_comms)
 {
@@ -263,6 +273,12 @@ Node::get_node_logging_interface()
   return node_logging_;
 }
 
+rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+Node::get_node_time_source_interface()
+{
+  return node_time_source_;
+}
+
 rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
 Node::get_node_timers_interface()
 {

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -31,15 +31,7 @@ NodeClock::NodeClock(
   node_services_(node_services),
   node_logging_(node_logging),
   ros_clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
-{
-  time_source_.attachNode(
-    node_base_,
-    node_topics_,
-    node_graph_,
-    node_services_,
-    node_logging_);
-  time_source_.attachClock(ros_clock_);
-}
+{}
 
 NodeClock::~NodeClock()
 {}

rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp

@@ -0,0 +1,50 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/node_interfaces/node_time_source.hpp"
+
+#include <memory>
+#include <string>
+
+using rclcpp::node_interfaces::NodeTimeSource;
+
+NodeTimeSource::NodeTimeSource(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters)
+: node_base_(node_base),
+  node_topics_(node_topics),
+  node_graph_(node_graph),
+  node_services_(node_services),
+  node_logging_(node_logging),
+  node_clock_(node_clock),
+  node_parameters_(node_parameters)
+{
+  time_source_.attachNode(
+    node_base_,
+    node_topics_,
+    node_graph_,
+    node_services_,
+    node_logging_,
+    node_clock_,
+    node_parameters_);
+  time_source_.attachClock(node_clock_->get_clock());
+}
+
+NodeTimeSource::~NodeTimeSource()
+{}

rclcpp/src/rclcpp/time_source.cpp

@@ -53,7 +53,9 @@ void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
     node->get_node_topics_interface(),
     node->get_node_graph_interface(),
     node->get_node_services_interface(),
-    node->get_node_logging_interface());
+    node->get_node_logging_interface(),
+    node->get_node_clock_interface(),
+    node->get_node_parameters_interface());
 }
 
 void TimeSource::attachNode(
@@ -61,17 +63,38 @@ void TimeSource::attachNode(
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface)
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface)
 {
   node_base_ = node_base_interface;
   node_topics_ = node_topics_interface;
   node_graph_ = node_graph_interface;
   node_services_ = node_services_interface;
   node_logging_ = node_logging_interface;
+  node_clock_ = node_clock_interface;
+  node_parameters_ = node_parameters_interface;
   // TODO(tfoote): Update QOS
 
   logger_ = node_logging_->get_logger();
 
+  rclcpp::Parameter use_sim_time_param;
+  if (node_parameters_->get_parameter("use_sim_time", use_sim_time_param)) {
+    if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
+      if (use_sim_time_param.get_value<bool>() == true) {
+        parameter_state_ = SET_TRUE;
+        enable_ros_time();
+        create_clock_sub();
+      }
+    } else {
+      RCLCPP_ERROR(logger_, "Invalid type for parameter 'use_sim_time' %s should be bool",
+        use_sim_time_param.get_type_name().c_str());
+    }
+  } else {
+    RCLCPP_DEBUG(logger_, "'use_sim_time' parameter not set, using wall time by default.");
+  }
+
+  // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
   parameter_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
     node_base_,
     node_topics_,
@@ -92,6 +115,9 @@ void TimeSource::detachNode()
   node_topics_.reset();
   node_graph_.reset();
   node_services_.reset();
+  node_logging_.reset();
+  node_clock_.reset();
+  node_parameters_.reset();
   disable_ros_time();
 }
 
@@ -103,11 +129,12 @@ void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
 
   std::lock_guard<std::mutex> guard(clock_list_lock_);
   associated_clocks_.push_back(clock);
-  // Set the clock if there's already data for it
+  // Set the clock to zero unless there's a recently received message
+  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
   if (last_msg_set_) {
-    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
-    set_clock(time_msg, ros_time_active_, clock);
+    time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
   }
+  set_clock(time_msg, ros_time_active_, clock);
 }
 
 void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
@@ -123,7 +150,9 @@ void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
 
 TimeSource::~TimeSource()
 {
-  if (node_base_ || node_topics_ || node_graph_ || node_services_) {
+  if (node_base_ || node_topics_ || node_graph_ || node_services_ ||
+    node_logging_ || node_clock_ || node_parameters_)
+  {
     this->detachNode();
   }
 }
@@ -257,13 +286,14 @@ void TimeSource::enable_ros_time()
   // Local storage
   ros_time_active_ = true;
 
-  // Update all attached clocks
+  // Update all attached clocks to zero or last recorded time
   std::lock_guard<std::mutex> guard(clock_list_lock_);
+  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
+  if (last_msg_set_) {
+    time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
+  }
   for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
-    msg->sec = 0;
-    msg->nanosec = 0;
-    set_clock(msg, true, *it);
+    set_clock(time_msg, true, *it);
   }
 }
 

rclcpp/test/test_time_source.cpp

@@ -112,7 +112,7 @@ TEST_F(TestTimeSource, reattach) {
   ASSERT_NO_THROW(ts.attachNode(node));
 }
 
-TEST_F(TestTimeSource, ROS_time_valid) {
+TEST_F(TestTimeSource, ROS_time_valid_attach_detach) {
   rclcpp::TimeSource ts;
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
 
@@ -131,6 +131,37 @@ TEST_F(TestTimeSource, ROS_time_valid) {
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 }
 
+TEST_F(TestTimeSource, ROS_time_valid_wall_time) {
+  rclcpp::TimeSource ts;
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  auto ros_clock2 = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachNode(node);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachClock(ros_clock2);
+  EXPECT_FALSE(ros_clock2->ros_time_is_active());
+}
+
+TEST_F(TestTimeSource, ROS_time_valid_sim_time) {
+  rclcpp::TimeSource ts;
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  auto ros_clock2 = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  node->set_parameter_if_not_set("use_sim_time", true);
+  ts.attachNode(node);
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  ts.attachClock(ros_clock2);
+  EXPECT_TRUE(ros_clock2->ros_time_is_active());
+}
+
 TEST_F(TestTimeSource, clock) {
   rclcpp::TimeSource ts(node);
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);