Add a clock interface to the Node API (#407)

node clock interface lower level abstraction
Update node and node interface to expose get_clock and now.
add unit tests to cover node clock API

rclcpp/CMakeLists.txt

@@ -49,6 +49,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/memory_strategy.cpp
   src/rclcpp/node.cpp
   src/rclcpp/node_interfaces/node_base.cpp
+  src/rclcpp/node_interfaces/node_clock.cpp
   src/rclcpp/node_interfaces/node_graph.cpp
   src/rclcpp/node_interfaces/node_parameters.cpp
   src/rclcpp/node_interfaces/node_services.cpp

rclcpp/include/rclcpp/node.hpp

@@ -40,6 +40,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
@@ -359,11 +360,24 @@ public:
     rclcpp::event::Event::SharedPtr event,
     std::chrono::nanoseconds timeout);
 
+  RCLCPP_PUBLIC
+  rclcpp::Clock::SharedPtr
+  get_clock();
+
+  RCLCPP_PUBLIC
+  Time
+  now();
+
   /// Return the Node's internal NodeBaseInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_base_interface();
 
+  /// Return the Node's internal NodeClockInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr
+  get_node_clock_interface();
+
   /// Return the Node's internal NodeGraphInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
@@ -402,6 +416,7 @@ private:
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
 
   bool use_intra_process_comms_;
 };

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -0,0 +1,71 @@
+// Copyright 2017 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_CLOCK_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_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_topics_interface.hpp"
+#include "rclcpp/time_source.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeClock part of the Node API.
+class NodeClock : public NodeClockInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClock)
+
+  RCLCPP_PUBLIC
+  explicit NodeClock(
+    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_PUBLIC
+  virtual
+  ~NodeClock();
+
+  /// Get a clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::SharedPtr
+  get_clock();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeClock)
+
+  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::Clock::SharedPtr ros_clock_;
+  rclcpp::TimeSource time_source_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -0,0 +1,44 @@
+// Copyright 2017 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_CLOCK_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_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 NodeClock part of the Node API.
+class NodeClockInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface)
+
+  /// Get a ROS clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::SharedPtr
+  get_clock() = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_

rclcpp/include/rclcpp/time_source.hpp

@@ -20,6 +20,7 @@
 
 #include "rcl/time.h"
 
+#include "builtin_interfaces/msg/time.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
 
 #include "rclcpp/node.hpp"
@@ -43,6 +44,13 @@ public:
   RCLCPP_PUBLIC
   void attachNode(rclcpp::node::Node::SharedPtr node);
 
+  RCLCPP_PUBLIC
+  void attachNode(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface);
+
   RCLCPP_PUBLIC
   void detachNode();
 
@@ -61,7 +69,10 @@ public:
 
 private:
   // Preserve the node reference
-  rclcpp::node::Node::SharedPtr node_;
+  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_;
 
   // The subscription for the clock callback
   using MessageT = builtin_interfaces::msg::Time;

rclcpp/src/rclcpp/node.cpp

@@ -24,6 +24,7 @@
 #include "rclcpp/graph_listener.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_base.hpp"
+#include "rclcpp/node_interfaces/node_clock.hpp"
 #include "rclcpp/node_interfaces/node_graph.hpp"
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 #include "rclcpp/node_interfaces/node_services.hpp"
@@ -58,6 +59,12 @@ Node::Node(
       node_topics_.get(),
       use_intra_process_comms
     )),
+  node_clock_(new rclcpp::node_interfaces::NodeClock(
+      node_base_,
+      node_topics_,
+      node_graph_,
+      node_services_
+    )),
   use_intra_process_comms_(use_intra_process_comms)
 {
 }
@@ -189,12 +196,32 @@ Node::wait_for_graph_change(
   node_graph_->wait_for_graph_change(event, timeout);
 }
 
+rclcpp::Clock::SharedPtr
+Node::get_clock()
+{
+  return node_clock_->get_clock();
+}
+
+rclcpp::Time
+Node::now()
+{
+  return node_clock_->get_clock()->now();
+}
+
+
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 Node::get_node_base_interface()
 {
   return node_base_;
 }
 
+rclcpp::node_interfaces::NodeClockInterface::SharedPtr
+Node::get_node_clock_interface()
+{
+  return node_clock_;
+}
+
+
 rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
 Node::get_node_graph_interface()
 {

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -0,0 +1,48 @@
+// Copyright 2017 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_clock.hpp"
+
+#include <memory>
+#include <string>
+
+using rclcpp::node_interfaces::NodeClock;
+
+NodeClock::NodeClock(
+  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)
+: node_base_(node_base),
+  node_topics_(node_topics),
+  node_graph_(node_graph),
+  node_services_(node_services),
+  ros_clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
+{
+  time_source_.attachNode(
+    node_base_,
+    node_topics_,
+    node_graph_,
+    node_services_);
+  time_source_.attachClock(ros_clock_);
+}
+
+NodeClock::~NodeClock()
+{}
+
+std::shared_ptr<rclcpp::Clock>
+NodeClock::get_clock()
+{
+  return ros_clock_;
+}

rclcpp/src/rclcpp/time_source.cpp

@@ -44,15 +44,53 @@ TimeSource::TimeSource()
 {
 }
 
-void TimeSource::attachNode(std::shared_ptr<rclcpp::node::Node> node)
+void TimeSource::attachNode(rclcpp::node::Node::SharedPtr node)
 {
-  node_ = node;
-  // TODO(tfoote): Update QOS
-  clock_subscription_ = node_->create_subscription<builtin_interfaces::msg::Time>(
-    "clock", std::bind(&TimeSource::clock_cb, this, std::placeholders::_1),
-    rmw_qos_profile_default);
+  attachNode(
+    node->get_node_base_interface(),
+    node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface());
+}
 
-  parameter_client_ = std::make_shared<rclcpp::parameter_client::AsyncParametersClient>(node);
+void TimeSource::attachNode(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface)
+{
+  node_base_ = node_base_interface;
+  node_topics_ = node_topics_interface;
+  node_graph_ = node_graph_interface;
+  node_services_ = node_services_interface;
+  // TODO(tfoote): Update QOS
+
+  const std::string & topic_name = "/clock";
+
+  rclcpp::callback_group::CallbackGroup::SharedPtr group;
+  using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+  auto msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
+  auto allocator = std::make_shared<Alloc>();
+
+  auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1);
+
+  clock_subscription_ = rclcpp::create_subscription<MessageT, decltype(cb), Alloc, SubscriptionT>(
+    node_topics_.get(),
+    topic_name,
+    std::move(cb),
+    rmw_qos_profile_default,
+    group,
+    false,
+    false,
+    msg_mem_strat,
+    allocator);
+
+  parameter_client_ = std::make_shared<rclcpp::parameter_client::AsyncParametersClient>(
+    node_base_,
+    node_topics_,
+    node_graph_,
+    node_services_
+    );
   parameter_subscription_ =
     parameter_client_->on_parameter_event(std::bind(&TimeSource::on_parameter_event,
       this, std::placeholders::_1));
@@ -63,7 +101,10 @@ void TimeSource::detachNode()
   this->ros_time_active_ = false;
   clock_subscription_.reset();
   parameter_client_.reset();
-  node_.reset();
+  node_base_.reset();
+  node_topics_.reset();
+  node_graph_.reset();
+  node_services_.reset();
   disable_ros_time();
 }
 
@@ -94,7 +135,7 @@ void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
 
 TimeSource::~TimeSource()
 {
-  if (node_) {
+  if (node_base_ || node_topics_ || node_graph_ || node_services_) {
     this->detachNode();
   }
 }

rclcpp/test/test_node.cpp

@@ -73,3 +73,19 @@ TEST_F(TestNode, get_name_and_namespace) {
     EXPECT_STREQ("/my/ns", node->get_namespace());
   }
 }
+
+TEST_F(TestNode, get_clock) {
+  auto node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+  auto ros_clock = node->get_clock();
+  EXPECT_TRUE(ros_clock != nullptr);
+  EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME);
+}
+
+TEST_F(TestNode, now) {
+  auto node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+  auto clock = node->get_clock();
+  auto now_builtin = node->now().nanoseconds();
+  auto now_external = clock->now().nanoseconds();
+  EXPECT_GE(now_external, now_builtin);
+  EXPECT_LT(now_external - now_builtin, 50000ul);
+}