update rclcpp to use the refactored TimeSource Clock logic (#371)

This implements a TimeSource in rclcpp, adds the Clock class.

rclcpp/CMakeLists.txt

@@ -31,8 +31,10 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/any_executable.cpp
   src/rclcpp/callback_group.cpp
   src/rclcpp/client.cpp
+  src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/duration.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions.cpp
   src/rclcpp/executor.cpp
@@ -59,6 +61,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/service.cpp
   src/rclcpp/subscription.cpp
   src/rclcpp/time.cpp
+  src/rclcpp/time_source.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
@@ -266,6 +269,17 @@ if(BUILD_TESTING)
     endforeach()
   endif()
 
+  ament_add_gtest(test_duration test/test_duration.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_duration)
+    ament_target_dependencies(test_duration
+      "rcl")
+    target_link_libraries(test_duration ${PROJECT_NAME})
+  endif()
+
+  ament_add_gmock(test_logging test/test_logging.cpp)
+  target_link_libraries(test_logging ${PROJECT_NAME})
+
   ament_add_gtest(test_time test/test_time.cpp
     APPEND_LIBRARY_DIRS "${append_library_dirs}")
   if(TARGET test_time)
@@ -274,8 +288,13 @@ if(BUILD_TESTING)
     target_link_libraries(test_time ${PROJECT_NAME})
   endif()
 
-  ament_add_gmock(test_logging test/test_logging.cpp)
+  ament_add_gtest(test_time_source test/test_time_source.cpp
-  target_link_libraries(test_logging ${PROJECT_NAME})
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_time_source)
+    ament_target_dependencies(test_time_source
+      "rcl")
+    target_link_libraries(test_time_source ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package(

rclcpp/include/rclcpp/clock.hpp

@@ -0,0 +1,143 @@
+// 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__CLOCK_HPP_
+#define RCLCPP__CLOCK_HPP_
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/time.h"
+
+namespace rclcpp
+{
+
+class TimeSource;
+
+/*  \brief A struct to represent a timejump.
+ *  It epresents the time jump duration and whether it changed clock type.
+ */
+struct TimeJump
+{
+  typedef enum ClockChange_t
+  {
+    ROS_TIME_NO_CHANGE,    ///< The time type before and after the jump is ROS_TIME
+    ROS_TIME_ACTIVATED,    ///< The time type switched to ROS_TIME from SYSTEM_TIME
+    ROS_TIME_DEACTIVATED,  ///< The time type switched to SYSTEM_TIME from ROS_TIME
+    SYSTEM_TIME_NO_CHANGE  ///< The time type before and after the jump is SYSTEM_TIME
+  } ClockChange_t;
+
+  ClockChange_t jump_type_;  ///< The change in clock_type if there is one.
+  rcl_duration_t delta_;     ///< The change in time value.
+};
+
+/* \brief A class to store a threshold for a TimeJump
+ * This class can be used to evaluate a time jump's magnitude.
+*/
+class JumpThreshold
+{
+public:
+  uint64_t min_forward_;   ///< The minimum jump forward to be considered exceeded..
+  uint64_t min_backward_;  ///< The minimum backwards jump to be considered exceeded.
+  bool on_clock_change_;   ///< Whether to trigger on any clock type change.
+
+  // Test if the threshold is exceeded by a TimeJump
+  RCLCPP_PUBLIC
+  bool
+  is_exceeded(const TimeJump & jump);
+};
+
+class JumpHandler
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler)
+  JumpHandler(
+    std::function<void()> pre_callback,
+    std::function<void(TimeJump)> post_callback,
+    JumpThreshold & threshold);
+
+  std::function<void()> pre_callback;
+  std::function<void(const TimeJump &)> post_callback;
+  JumpThreshold notice_threshold;
+};
+
+class Clock
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Clock)
+
+  RCLCPP_PUBLIC
+  explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  ~Clock();
+
+  RCLCPP_PUBLIC
+  Time
+  now();
+
+  RCLCPP_PUBLIC
+  bool
+  ros_time_is_active();
+
+  RCLCPP_PUBLIC
+  rcl_clock_type_t
+  get_clock_type();
+
+  // Add a callback to invoke if the jump threshold is exceeded.
+  /**
+   * These callback functions must remain valid as long as the
+   * returned shared pointer is valid.
+   */
+  RCLCPP_PUBLIC
+  JumpHandler::SharedPtr
+  create_jump_callback(
+    std::function<void()> pre_callback,
+    std::function<void(const TimeJump &)> post_callback,
+    JumpThreshold & threshold);
+
+private:
+  // A method for TimeSource to get a list of callbacks to invoke while updating
+  RCLCPP_PUBLIC
+  std::vector<JumpHandler::SharedPtr>
+  get_triggered_callback_handlers(const TimeJump & jump);
+
+  // Invoke callbacks that are valid and outside threshold.
+  RCLCPP_PUBLIC
+  static void invoke_prejump_callbacks(
+    const std::vector<JumpHandler::SharedPtr> & callbacks);
+
+  RCLCPP_PUBLIC
+  static void invoke_postjump_callbacks(
+    const std::vector<JumpHandler::SharedPtr> & callbacks,
+    const TimeJump & jump);
+
+  /// Internal storage backed by rcl
+  rcl_clock_t rcl_clock_;
+  friend TimeSource;  /// Allow TimeSource to access the rcl_clock_ datatype.
+  rcl_allocator_t allocator_;
+
+  std::mutex callback_list_mutex_;
+  std::vector<std::weak_ptr<JumpHandler>> active_jump_handlers_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CLOCK_HPP_

rclcpp/include/rclcpp/duration.hpp

@@ -0,0 +1,102 @@
+// 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__DURATION_HPP_
+#define RCLCPP__DURATION_HPP_
+
+#include <chrono>
+
+#include "builtin_interfaces/msg/duration.hpp"
+#include "rcl/time.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+class Duration
+{
+public:
+  RCLCPP_PUBLIC
+  Duration(int32_t seconds, uint32_t nanoseconds);
+
+  RCLCPP_PUBLIC
+  explicit Duration(
+    rcl_duration_value_t nanoseconds);
+
+  RCLCPP_PUBLIC
+  explicit Duration(
+    std::chrono::nanoseconds nanoseconds);
+
+  RCLCPP_PUBLIC
+  Duration(
+    const builtin_interfaces::msg::Duration & duration_msg);
+
+  RCLCPP_PUBLIC
+  explicit Duration(const rcl_duration_t & duration);
+
+  RCLCPP_PUBLIC
+  Duration(const Duration & rhs);
+
+  RCLCPP_PUBLIC
+  virtual ~Duration();
+
+  RCLCPP_PUBLIC
+  operator builtin_interfaces::msg::Duration() const;
+
+  RCLCPP_PUBLIC
+  Duration &
+  operator=(const Duration & rhs);
+
+  RCLCPP_PUBLIC
+  Duration &
+  operator=(const builtin_interfaces::msg::Duration & Duration_msg);
+
+  RCLCPP_PUBLIC
+  bool
+  operator==(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<=(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>=(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator+(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  Duration
+  operator-(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  rcl_duration_value_t
+  nanoseconds() const;
+
+private:
+  rcl_duration_t rcl_duration_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DURATION_HPP_

rclcpp/include/rclcpp/time.hpp

@@ -21,28 +21,32 @@
 
 #include "rcl/time.h"
 
+#include "rclcpp/duration.hpp"
+
 namespace rclcpp
 {
 
+class Clock;
+
 class Time
 {
 public:
   RCLCPP_PUBLIC
-  static
+  Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
-  Time
-  now(rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
 
   RCLCPP_PUBLIC
-  Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
+  explicit Time(uint64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
-
-  RCLCPP_PUBLIC
-  explicit Time(uint64_t nanoseconds = 0, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
 
   RCLCPP_PUBLIC
   Time(const Time & rhs);
 
   RCLCPP_PUBLIC
-  Time(const builtin_interfaces::msg::Time & time_msg);  // NOLINT
+  Time(
+    const builtin_interfaces::msg::Time & time_msg,
+    rcl_clock_type_t ros_time = RCL_ROS_TIME);
+
+  RCLCPP_PUBLIC
+  explicit Time(const rcl_time_point_t & time_point);
 
   RCLCPP_PUBLIC
   virtual ~Time();
@@ -84,21 +88,32 @@ public:
 
   RCLCPP_PUBLIC
   Time
-  operator+(const rclcpp::Time & rhs) const;
+  operator+(const rclcpp::Duration & rhs) const;
 
   RCLCPP_PUBLIC
-  Time
+  Duration
   operator-(const rclcpp::Time & rhs) const;
 
   RCLCPP_PUBLIC
-  uint64_t
+  Time
+  operator-(const rclcpp::Duration & rhs) const;
+
+  RCLCPP_PUBLIC
+  rcl_time_point_value_t
   nanoseconds() const;
 
+  RCLCPP_PUBLIC
+  rcl_clock_type_t
+  get_clock_type() const;
+
 private:
-  rcl_time_source_t rcl_time_source_;
   rcl_time_point_t rcl_time_;
+  friend Clock;  // Allow clock to manipulate internal data
 };
 
+Time
+operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__TIME_HPP_

rclcpp/include/rclcpp/time_source.hpp

@@ -0,0 +1,116 @@
+// 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__TIME_SOURCE_HPP_
+#define RCLCPP__TIME_SOURCE_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rcl/time.h"
+
+#include "rcl_interfaces/msg/parameter_event.hpp"
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter_client.hpp"
+
+
+namespace rclcpp
+{
+class Clock;
+
+class TimeSource
+{
+public:
+  RCLCPP_PUBLIC
+  explicit TimeSource(rclcpp::node::Node::SharedPtr node);
+
+  RCLCPP_PUBLIC
+  TimeSource();
+
+  RCLCPP_PUBLIC
+  void attachNode(rclcpp::node::Node::SharedPtr node);
+
+  RCLCPP_PUBLIC
+  void detachNode();
+
+  /// Attach a clock to the time source to be updated
+  /**
+   * \throws std::invalid_argument if node is nullptr
+   */
+  RCLCPP_PUBLIC
+  void attachClock(rclcpp::Clock::SharedPtr clock);
+
+  RCLCPP_PUBLIC
+  void detachClock(rclcpp::Clock::SharedPtr clock);
+
+  RCLCPP_PUBLIC
+  ~TimeSource();
+
+private:
+  // Preserve the node reference
+  rclcpp::node::Node::SharedPtr node_;
+
+  // The subscription for the clock callback
+  using MessageT = builtin_interfaces::msg::Time;
+  using Alloc = std::allocator<void>;
+  using SubscriptionT = rclcpp::subscription::Subscription<MessageT, Alloc>;
+  std::shared_ptr<SubscriptionT> clock_subscription_;
+
+  // The clock callback itself
+  void clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg);
+
+  // Parameter Client pointer
+  std::shared_ptr<rclcpp::parameter_client::AsyncParametersClient> parameter_client_;
+
+  // Parameter Event subscription
+  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
+  using ParamSubscriptionT = rclcpp::subscription::Subscription<ParamMessageT, Alloc>;
+  std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
+
+  // Callback for parameter updates
+  void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+
+  // An enum to hold the parameter state
+  enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
+  UseSimTimeParameterState parameter_state_;
+
+  // An internal method to use in the clock callback that iterates and enables all clocks
+  void enable_ros_time();
+  // An internal method to use in the clock callback that iterates and disables all clocks
+  void disable_ros_time();
+
+  // Internal helper functions used inside iterators
+  static void enable_ros_time(rclcpp::Clock::SharedPtr clock);
+  static void disable_ros_time(rclcpp::Clock::SharedPtr clock);
+  static void set_clock(
+    const builtin_interfaces::msg::Time::SharedPtr msg,
+    bool set_ros_time_enabled,
+    rclcpp::Clock::SharedPtr clock);
+
+  // Local storage of validity of ROS time
+  // This is needed when new clocks are added.
+  bool ros_time_active_;
+  // Last set message to be passed to newly registered clocks
+  builtin_interfaces::msg::Time::SharedPtr last_msg_set_;
+
+  // A lock to protect iterating the associated_clocks_ field.
+  std::mutex clock_list_lock_;
+  // A vector to store references to associated clocks.
+  std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIME_SOURCE_HPP_

rclcpp/src/rclcpp/clock.cpp

@@ -0,0 +1,164 @@
+// 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/clock.hpp"
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace rclcpp
+{
+bool
+JumpThreshold::is_exceeded(const TimeJump & jump)
+{
+  if (on_clock_change_ &&
+    (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED ||
+    jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED))
+  {
+    return true;
+  }
+  if ((uint64_t)jump.delta_.nanoseconds > min_forward_ ||
+    (uint64_t)jump.delta_.nanoseconds < min_backward_)
+  {
+    return true;
+  }
+  return false;
+}
+
+JumpHandler::JumpHandler(
+  std::function<void()> pre_callback,
+  std::function<void(TimeJump)> post_callback,
+  JumpThreshold & threshold)
+: pre_callback(pre_callback),
+  post_callback(post_callback),
+  notice_threshold(threshold)
+{}
+
+Clock::Clock(rcl_clock_type_t clock_type)
+{
+  allocator_ = rcl_get_default_allocator();
+  auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+}
+
+Clock::~Clock()
+{
+  auto ret = rcl_clock_fini(&rcl_clock_);
+  if (ret != RCL_RET_OK) {
+    RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
+  }
+}
+
+Time
+Clock::now()
+{
+  Time now(0, 0, rcl_clock_.type);
+
+  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+
+  return now;
+}
+
+bool
+Clock::ros_time_is_active()
+{
+  if (!rcl_clock_valid(&rcl_clock_)) {
+    RCUTILS_LOG_ERROR("ROS time not valid!");
+    return false;
+  }
+
+  bool is_enabled;
+  auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "Failed to check ros_time_override_status");
+  }
+  return is_enabled;
+}
+
+rcl_clock_type_t
+Clock::get_clock_type()
+{
+  return rcl_clock_.type;
+}
+
+rclcpp::JumpHandler::SharedPtr
+Clock::create_jump_callback(
+  std::function<void()> pre_callback,
+  std::function<void(const TimeJump &)> post_callback,
+  JumpThreshold & threshold)
+{
+  // JumpHandler jump_callback;
+  auto jump_callback =
+    std::make_shared<rclcpp::JumpHandler>(pre_callback, post_callback, threshold);
+  {
+    std::lock_guard<std::mutex> guard(callback_list_mutex_);
+    active_jump_handlers_.push_back(jump_callback);
+  }
+  return jump_callback;
+}
+
+std::vector<JumpHandler::SharedPtr>
+Clock::get_triggered_callback_handlers(const TimeJump & jump)
+{
+  std::vector<JumpHandler::SharedPtr> callbacks;
+  std::lock_guard<std::mutex> guard(callback_list_mutex_);
+  for (auto wjcb = active_jump_handlers_.begin(); wjcb != active_jump_handlers_.end(); wjcb++) {
+    if (auto jcb = wjcb->lock()) {
+      if (jcb->notice_threshold.is_exceeded(jump)) {
+        callbacks.push_back(jcb);
+      }
+    } else {
+      active_jump_handlers_.erase(wjcb);
+    }
+  }
+  return callbacks;
+}
+
+void
+Clock::invoke_prejump_callbacks(
+  const std::vector<JumpHandler::SharedPtr> & callbacks)
+{
+  for (const auto cb : callbacks) {
+    cb->pre_callback();
+  }
+}
+
+void
+Clock::invoke_postjump_callbacks(
+  const std::vector<JumpHandler::SharedPtr> & callbacks,
+  const TimeJump & jump)
+{
+  for (auto cb : callbacks) {
+    cb->post_callback(jump);
+  }
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/duration.cpp

@@ -0,0 +1,190 @@
+// 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 <utility>
+#include <limits>
+
+#include "rclcpp/clock.hpp"
+#include "rclcpp/time.hpp"
+
+#include "builtin_interfaces/msg/duration.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace rclcpp
+{
+
+Duration::Duration(int32_t seconds, uint32_t nanoseconds)
+{
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(seconds));
+  rcl_duration_.nanoseconds += nanoseconds;
+}
+
+Duration::Duration(int64_t nanoseconds)
+{
+  rcl_duration_.nanoseconds = nanoseconds;
+}
+
+Duration::Duration(std::chrono::nanoseconds nanoseconds)
+{
+  rcl_duration_.nanoseconds = nanoseconds.count();
+}
+
+Duration::Duration(const Duration & rhs)
+{
+  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
+}
+
+Duration::Duration(
+  const builtin_interfaces::msg::Duration & duration_msg)
+{
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds += duration_msg.nanosec;
+}
+
+Duration::Duration(const rcl_duration_t & duration)
+: rcl_duration_(duration)
+{
+  // noop
+}
+
+Duration::~Duration()
+{
+}
+
+Duration::operator builtin_interfaces::msg::Duration() const
+{
+  builtin_interfaces::msg::Duration msg_duration;
+  msg_duration.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_duration_.nanoseconds));
+  msg_duration.nanosec =
+    static_cast<std::uint32_t>(rcl_duration_.nanoseconds % (1000 * 1000 * 1000));
+  return msg_duration;
+}
+
+Duration &
+Duration::operator=(const Duration & rhs)
+{
+  rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds;
+  return *this;
+}
+
+Duration &
+Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
+{
+  if (duration_msg.sec < 0) {
+    throw std::runtime_error("cannot store a negative duration point in rclcpp::Duration");
+  }
+  rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
+  rcl_duration_.nanoseconds += duration_msg.nanosec;
+  return *this;
+}
+
+bool
+Duration::operator==(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator<(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds < rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator<=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds <= rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator>=(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds >= rhs.rcl_duration_.nanoseconds;
+}
+
+bool
+Duration::operator>(const rclcpp::Duration & rhs) const
+{
+  return rcl_duration_.nanoseconds > rhs.rcl_duration_.nanoseconds;
+}
+
+void
+bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max)
+{
+  auto abs_lhs = (uint64_t)std::abs(lhsns);
+  auto abs_rhs = (uint64_t)std::abs(rhsns);
+
+  if (lhsns > 0 && rhsns > 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::overflow_error("addition leads to int64_t overflow");
+    }
+  } else if (lhsns < 0 && rhsns < 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::underflow_error("addition leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator+(const rclcpp::Duration & rhs) const
+{
+  bounds_check_duration_sum(
+    this->rcl_duration_.nanoseconds,
+    rhs.rcl_duration_.nanoseconds,
+    std::numeric_limits<rcl_duration_value_t>::max());
+  return Duration(
+    rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
+}
+
+void
+bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max)
+{
+  auto abs_lhs = (uint64_t)std::abs(lhsns);
+  auto abs_rhs = (uint64_t)std::abs(rhsns);
+
+  if (lhsns > 0 && rhsns < 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::overflow_error("duration subtraction leads to int64_t overflow");
+    }
+  } else if (lhsns < 0 && rhsns > 0) {
+    if (abs_lhs + abs_rhs > (uint64_t) max) {
+      throw std::underflow_error("duration subtraction leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator-(const rclcpp::Duration & rhs) const
+{
+  bounds_check_duration_difference(
+    this->rcl_duration_.nanoseconds,
+    rhs.rcl_duration_.nanoseconds,
+    std::numeric_limits<rcl_duration_value_t>::max());
+
+  return Duration(
+    rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
+}
+
+rcl_duration_value_t
+Duration::nanoseconds() const
+{
+  return rcl_duration_.nanoseconds;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/time.cpp

@@ -12,10 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/time.hpp"
+#include <limits>
-
 #include <utility>
 
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
+
 #include "builtin_interfaces/msg/time.hpp"
 
 #include "rcl/time.h"
@@ -27,30 +29,11 @@
 namespace
 {
 
-rcl_time_source_t
-init_time_source(rcl_time_source_type_t clock = RCL_SYSTEM_TIME)
-{
-  rcl_time_source_t time_source;
-  auto ret = rcl_time_source_init(clock, &time_source);
-
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "could not initialize time source");
-  }
-
-  return time_source;
-}
-
 rcl_time_point_t
-init_time_point(rcl_time_source_t & time_source)
+init_time_point(rcl_clock_type_t & clock_type)
 {
   rcl_time_point_t time_point;
-  auto ret = rcl_time_point_init(&time_point, &time_source);
+  time_point.clock_type = clock_type;
-
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "could not initialize time point");
-  }
 
   return time_point;
 }
@@ -60,32 +43,8 @@ init_time_point(rcl_time_source_t & time_source)
 namespace rclcpp
 {
 
-Time
+Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type)
-Time::now(rcl_time_source_type_t clock)
+: rcl_time_(init_time_point(clock_type))
-{
-  // TODO(karsten1987): This impl throws explicitely on RCL_ROS_TIME
-  // we have to do this, because rcl_time_source_init returns RCL_RET_OK
-  // for RCL_ROS_TIME, however defaults to system time under the hood.
-  // ref: https://github.com/ros2/rcl/blob/master/rcl/src/rcl/time.c#L66-L77
-  // This section can be removed when rcl supports ROS_TIME correctly.
-  if (clock == RCL_ROS_TIME) {
-    throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
-  }
-
-  Time now(0, 0, clock);
-
-  auto ret = rcl_time_point_get_now(&now.rcl_time_);
-  if (ret != RCL_RET_OK) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "could not get current time stamp");
-  }
-
-  return now;
-}
-
-Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock)
-: rcl_time_source_(init_time_source(clock)),
-  rcl_time_(init_time_point(rcl_time_source_))
 {
   if (seconds < 0) {
     throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
@@ -95,24 +54,23 @@ Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock)
   rcl_time_.nanoseconds += nanoseconds;
 }
 
-Time::Time(uint64_t nanoseconds, rcl_time_source_type_t clock)
+Time::Time(uint64_t nanoseconds, rcl_clock_type_t clock_type)
-: rcl_time_source_(init_time_source(clock)),
+: rcl_time_(init_time_point(clock_type))
-  rcl_time_(init_time_point(rcl_time_source_))
 {
   rcl_time_.nanoseconds = nanoseconds;
 }
 
 Time::Time(const Time & rhs)
-: rcl_time_source_(init_time_source(rhs.rcl_time_source_.type)),
+: rcl_time_(rhs.rcl_time_)
-  rcl_time_(init_time_point(rcl_time_source_))
 {
   rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
 }
 
-Time::Time(const builtin_interfaces::msg::Time & time_msg)  // NOLINT
+Time::Time(
-: rcl_time_source_(init_time_source(RCL_SYSTEM_TIME)),
+  const builtin_interfaces::msg::Time & time_msg,
-  rcl_time_(init_time_point(rcl_time_source_))
+  rcl_clock_type_t ros_time)
 {
+  rcl_time_ = init_time_point(ros_time);
   if (time_msg.sec < 0) {
     throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
   }
@@ -121,14 +79,14 @@ Time::Time(const builtin_interfaces::msg::Time & time_msg)  // NOLINT
   rcl_time_.nanoseconds += time_msg.nanosec;
 }
 
+Time::Time(const rcl_time_point_t & time_point)
+: rcl_time_(time_point)
+{
+  // noop
+}
+
 Time::~Time()
 {
-  if (rcl_time_source_fini(&rcl_time_source_) != RCL_RET_OK) {
-    RCUTILS_LOG_FATAL("failed to reclaim rcl_time_source_t in destructor of rclcpp::Time")
-  }
-  if (rcl_time_point_fini(&rcl_time_) != RCL_RET_OK) {
-    RCUTILS_LOG_FATAL("failed to reclaim rcl_time_point_t in destructor of rclcpp::Time")
-  }
 }
 
 Time::operator builtin_interfaces::msg::Time() const
@@ -142,9 +100,7 @@ Time::operator builtin_interfaces::msg::Time() const
 Time &
 Time::operator=(const Time & rhs)
 {
-  rcl_time_source_ = init_time_source(rhs.rcl_time_source_.type);
+  rcl_time_ = rhs.rcl_time_;
-  rcl_time_ = init_time_point(rcl_time_source_);
-  rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
   return *this;
 }
 
@@ -155,8 +111,9 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg)
     throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
   }
 
-  this->rcl_time_source_ = init_time_source();
+
-  this->rcl_time_ = init_time_point(this->rcl_time_source_);
+  rcl_clock_type_t ros_time = RCL_ROS_TIME;
+  rcl_time_ = init_time_point(ros_time);  // TODO(tfoote) hard coded ROS here
 
   rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
   rcl_time_.nanoseconds += time_msg.nanosec;
@@ -166,7 +123,7 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg)
 bool
 Time::operator==(const rclcpp::Time & rhs) const
 {
-  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
     throw std::runtime_error("can't compare times with different time sources");
   }
 
@@ -182,7 +139,7 @@ Time::operator!=(const rclcpp::Time & rhs) const
 bool
 Time::operator<(const rclcpp::Time & rhs) const
 {
-  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
     throw std::runtime_error("can't compare times with different time sources");
   }
 
@@ -192,7 +149,7 @@ Time::operator<(const rclcpp::Time & rhs) const
 bool
 Time::operator<=(const rclcpp::Time & rhs) const
 {
-  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
     throw std::runtime_error("can't compare times with different time sources");
   }
 
@@ -202,7 +159,7 @@ Time::operator<=(const rclcpp::Time & rhs) const
 bool
 Time::operator>=(const rclcpp::Time & rhs) const
 {
-  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
     throw std::runtime_error("can't compare times with different time sources");
   }
 
@@ -212,7 +169,7 @@ Time::operator>=(const rclcpp::Time & rhs) const
 bool
 Time::operator>(const rclcpp::Time & rhs) const
 {
-  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
     throw std::runtime_error("can't compare times with different time sources");
   }
 
@@ -220,33 +177,55 @@ Time::operator>(const rclcpp::Time & rhs) const
 }
 
 Time
-Time::operator+(const rclcpp::Time & rhs) const
+Time::operator+(const rclcpp::Duration & rhs) const
 {
-  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+  if (rhs.nanoseconds() > 0 && (uint64_t)rhs.nanoseconds() >
-    throw std::runtime_error("can't add times with different time sources");
+    std::numeric_limits<rcl_time_point_value_t>::max() -
-  }
+    (rcl_time_point_value_t)this->nanoseconds())
-
+  {
-  auto ns = rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds;
-  if (ns < rcl_time_.nanoseconds) {
     throw std::overflow_error("addition leads to uint64_t overflow");
   }
+  return Time(this->nanoseconds() + rhs.nanoseconds(), this->get_clock_type());
+}
 
-  return Time(rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds);
+Duration
+Time::operator-(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
+    throw std::runtime_error("can't subtract times with different time sources");
+  }
+
+  if (rcl_time_.nanoseconds >
+    (uint64_t)std::numeric_limits<rcl_duration_value_t>::max() + rhs.rcl_time_.nanoseconds)
+  {
+    throw std::underflow_error("time subtraction leads to int64_t overflow");
+  }
+
+  if (rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds) {
+    rcl_time_point_value_t negative_delta = rhs.rcl_time_.nanoseconds - rcl_time_.nanoseconds;
+    if (negative_delta > (uint64_t) std::numeric_limits<rcl_duration_value_t>::min()) {
+      throw std::underflow_error("time subtraction leads to int64_t underflow");
+    }
+  }
+
+  return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
 }
 
 Time
-Time::operator-(const rclcpp::Time & rhs) const
+Time::operator-(const rclcpp::Duration & rhs) const
 {
-  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+  if (rhs.nanoseconds() > 0 && rcl_time_.nanoseconds >
-    throw std::runtime_error("can't add times with different time sources");
+    std::numeric_limits<rcl_time_point_value_t>::max() - (uint64_t)rhs.nanoseconds())
+  {
+    throw std::underflow_error("time subtraction leads to uint64_t overflow");
+  }
+  if (rcl_time_.nanoseconds < (uint64_t) std::numeric_limits<rcl_duration_value_t>::max() &&
+    (int64_t)rcl_time_.nanoseconds < (int64_t)rhs.nanoseconds())
+  {
+    throw std::underflow_error("time subtraction leads to uint64_t underflow");
   }
 
-  auto ns = rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds;
+  return Time(rcl_time_.nanoseconds - rhs.nanoseconds(), rcl_time_.clock_type);
-  if (ns > rcl_time_.nanoseconds) {
-    throw std::underflow_error("subtraction leads to uint64_t underflow");
-  }
-
-  return Time(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
 }
 
 uint64_t
@@ -255,4 +234,22 @@ Time::nanoseconds() const
   return rcl_time_.nanoseconds;
 }
 
+rcl_clock_type_t
+Time::get_clock_type() const
+{
+  return rcl_time_.clock_type;
+}
+
+Time
+operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs)
+{
+  if (rhs.nanoseconds() >
+    std::numeric_limits<rcl_time_point_value_t>::max() - (rcl_time_point_value_t)lhs.nanoseconds())
+  {
+    throw std::overflow_error("addition leads to uint64_t overflow");
+  }
+  return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type());
+}
+
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/time_source.cpp

@@ -0,0 +1,262 @@
+// 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 <memory>
+#include <utility>
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rcl/time.h"
+
+#include "rcutils/logging_macros.h"
+
+#include "rclcpp/clock.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/time_source.hpp"
+
+
+namespace rclcpp
+{
+
+TimeSource::TimeSource(std::shared_ptr<rclcpp::node::Node> node)
+: ros_time_active_(false)
+{
+  this->attachNode(node);
+}
+
+TimeSource::TimeSource()
+: ros_time_active_(false)
+{
+}
+
+void TimeSource::attachNode(std::shared_ptr<rclcpp::node::Node> 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);
+
+  parameter_client_ = std::make_shared<rclcpp::parameter_client::AsyncParametersClient>(node);
+  parameter_subscription_ =
+    parameter_client_->on_parameter_event(std::bind(&TimeSource::on_parameter_event,
+      this, std::placeholders::_1));
+}
+
+void TimeSource::detachNode()
+{
+  this->ros_time_active_ = false;
+  clock_subscription_.reset();
+  parameter_client_.reset();
+  node_.reset();
+  disable_ros_time();
+}
+
+void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
+{
+  if (clock->get_clock_type() != RCL_ROS_TIME) {
+    throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS 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
+  if (last_msg_set_) {
+    set_clock(last_msg_set_, ros_time_active_, clock);
+  }
+}
+
+void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
+{
+  std::lock_guard<std::mutex> guard(clock_list_lock_);
+  auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock);
+  if (result != associated_clocks_.end()) {
+    associated_clocks_.erase(result);
+  } else {
+    RCUTILS_LOG_ERROR("Failed to remove clock");
+  }
+}
+
+TimeSource::~TimeSource()
+{
+  if (node_) {
+    this->detachNode();
+  }
+}
+
+void TimeSource::set_clock(
+  const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
+  std::shared_ptr<rclcpp::Clock> clock)
+{
+  // Compute diff
+  rclcpp::Time msg_time = rclcpp::Time(*msg);
+  rclcpp::Time now = clock->now();
+  auto diff = now - msg_time;
+  rclcpp::TimeJump jump;
+  jump.delta_.nanoseconds = diff.nanoseconds();
+
+  // Compute jump type
+  if (clock->ros_time_is_active()) {
+    if (set_ros_time_enabled) {
+      jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE;
+    } else {
+      jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED;
+    }
+  } else if (!clock->ros_time_is_active()) {
+    if (set_ros_time_enabled) {
+      jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_ACTIVATED;
+    } else {
+      jump.jump_type_ = TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE;
+    }
+  }
+
+  if (jump.jump_type_ == TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE) {
+    // No change/no updates don't act.
+    return;
+  }
+
+  auto active_callbacks = clock->get_triggered_callback_handlers(jump);
+  clock->invoke_prejump_callbacks(active_callbacks);
+
+  // Do change
+  if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED) {
+    disable_ros_time(clock);
+  } else if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED) {
+    enable_ros_time(clock);
+  }
+
+  if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED ||
+    jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE)
+  {
+    auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), msg_time.nanoseconds());
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "Failed to set ros_time_override_status");
+    }
+  }
+  // Post change callbacks
+  clock->invoke_postjump_callbacks(active_callbacks, jump);
+}
+
+void TimeSource::clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg)
+{
+  if (!this->ros_time_active_) {
+    enable_ros_time();
+  }
+  // Cache the last message in case a new clock is attached.
+  last_msg_set_ = msg;
+
+  std::lock_guard<std::mutex> guard(clock_list_lock_);
+  for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
+    set_clock(msg, true, *it);
+  }
+}
+
+void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
+{
+  for (auto & new_parameter : event->new_parameters) {
+    if (new_parameter.value.type == rclcpp::parameter::ParameterType::PARAMETER_BOOL) {
+      if (new_parameter.name == "use_sim_time") {
+        bool value = new_parameter.value.bool_value;
+        if (value) {
+          parameter_state_ = SET_TRUE;
+          enable_ros_time();
+        } else {
+          parameter_state_ = SET_FALSE;
+          disable_ros_time();
+        }
+      }
+    }
+  }
+  for (auto & changed_parameter : event->changed_parameters) {
+    if (changed_parameter.value.type == rclcpp::parameter::ParameterType::PARAMETER_BOOL) {
+      if (changed_parameter.name == "use_sim_time") {
+        bool value = changed_parameter.value.bool_value;
+        if (value) {
+          parameter_state_ = SET_TRUE;
+          enable_ros_time();
+        } else {
+          parameter_state_ = SET_FALSE;
+          disable_ros_time();
+        }
+      }
+    }
+  }
+  for (auto & deleted_parameter : event->deleted_parameters) {
+    if (deleted_parameter.name == "use_sim_time") {
+      // If the parameter is deleted mark it as unset but dont' change state.
+      parameter_state_ = UNSET;
+    }
+  }
+}
+
+void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
+{
+  auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "Failed to enable ros_time_override_status");
+  }
+}
+
+void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
+{
+  auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "Failed to enable ros_time_override_status");
+  }
+}
+
+void TimeSource::enable_ros_time()
+{
+  if (ros_time_active_) {
+    // already enabled no-op
+    return;
+  }
+
+  // Local storage
+  ros_time_active_ = true;
+
+  // Update all attached clocks
+  std::lock_guard<std::mutex> guard(clock_list_lock_);
+  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);
+  }
+}
+
+void TimeSource::disable_ros_time()
+{
+  if (!ros_time_active_) {
+    // already disabled no-op
+    return;
+  }
+
+  // Local storage
+  ros_time_active_ = false;
+
+  // Update all attached clocks
+  std::lock_guard<std::mutex> guard(clock_list_lock_);
+  for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
+    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+    set_clock(msg, false, *it);
+  }
+}
+
+}  // namespace rclcpp

rclcpp/test/test_duration.cpp

@@ -0,0 +1,88 @@
+// 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 <gtest/gtest.h>
+
+#include <algorithm>
+#include <chrono>
+#include <limits>
+#include <string>
+
+#include "rcl/error_handling.h"
+#include "rcl/time.h"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/duration.hpp"
+
+
+using namespace std::chrono_literals;
+
+class TestDuration : public ::testing::Test
+{
+};
+
+// TEST(TestDuration, conversions) {
+// TODO(tfoote) Implement conversion methods
+// }
+
+TEST(TestDuration, operators) {
+  rclcpp::Duration old(1, 0);
+  rclcpp::Duration young(2, 0);
+
+  EXPECT_TRUE(old < young);
+  EXPECT_TRUE(young > old);
+  EXPECT_TRUE(old <= young);
+  EXPECT_TRUE(young >= old);
+  EXPECT_FALSE(young == old);
+
+  rclcpp::Duration add = old + young;
+  EXPECT_EQ(add.nanoseconds(), (rcl_duration_value_t)(old.nanoseconds() + young.nanoseconds()));
+  EXPECT_EQ(add, old + young);
+
+  rclcpp::Duration sub = young - old;
+  EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds()));
+  EXPECT_EQ(sub, young - old);
+
+  rclcpp::Duration time = rclcpp::Duration(0, 0);
+  rclcpp::Duration copy_constructor_duration(time);
+  rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0);
+  assignment_op_duration = time;
+
+  EXPECT_TRUE(time == copy_constructor_duration);
+  EXPECT_TRUE(time == assignment_op_duration);
+}
+
+TEST(TestDuration, chrono_overloads) {
+  int64_t ns = 123456789l;
+  auto chrono_ns = std::chrono::nanoseconds(ns);
+  auto d1 = rclcpp::Duration(ns);
+  auto d2 = rclcpp::Duration(chrono_ns);
+  auto d3 = rclcpp::Duration(123456789ns);
+  EXPECT_EQ(d1, d2);
+  EXPECT_EQ(d1, d3);
+  EXPECT_EQ(d2, d3);
+}
+
+TEST(TestDuration, overflows) {
+  rclcpp::Duration max(std::numeric_limits<rcl_duration_value_t>::max());
+  rclcpp::Duration min(std::numeric_limits<rcl_duration_value_t>::min());
+
+  rclcpp::Duration one(1);
+  rclcpp::Duration negative_one(-1);
+
+  EXPECT_THROW(max + one, std::overflow_error);
+  EXPECT_THROW(min - one, std::underflow_error);
+  EXPECT_THROW(negative_one + min, std::underflow_error);
+  EXPECT_THROW(negative_one - max, std::underflow_error);
+}

rclcpp/test/test_time.cpp

@@ -20,6 +20,7 @@
 
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
+#include "rclcpp/clock.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/time.hpp"
 
@@ -32,27 +33,39 @@ protected:
   }
 };
 
+TEST(TestTime, clock_type_access) {
+  rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  EXPECT_EQ(RCL_ROS_TIME, ros_clock.get_clock_type());
+
+  rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
+  EXPECT_EQ(RCL_SYSTEM_TIME, system_clock.get_clock_type());
+
+  rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+  EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type());
+}
+
 TEST(TestTime, time_sources) {
   using builtin_interfaces::msg::Time;
-  // TODO(Karsten1987): Fix this test once ROS_TIME is implemented
+  rclcpp::Clock ros_clock(RCL_ROS_TIME);
-  EXPECT_ANY_THROW(rclcpp::Time::now(RCL_ROS_TIME));
+  Time ros_now = ros_clock.now();
+  EXPECT_NE(0, ros_now.sec);
+  EXPECT_NE(0u, ros_now.nanosec);
 
-  Time system_now = rclcpp::Time::now(RCL_SYSTEM_TIME);
+  rclcpp::Clock system_clock(RCL_ROS_TIME);
+  Time system_now = system_clock.now();
   EXPECT_NE(0, system_now.sec);
   EXPECT_NE(0u, system_now.nanosec);
 
-  Time steady_now = rclcpp::Time::now(RCL_STEADY_TIME);
+  rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+  Time steady_now = steady_clock.now();
   EXPECT_NE(0, steady_now.sec);
   EXPECT_NE(0u, steady_now.nanosec);
-
-  // default
-  Time default_now = rclcpp::Time::now();
-  EXPECT_NE(0, default_now.sec);
-  EXPECT_NE(0u, default_now.nanosec);
 }
 
 TEST(TestTime, conversions) {
-  rclcpp::Time now = rclcpp::Time::now();
+  rclcpp::Clock system_clock(RCL_ROS_TIME);
+
+  rclcpp::Time now = system_clock.now();
   builtin_interfaces::msg::Time now_msg = now;
 
   rclcpp::Time now_again = now_msg;
@@ -95,12 +108,8 @@ TEST(TestTime, operators) {
   EXPECT_FALSE(young == old);
   EXPECT_TRUE(young != old);
 
-  rclcpp::Time add = old + young;
+  rclcpp::Duration sub = young - old;
-  EXPECT_EQ(add.nanoseconds(), old.nanoseconds() + young.nanoseconds());
+  EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds()));
-  EXPECT_EQ(add, old + young);
-
-  rclcpp::Time sub = young - old;
-  EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds());
   EXPECT_EQ(sub, young - old);
 
   rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME);
@@ -112,11 +121,13 @@ TEST(TestTime, operators) {
   EXPECT_ANY_THROW((void)(system_time >= steady_time));
   EXPECT_ANY_THROW((void)(system_time < steady_time));
   EXPECT_ANY_THROW((void)(system_time > steady_time));
-  EXPECT_ANY_THROW((void)(system_time + steady_time));
   EXPECT_ANY_THROW((void)(system_time - steady_time));
 
-  rclcpp::Time now = rclcpp::Time::now(RCL_SYSTEM_TIME);
+  rclcpp::Clock system_clock(RCL_ROS_TIME);
-  rclcpp::Time later = rclcpp::Time::now(RCL_STEADY_TIME);
+  rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+
+  rclcpp::Time now = system_clock.now();
+  rclcpp::Time later = steady_clock.now();
 
   EXPECT_ANY_THROW((void)(now == later));
   EXPECT_ANY_THROW((void)(now != later));
@@ -124,7 +135,6 @@ TEST(TestTime, operators) {
   EXPECT_ANY_THROW((void)(now >= later));
   EXPECT_ANY_THROW((void)(now < later));
   EXPECT_ANY_THROW((void)(now > later));
-  EXPECT_ANY_THROW((void)(now + later));
   EXPECT_ANY_THROW((void)(now - later));
 
   for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME}) {
@@ -139,9 +149,10 @@ TEST(TestTime, operators) {
 }
 
 TEST(TestTime, overflows) {
-  rclcpp::Time max(std::numeric_limits<uint64_t>::max());
+  rclcpp::Time max_time(std::numeric_limits<rcl_time_point_value_t>::max());
-  rclcpp::Time one(1);
+  rclcpp::Time min_time(std::numeric_limits<rcl_time_point_value_t>::min());
+  rclcpp::Duration one(1);
 
-  EXPECT_THROW(max + one, std::overflow_error);
+  EXPECT_THROW(max_time + one, std::overflow_error);
-  EXPECT_THROW(one - max, std::underflow_error);
+  EXPECT_THROW(min_time - one, std::underflow_error);
 }

rclcpp/test/test_time_source.cpp

@@ -0,0 +1,274 @@
+// 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 <gtest/gtest.h>
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <string>
+
+#include "rcl/error_handling.h"
+#include "rcl/time.h"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/time_source.hpp"
+
+class TestTimeSource : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::node::Node>("my_node");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::node::Node::SharedPtr node;
+};
+
+
+TEST_F(TestTimeSource, detachUnattached) {
+  rclcpp::TimeSource ts;
+
+  ASSERT_NO_THROW(ts.detachNode());
+
+  // Try multiple detach to see if error
+  ASSERT_NO_THROW(ts.detachNode());
+}
+
+TEST_F(TestTimeSource, reattach) {
+  rclcpp::TimeSource ts;
+  // Try reattach
+  ASSERT_NO_THROW(ts.attachNode(node));
+  ASSERT_NO_THROW(ts.attachNode(node));
+}
+
+TEST_F(TestTimeSource, ROS_time_valid) {
+  rclcpp::TimeSource ts;
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+  ts.attachClock(ros_clock);
+  auto now = ros_clock->now();
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachNode(node);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.detachNode();
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachNode(node);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+}
+
+TEST_F(TestTimeSource, clock) {
+  rclcpp::TimeSource ts(node);
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
+      rmw_qos_profile_default);
+  rclcpp::WallRate loop_rate(50);
+  for (int i = 0; i < 5; ++i) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+    msg->sec = i;
+    msg->nanosec = 1000;
+    clock_pub->publish(msg);
+    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
+  }
+  auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
+  auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
+
+  // Now that we've recieved a message it should be active with parameter unset
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  auto t_out = ros_clock->now();
+
+  EXPECT_NE(0UL, t_out.nanoseconds());
+  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
+  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
+}
+
+class CallbackObject
+{
+public:
+  CallbackObject()
+  : last_precallback_id_(0),
+    last_postcallback_id_(0)
+  {}
+  int last_precallback_id_;
+  void pre_callback(int id) {last_precallback_id_ = id;}
+
+  int last_postcallback_id_;
+  rclcpp::TimeJump last_timejump_;
+  void post_callback(const rclcpp::TimeJump & jump, int id)
+  {
+    last_postcallback_id_ = id; last_timejump_ = jump;
+  }
+};
+
+TEST_F(TestTimeSource, callbacks) {
+  CallbackObject cbo;
+  rclcpp::JumpThreshold jump_threshold;
+  jump_threshold.min_forward_ = 0;
+  jump_threshold.min_backward_ = 0;
+  jump_threshold.on_clock_change_ = true;
+
+  rclcpp::TimeSource ts(node);
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  // Register a callback for time jumps
+  rclcpp::JumpHandler::SharedPtr callback_holder = ros_clock->create_jump_callback(
+    std::bind(&CallbackObject::pre_callback, &cbo, 1),
+    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1),
+    jump_threshold);
+
+  EXPECT_EQ(0, cbo.last_precallback_id_);
+  EXPECT_EQ(0, cbo.last_postcallback_id_);
+
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
+      rmw_qos_profile_default);
+
+  rclcpp::WallRate loop_rate(50);
+  for (int i = 0; i < 5; ++i) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+    msg->sec = i;
+    msg->nanosec = 1000;
+    clock_pub->publish(msg);
+    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
+  }
+  auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
+  auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
+
+  EXPECT_EQ(1, cbo.last_precallback_id_);
+  EXPECT_EQ(1, cbo.last_postcallback_id_);
+
+  // Now that we've recieved a message it should be active with parameter unset
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  auto t_out = ros_clock->now();
+
+  EXPECT_NE(0UL, t_out.nanoseconds());
+  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
+  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
+
+
+  // Change callbacks
+  callback_holder = ros_clock->create_jump_callback(
+    std::bind(&CallbackObject::pre_callback, &cbo, 2),
+    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2),
+    jump_threshold);
+
+  for (int i = 0; i < 5; ++i) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+    msg->sec = i;
+    msg->nanosec = 2000;
+    clock_pub->publish(msg);
+    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
+  }
+
+  EXPECT_EQ(2, cbo.last_precallback_id_);
+  EXPECT_EQ(2, cbo.last_postcallback_id_);
+
+  // Now that we've recieved a message it should be active with parameter unset
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  t_out = ros_clock->now();
+
+  EXPECT_NE(0UL, t_out.nanoseconds());
+  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
+  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
+}
+
+
+TEST_F(TestTimeSource, parameter_activation) {
+  rclcpp::TimeSource ts(node);
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  auto parameter_service = std::make_shared<rclcpp::parameter_service::ParameterService>(node);
+  auto parameters_client = std::make_shared<rclcpp::parameter_client::SyncParametersClient>(node);
+
+  using namespace std::chrono_literals;
+  EXPECT_TRUE(parameters_client->wait_for_service(2s));
+  auto set_parameters_results = parameters_client->set_parameters({
+    rclcpp::parameter::ParameterVariant("use_sim_time", true)
+  });
+  for (auto & result : set_parameters_results) {
+    EXPECT_TRUE(result.successful);
+  }
+  rclcpp::spin_some(node);
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+
+  set_parameters_results = parameters_client->set_parameters({
+    rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET)
+  });
+  for (auto & result : set_parameters_results) {
+    EXPECT_TRUE(result.successful);
+  }
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  set_parameters_results = parameters_client->set_parameters({
+    rclcpp::parameter::ParameterVariant("use_sim_time", false)
+  });
+  for (auto & result : set_parameters_results) {
+    EXPECT_TRUE(result.successful);
+  }
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  set_parameters_results = parameters_client->set_parameters({
+    rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET)
+  });
+  for (auto & result : set_parameters_results) {
+    EXPECT_TRUE(result.successful);
+  }
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+}