Add the core functionality for ros time support.

This is as defined in http://design.ros2.org/articles/clock_and_time.html

This has the core functionality implemented in rcl.
It will need to be extended into each client library.
There are also areas to fill in for more support for duration, rates, and timers.

rcl/include/rcl/time.h

@@ -28,52 +28,376 @@ extern "C"
 #define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000 * 1000)
 #define RCL_US_TO_NS(microseconds) (microseconds * 1000)
 
-#define RCL_NS_TO_S(nanoseconds) (nanoseconds / 1000 * 1000 * 1000)
+#define RCL_NS_TO_S(nanoseconds) (nanoseconds / (1000 * 1000 * 1000))
-#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000 * 1000)
+#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / (1000 * 1000))
 #define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000)
 
-/// Struct which encapsulates a point in time according to a system clock.
-/* The system clock's point of reference is the Unix Epoch (January 1st, 1970).
- * See: http://stackoverflow.com/a/32189845/671658
- * Therefore all times in this struct are positive and count the nanoseconds
- * since the Unix epoch and this struct cannot be used on systems which report
- * a current time before the Unix Epoch.
- * Leap seconds are not considered.
- *
- * The struct represents time as nanoseconds in an unsigned 64-bit integer.
- * The struct is capable of representing any time until the year 2554 with
- * nanosecond precisions.
- */
-typedef struct rcl_system_time_point_t
-{
-  uint64_t nanoseconds;
-} rcl_system_time_point_t;
 
-/// Struct which encapsulates a point in time according to a steady clock.
+enum rcl_time_source_type_t
-/* The steady clock's point of reference is system defined, but often the time
- * that the process started plus a signed random offset.
- * See: http://stackoverflow.com/a/32189845/671658
- * However, the clock is guaranteed to be monotonically increasing.
- * Therefore all times in this struct are positive and count nanoseconds
- * since an unspecified, but constant point in time.
- *
- * The struct represents time as nanoseconds in an unsigned 64-bit integer.
- */
-typedef struct rcl_steady_time_point_t
 {
-  uint64_t nanoseconds;
+  RCL_TIME_SOURCE_UNINITIALIZED = 0,
-} rcl_steady_time_point_t;
+  RCL_ROS_TIME,
+  RCL_SYSTEM_TIME,
+  RCL_STEADY_TIME
+};
+
+typedef uint64_t rcl_time_point_value_t;
+typedef int64_t rcl_duration_value_t;
+
+typedef struct rcl_time_source_t
+{
+  enum rcl_time_source_type_t type;
+  void (* pre_update)(void);
+  void (* post_update)(void);
+  rcl_ret_t (* get_now)(void * data, rcl_time_point_value_t * now);
+  // void (*set_now) (rcl_time_point_value_t);
+  void * data;
+} rcl_time_source_t;
+
+struct rcl_ros_time_source_storage_t;
+typedef struct rcl_time_point_t
+{
+  rcl_time_point_value_t nanoseconds;
+  rcl_time_source_t * time_source;
+} rcl_time_point_t;
 
-/// Struct which encapsulates a duration of time between two points in time.
-/* The struct can represent any time within the range [~292 years, ~-292 years]
- * with nanosecond precision.
- */
 typedef struct rcl_duration_t
 {
-  int64_t nanoseconds;
+  rcl_duration_value_t nanoseconds;
+  rcl_time_source_t * time_source;
 } rcl_duration_t;
 
-/// Retrieve the current time as a rcl_system_time_point_t struct.
+// typedef struct rcl_rate_t
+// {
+//   rcl_time_point_value_t trigger_time;
+//   int64_t period;
+//   rcl_time_source_t * time_source;
+// } rcl_rate_t;
+// TODO(tfoote) integrate rate and timer implementations
+
+/// Check if the time_source has valid values.
+/* This function returns true if the time source appears to be valid.
+* It will check that the type is not uninitialized, and that pointers
+* are not invalid. Note that if data is uninitialized it may give a
+* false positive.
+*
+* \param[in] time_source the handle to the time_source which is being queried
+* \return true if the source is believed to be valid, otherwise return false.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+bool rcl_time_source_valid(rcl_time_source_t * time_source);
+
+/// Initialize a timesource as a RCL_ROS_TIME time source.
+/* This will allocate all necessary internal structures, and initialize variables.
+* It is specifically setting up a RCL_ROS_TIME time source.
+*
+* \param[in] time_source the handle to the time_source which is being initialized
+* \return RCL_RET_OK if the time source was successfully initialized, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_init_ros_time_source(rcl_time_source_t * time_source);
+
+/// Finalize a timesource as a RCL_ROS_TIME time source.
+/* This will deallocate all necessary internal structures, and clean up any variables.
+* It is specifically setting up a RCL_ROS_TIME time source. It is expected to be paired with
+* the init fuction.
+*
+* \param[in] time_source the handle to the time_source which is being initialized
+* \return RCL_RET_OK if the time source was successfully finalized, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_fini_ros_time_source(rcl_time_source_t * time_source);
+
+/// Initialize a timesource as a RCL_STEADY_TIME time source.
+/* This will allocate all necessary internal structures, and initialize variables.
+* It is specifically setting up a RCL_STEADY_TIME time source.
+*
+* \param[in] time_source the handle to the time_source which is being initialized
+* \return RCL_RET_OK if the time source was successfully initialized, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_init_steady_time_source(rcl_time_source_t * time_source);
+
+/// Finalize a timesource as a RCL_STEADY_TIME time source.
+/* Finalize the timesource as a RCL_STEADY_TIME time source.
+*
+* This will deallocate all necessary internal structures, and clean up any variables.
+* It is specifically setting up a steady time source. It is expected to be paired with
+* the init fuction.
+*
+* \param[in] time_source the handle to the time_source which is being initialized
+* \return RCL_RET_OK if the time source was successfully finalized, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_fini_steady_time_source(rcl_time_source_t * time_source);
+
+/// Initialize a timesource as a RCL_SYSTEM_TIME time source.
+/* Initialize the timesource as a RCL_SYSTEM_TIME time source.
+*
+* This will allocate all necessary internal structures, and initialize variables.
+* It is specifically setting up a system time source.
+*
+* \param[in] time_source the handle to the time_source which is being initialized
+* \return RCL_RET_OK if the time source was successfully initialized, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_init_system_time_source(rcl_time_source_t * time_source);
+
+/// Finalize a timesource as a RCL_SYSTEM_TIME time source.
+/* Finalize the timesource as a RCL_SYSTEM_TIME time source.
+*
+* This will deallocate all necessary internal structures, and clean up any variables.
+* It is specifically setting up a system time source. It is expected to be paired with
+* the init fuction.
+*
+* \param[in] time_source the handle to the time_source which is being initialized.
+* \return RCL_RET_OK if the time source was successfully finalized, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_fini_system_time_source(rcl_time_source_t * time_source);
+
+/// Initialize a time point using the time_source.
+/* This function will initialize the time_point using the time_source
+* as a reference.
+* If the time_source is null it will use the system default time_source.
+*
+* This will allocate all necessary internal structures, and initialize variables.
+* The time_source may be of types RCL_ROS_TIME, RCL_STEADY_TIME, or RCL_SYSTEM_TIME.
+*
+* \param[in] time_point the handle to the time_source which is being initialized.
+* \param[in] time_source the handle to the time_source will be used for reference.
+* \return RCL_RET_OK if the last call time was retrieved successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source);
+
+/// Finalize a time_point
+/* Finalize the time_point such that it is ready for deallocation.
+*
+* This will deallocate all necessary internal structures, and clean up any variables.
+* *
+* \param[in] time_point the handle to the time_source which is being finalized.
+* \return RCL_RET_OK if the last call time was retrieved successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_fini_time_point(rcl_time_point_t * time_point);
+
+/// Initialize a duration using the time_source.
+/* This function will initialize the duration using the time_source
+* as a reference.
+* If the time_source is null it will use the system default time_source.
+*
+* This will allocate all necessary internal structures, and initialize variables.
+* The time_source may be of types ros, steady, or system.
+*
+* \param[in] duration the handle to the duration which is being initialized.
+* \param[in] time_source the handle to the time_source will be used for reference.
+* \return RCL_RET_OK if the last call time was retrieved successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source);
+
+/// Finalize a duration
+/* Finalize the duration such that it is ready for deallocation.
+*
+* This will deallocate all necessary internal structures, and clean up any variables.
+* *
+* \param[in] duration the handle to the duration which is being finalized.
+* \return RCL_RET_OK if the last call time was retrieved successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_fini_duration(rcl_duration_t * duration);
+
+/// Get the default RCL_ROS_TIME time source
+/* This function will get the process default time source.
+* This time source is specifically of the ROS time abstraction,
+* and may be overridden by updates.
+*
+* If the default has not yet been used it will allocate
+* and initialize the time source.
+*
+* \return rcl_time_source_t if it successfully found or allocated a
+          time source. If an error occurred it will return NULL.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_time_source_t * rcl_get_default_ros_time_source(void);
+
+/// Get the default RCL_STEADY_TIME time source
+/* This function will get the process default time source.
+* This time source is specifically of the steady time abstraction,
+* it should not be able to be overridden..
+*
+* If the default has not yet been used it will allocate
+* and initialize the time source.
+*
+* \return rcl_time_source_t if it successfully found or allocated a
+          time source. If an error occurred it will return NULL.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_time_source_t * rcl_get_default_steady_time_source(void);
+
+/// Get the default RCL_SYSTEM_TIME time source
+/* This function will get the process default time source.
+* This time source is specifically of the system time abstraction,
+* and may be overridden by updates to the system clock.
+*
+* If the default has not yet been used it will allocate
+* and initialize the time source.
+*
+* \return rcl_time_source_t if it successfully found or allocated a
+          time source. If an error occurred it will return NULL.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_time_source_t * rcl_get_default_system_time_source(void);
+
+/// Set the current time on the RCL_ROS_TIME time source
+/* This function is used to set the time on a ros time source.
+* It will error if passed a differnt time source.
+*
+* This should not block, except on Windows. One caveat is that
+* if the ros time abstraction is active, it will invoke the user
+* defined callbacks, for pre and post update notifications. The
+* calbacks are supposed to be short running and non-blocking.
+*
+* \param[in] process_time_source The time source on which to set the value.
+* \return RCL_RET_OK if the value was set successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source);
+
+/// Compute the difference between two time points
+/* This function takes two time points and computes the duration between them.
+* The two time points must be using the same time abstraction..
+*  And the resultant duration will also be of the same abstraction.
+*
+* The value will be computed as duration = finish - start. If start is after
+* finish the duration will be negative.
+*
+* \param[in] start The time point for the start of the duration.
+* \param[in] finish The time point for the end of the duration.
+* \param[out] delta The duration between the start and finish.
+* \return RCL_RET_OK if the difference was computed successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish,
+  rcl_duration_t * delta);
+
+/// Fill the time point with the current value of the associated clock.
+/* This function will populate the data of the time_point object with the
+* current value from it's associated time abstraction.
+*
+* \param[out] time_point The time_point to populate.
+* \return RCL_RET_OK if the last call time was retrieved successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_get_time_point_now(rcl_time_point_t * time_point);
+
+
+/// Enable the ros time abstraction override.
+/* This method will enable the ros time abstraction override values,
+* such that the time source will report the set value instead of falling
+* back to system time.
+*
+* \param[in] time_source The time_source to enable.
+* \return RCL_RET_OK if the time source was enabled successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_enable_ros_time_override(rcl_time_source_t * time_source);
+
+/// Disable the ros time abstraction override.
+/* This method will disable the RCL_ROS_TIME time abstraction override values,
+* such that the time source will report the system time even if a custom
+* value has been set.
+*
+* \param[in] time_source The time_source to disable.
+* \return RCL_RET_OK if the time source was disabled successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_disable_ros_time_override(rcl_time_source_t * time_source);
+
+
+/// Check if the RCL_ROS_TIME time source has the override enabled.
+/* This will populate the is_enabled object to indicate if the
+* time overide is enabled. If it is enabled, the set value will be returned.
+* Otherwise this time source will return the equivalent to system time abstraction.
+*
+* \param[in] time_source The time_source to query.
+* \param[out] is_enabled Whether the override is enabled..
+* \return RCL_RET_OK if the time source was queried successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_is_enabled_ros_time_override(rcl_time_source_t * time_source,
+  bool * is_enabled);
+
+/// Set the current time for this RCL_ROS_TIME time source.
+/* This function will update the internal storage for the RCL_ROS_TIME time source.
+* If queried and override enabled the time source will return this value,
+* otherwise it will return the system time.
+*
+* \param[in] time_source The time_source to update.
+* \param[in] time_value The new current time.
+* \return RCL_RET_OK if the time source was set successfully, or
+*         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+*         RCL_RET_ERROR an unspecified error occur.
+*/
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t rcl_set_ros_time_override(rcl_time_source_t * time_source,
+  rcl_time_point_value_t time_value);
+
+/// Retrieve the current time as a rcl_time_point_value_t (an alias for unint64_t).
 /* This function returns the time from a system clock.
  * The closest equivalent would be to std::chrono::system_clock::now();
  *
@@ -88,7 +412,10 @@ typedef struct rcl_duration_t
  * On Windows this is lock-free if the C11's stdatomic.h function
  * atomic_is_lock_free() returns true for atomic_int_least64_t.
  *
- * \param[out] now a struct in which the current time is stored
+ * TODO(tfoote) I talked with @wjwwood about possibly promoting this
+ *  method into rmw for more reuse.
+ *
+ * \param[out] now a datafield in which the current time is stored
  * \return RCL_RET_OK if the current time was successfully obtained, or
  *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
  *         RCL_RET_ERROR an unspecified error occur.
@@ -96,15 +423,15 @@ typedef struct rcl_duration_t
 RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
-rcl_system_time_point_now(rcl_system_time_point_t * now);
+rcl_system_time_now(rcl_time_point_value_t * now);
 
-/// Retrieve the current time as a rcl_steady_time_point_t struct.
+/// Retrieve the current time as a rcl_time_point_value_t object..
 /* This function returns the time from a monotonically increasing clock.
  * The closest equivalent would be to std::chrono::steady_clock::now();
  *
  * The resolution (e.g. nanoseconds vs microseconds) is not guaranteed.
  *
- * The now argument must point to an allocated rcl_steady_time_point_t struct,
+ * The now argument must point to an allocated rcl_time_point_value_t object,
  * as the result is copied into this variable.
  *
  * This function may allocate heap memory when an error occurs.
@@ -113,6 +440,9 @@ rcl_system_time_point_now(rcl_system_time_point_t * now);
  * On Windows this is lock-free if the C11's stdatomic.h function
  * atomic_is_lock_free() returns true for atomic_int_least64_t.
  *
+ * TODO(tfoote) I talked with @wjwwood about possibly promoting this
+ *  method into rmw for more reuse.
+ *
  * \param[out] now a struct in which the current time is stored
  * \return RCL_RET_OK if the current time was successfully obtained, or
  *         RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
@@ -121,7 +451,7 @@ rcl_system_time_point_now(rcl_system_time_point_t * now);
 RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
-rcl_steady_time_point_now(rcl_steady_time_point_t * now);
+rcl_steady_time_now(rcl_time_point_value_t * now);
 
 #if __cplusplus
 }

rcl/src/rcl/time.c

@@ -12,8 +12,315 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <stdlib.h>
+
 #if defined(WIN32)
 #include "./time_win32.c"
 #else  // defined(WIN32)
 #include "./time_unix.c"
 #endif  // defined(WIN32)
+
+#include "./stdatomic_helper.h"
+
+// Process default ROS time sources
+static rcl_time_source_t * rcl_default_ros_time_source;
+static rcl_time_source_t * rcl_default_steady_time_source;
+static rcl_time_source_t * rcl_default_system_time_source;
+
+
+// Internal storage for RCL_ROS_TIME implementation
+typedef struct
+{
+  atomic_uint_least64_t current_time;
+  bool active;
+  // TODO(tfoote): store subscription here
+} rcl_ros_time_source_storage_t;
+
+// Implementation only
+rcl_ret_t rcl_get_steady_time(void * data, rcl_time_point_value_t * current_time)
+{
+  (void) data;  // unused
+  return rcl_steady_time_now(current_time);
+}
+
+// Implementation only
+rcl_ret_t rcl_get_system_time(void * data, rcl_time_point_value_t * current_time)
+{
+  (void) data;  // unused
+  return rcl_system_time_now(current_time);
+}
+
+// Internal method for zeroing values on init, assumes time_source is valid
+void rcl_init_generic_time_source(rcl_time_source_t * time_source)
+{
+  time_source->type = RCL_TIME_SOURCE_UNINITIALIZED;
+  time_source->pre_update = NULL;
+  time_source->post_update = NULL;
+  time_source->get_now = NULL;
+  time_source->data = NULL;
+}
+
+// The function used to get the current ros time.
+// This is in the implementation only
+rcl_ret_t rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time)
+{
+  rcl_ros_time_source_storage_t * t = (rcl_ros_time_source_storage_t *)data;
+  if (!t->active) {
+    return rcl_get_system_time(data, current_time);
+  }
+  *current_time = rcl_atomic_load_uint64_t(&(t->current_time));
+  return RCL_RET_OK;
+}
+
+bool rcl_time_source_valid(rcl_time_source_t * time_source)
+{
+  if (time_source == NULL ||
+    time_source->type == RCL_TIME_SOURCE_UNINITIALIZED ||
+    time_source->get_now == NULL)
+  {
+    return false;
+  }
+  return true;
+}
+
+rcl_ret_t rcl_init_ros_time_source(rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  rcl_init_generic_time_source(time_source);
+  time_source->data = calloc(1, sizeof(rcl_ros_time_source_storage_t));
+  time_source->get_now = rcl_get_ros_time;
+  time_source->type = RCL_ROS_TIME;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_fini_ros_time_source(rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  if (time_source->type != RCL_ROS_TIME) {
+    RCL_SET_ERROR_MSG("time_source not of type RCL_ROS_TIME");
+    return RCL_RET_ERROR;
+  }
+  free((rcl_ros_time_source_storage_t *)time_source->data);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_init_steady_time_source(rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  rcl_init_generic_time_source(time_source);
+  time_source->get_now = rcl_get_steady_time;
+  time_source->type = RCL_STEADY_TIME;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_fini_steady_time_source(rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  if (time_source->type != RCL_STEADY_TIME) {
+    RCL_SET_ERROR_MSG("time_source not of type RCL_STEADY_TIME");
+    return RCL_RET_ERROR;
+  }
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_init_system_time_source(rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  rcl_init_generic_time_source(time_source);
+  time_source->get_now = rcl_get_system_time;
+  time_source->type = RCL_SYSTEM_TIME;
+  if (!time_source) {
+    return RCL_RET_ERROR;
+  }
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_fini_system_time_source(rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  if (time_source->type != RCL_SYSTEM_TIME) {
+    RCL_SET_ERROR_MSG("time_source not of type RCL_SYSTEM_TIME");
+    return RCL_RET_ERROR;
+  }
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT);
+  if (!time_source) {
+    time_point->time_source = rcl_get_default_ros_time_source();
+    return RCL_RET_OK;
+  }
+  time_point->time_source = time_source;
+
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_fini_time_point(rcl_time_point_t * time_point)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT);
+  (void) time_point;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT);
+  if (!time_source) {
+    duration->time_source = rcl_get_default_ros_time_source();
+    return RCL_RET_OK;
+  }
+  duration->time_source = time_source;
+
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_fini_duration(rcl_duration_t * duration)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT);
+  (void) duration;
+  return RCL_RET_OK;
+}
+
+rcl_time_source_t * rcl_get_default_ros_time_source(void)
+{
+  if (!rcl_default_ros_time_source) {
+    rcl_default_ros_time_source = calloc(1, sizeof(rcl_time_source_t));
+    rcl_ret_t retval = rcl_init_ros_time_source(rcl_default_ros_time_source);
+    if (retval != RCL_RET_OK) {
+      return NULL;
+    }
+  }
+  return rcl_default_ros_time_source;
+}
+
+rcl_time_source_t * rcl_get_default_steady_time_source(void)
+{
+  if (!rcl_default_steady_time_source) {
+    rcl_default_steady_time_source = calloc(1, sizeof(rcl_time_source_t));
+    rcl_ret_t retval = rcl_init_steady_time_source(rcl_default_steady_time_source);
+    if (retval != RCL_RET_OK) {
+      return NULL;
+    }
+  }
+  return rcl_default_steady_time_source;
+}
+
+rcl_time_source_t * rcl_get_default_system_time_source(void)
+{
+  if (!rcl_default_system_time_source) {
+    rcl_default_system_time_source = calloc(1, sizeof(rcl_time_source_t));
+    rcl_ret_t retval = rcl_init_system_time_source(rcl_default_system_time_source);
+    if (retval != RCL_RET_OK) {
+      return NULL;
+    }
+  }
+  return rcl_default_system_time_source;
+}
+
+rcl_ret_t rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(process_time_source, RCL_RET_INVALID_ARGUMENT);
+  if (rcl_default_ros_time_source) {
+    free(rcl_default_ros_time_source);
+  }
+  rcl_default_ros_time_source = process_time_source;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish,
+  rcl_duration_t * delta)
+{
+  if (start->time_source->type != finish->time_source->type) {
+    RCL_SET_ERROR_MSG("Cannot difference between time points with time_sources types.");
+    return RCL_RET_ERROR;
+  }
+  if (finish->nanoseconds < start->nanoseconds) {
+    rcl_time_point_value_t intermediate = start->nanoseconds - finish->nanoseconds;
+    delta->nanoseconds = -1 * (int) intermediate;
+  }
+  delta->nanoseconds = (int)(finish->nanoseconds - start->nanoseconds);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_get_time_point_now(rcl_time_point_t * time_point)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT);
+  if (time_point->time_source && time_point->time_source->get_now) {
+    return time_point->time_source->get_now(time_point->time_source->data,
+             &(time_point->nanoseconds));
+  }
+  RCL_SET_ERROR_MSG("time_source is not initialized or does not have get_now registered.");
+  return RCL_RET_ERROR;
+}
+
+rcl_ret_t rcl_enable_ros_time_override(rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  if (time_source->type != RCL_ROS_TIME) {
+    RCL_SET_ERROR_MSG("Time source is not RCL_ROS_TIME cannot enable override.")
+    return RCL_RET_ERROR;
+  }
+  rcl_ros_time_source_storage_t * storage = \
+    (rcl_ros_time_source_storage_t *)time_source->data;
+  if (!storage) {
+    RCL_SET_ERROR_MSG("Storage not initialized, cannot enable.")
+    return RCL_RET_ERROR;
+  }
+  storage->active = true;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_disable_ros_time_override(rcl_time_source_t * time_source)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  if (time_source->type != RCL_ROS_TIME) {
+    return RCL_RET_ERROR;
+  }
+  rcl_ros_time_source_storage_t * storage = \
+    (rcl_ros_time_source_storage_t *)time_source->data;
+  if (!storage) {
+    RCL_SET_ERROR_MSG("Storage not initialized, cannot disable.")
+    return RCL_RET_ERROR;
+  }
+  storage->active = false;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_is_enabled_ros_time_override(rcl_time_source_t * time_source,
+  bool * is_enabled)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(is_enabled, RCL_RET_INVALID_ARGUMENT);
+  if (time_source->type != RCL_ROS_TIME) {
+    return RCL_RET_ERROR;
+  }
+  rcl_ros_time_source_storage_t * storage = \
+    (rcl_ros_time_source_storage_t *)time_source->data;
+  if (!storage) {
+    RCL_SET_ERROR_MSG("Storage not initialized, cannot query.")
+    return RCL_RET_ERROR;
+  }
+  *is_enabled = storage->active;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_set_ros_time_override(rcl_time_source_t * time_source,
+  rcl_time_point_value_t time_value)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  if (time_source->type != RCL_ROS_TIME) {
+    return RCL_RET_ERROR;
+  }
+  rcl_ros_time_source_storage_t * storage = \
+    (rcl_ros_time_source_storage_t *)time_source->data;
+  if (storage->active && time_source->pre_update) {
+    time_source->pre_update();
+  }
+  rcl_atomic_store(&(storage->current_time), time_value);
+  if (storage->active && time_source->post_update) {
+    time_source->post_update();
+  }
+  return RCL_RET_OK;
+}

rcl/src/rcl/time_unix.c

@@ -45,7 +45,7 @@ extern "C"
 #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0))
 
 rcl_ret_t
-rcl_system_time_point_now(rcl_system_time_point_t * now)
+rcl_system_time_now(rcl_time_point_value_t * now)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
   struct timespec timespec_now;
@@ -66,12 +66,12 @@ rcl_system_time_point_now(rcl_system_time_point_t * now)
     RCL_SET_ERROR_MSG("unexpected negative time");
     return RCL_RET_ERROR;
   }
-  now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec;
+  *now = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec;
   return RCL_RET_OK;
 }
 
 rcl_ret_t
-rcl_steady_time_point_now(rcl_steady_time_point_t * now)
+rcl_steady_time_now(rcl_time_point_value_t * now)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
   // If clock_gettime is available or on OS X, use a timespec.
@@ -97,7 +97,7 @@ rcl_steady_time_point_now(rcl_steady_time_point_t * now)
     RCL_SET_ERROR_MSG("unexpected negative time");
     return RCL_RET_ERROR;
   }
-  now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec;
+  *now = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec;
   return RCL_RET_OK;
 }
 

rcl/src/rcl/time_win32.c

@@ -30,7 +30,7 @@ extern "C"
 #include "rcl/error_handling.h"
 
 rcl_ret_t
-rcl_system_time_point_now(rcl_system_time_point_t * now)
+rcl_system_time_now(rcl_time_point_value_t * now)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
   FILETIME ft;
@@ -42,12 +42,12 @@ rcl_system_time_point_now(rcl_system_time_point_t * now)
   //   https://support.microsoft.com/en-us/kb/167296
   uli.QuadPart -= 116444736000000000;
   // Convert to nanoseconds from 100's of nanoseconds.
-  now->nanoseconds = uli.QuadPart * 100;
+  *now = uli.QuadPart * 100;
   return RCL_RET_OK;
 }
 
 rcl_ret_t
-rcl_steady_time_point_now(rcl_steady_time_point_t * now)
+rcl_steady_time_now(rcl_time_point_value_t * now)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
   LARGE_INTEGER cpu_frequency, performance_count;
@@ -59,8 +59,8 @@ rcl_steady_time_point_now(rcl_steady_time_point_t * now)
   QueryPerformanceFrequency(&cpu_frequency);
   QueryPerformanceCounter(&performance_count);
   // Convert to nanoseconds before converting from ticks to avoid precision loss.
-  now->nanoseconds = RCL_S_TO_NS(performance_count.QuadPart);
+  rcl_time_point_value_t intermediate = RCL_S_TO_NS(performance_count.QuadPart);
-  now->nanoseconds /= cpu_frequency.QuadPart;
+  *now = intermediate / cpu_frequency.QuadPart;
   return RCL_RET_OK;
 }
 

rcl/src/rcl/timer.c

@@ -56,15 +56,15 @@ rcl_timer_init(
     RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized");
     return RCL_RET_ALREADY_INIT;
   }
-  rcl_steady_time_point_t now_steady;
+  rcl_time_point_value_t now_steady;
-  rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady);
+  rcl_ret_t now_ret = rcl_steady_time_now(&now_steady);
   if (now_ret != RCL_RET_OK) {
     return now_ret;  // rcl error state should already be set.
   }
   rcl_timer_impl_t impl = {
     .callback = ATOMIC_VAR_INIT((uintptr_t)callback),
     .period = ATOMIC_VAR_INIT(period),
-    .last_call_time = ATOMIC_VAR_INIT(now_steady.nanoseconds),
+    .last_call_time = ATOMIC_VAR_INIT(now_steady),
     .canceled = ATOMIC_VAR_INIT(false),
     .allocator = allocator,
   };
@@ -100,14 +100,14 @@ rcl_timer_call(rcl_timer_t * timer)
     RCL_SET_ERROR_MSG("timer is canceled");
     return RCL_RET_TIMER_CANCELED;
   }
-  rcl_steady_time_point_t now_steady;
+  rcl_time_point_value_t now_steady;
-  rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady);
+  rcl_ret_t now_ret = rcl_steady_time_now(&now_steady);
   if (now_ret != RCL_RET_OK) {
     return now_ret;  // rcl error state should already be set.
   }
-  uint64_t previous_ns =
+  rcl_time_point_value_t previous_ns =
-    rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady.nanoseconds);
+    rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady);
-  uint64_t since_last_call = now_steady.nanoseconds - previous_ns;
+  uint64_t since_last_call = now_steady - previous_ns;
   rcl_timer_callback_t typed_callback =
     (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
   typed_callback(timer, since_last_call);
@@ -135,30 +135,32 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt
   RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
-  rcl_steady_time_point_t now;
+  rcl_time_point_value_t now;
-  rcl_ret_t ret = rcl_steady_time_point_now(&now);
+  rcl_ret_t ret = rcl_steady_time_now(&now);
   if (ret != RCL_RET_OK) {
     return ret;  // rcl error state should already be set.
   }
   uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
   *time_until_next_call =
-    (rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now.nanoseconds;
+    (rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now;
   return RCL_RET_OK;
 }
 
 rcl_ret_t
-rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call)
+rcl_timer_get_time_since_last_call(
+  const rcl_timer_t * timer,
+  rcl_time_point_value_t * time_since_last_call)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
-  rcl_steady_time_point_t now;
+  rcl_time_point_value_t now;
-  rcl_ret_t ret = rcl_steady_time_point_now(&now);
+  rcl_ret_t ret = rcl_steady_time_now(&now);
   if (ret != RCL_RET_OK) {
     return ret;  // rcl error state should already be set.
   }
   *time_since_last_call =
-    now.nanoseconds - rcl_atomic_load_uint64_t(&timer->impl->last_call_time);
+    now - rcl_atomic_load_uint64_t(&timer->impl->last_call_time);
   return RCL_RET_OK;
 }
 
@@ -224,12 +226,12 @@ rcl_timer_reset(rcl_timer_t * timer)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
-  rcl_steady_time_point_t now;
+  rcl_time_point_value_t now;
-  rcl_ret_t now_ret = rcl_steady_time_point_now(&now);
+  rcl_ret_t now_ret = rcl_steady_time_now(&now);
   if (now_ret != RCL_RET_OK) {
     return now_ret;  // rcl error state should already be set.
   }
-  rcl_atomic_store(&timer->impl->last_call_time, now.nanoseconds);
+  rcl_atomic_store(&timer->impl->last_call_time, now);
   rcl_atomic_store(&timer->impl->canceled, false);
   return RCL_RET_OK;
 }

rcl/test/rcl/test_time.cpp

@@ -47,74 +47,334 @@ public:
   }
 };
 
-/* Tests the rcl_system_time_point_now() function.
+// Tests the rcl_system_time_now() function.
- */
+TEST_F(TestTimeFixture, test_rcl_system_time_now) {
-TEST_F(TestTimeFixture, test_rcl_system_time_point_now) {
   assert_no_realloc_begin();
   rcl_ret_t ret;
   // Check for invalid argument error condition (allowed to alloc).
-  ret = rcl_system_time_point_now(nullptr);
+  ret = rcl_system_time_now(nullptr);
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
   rcl_reset_error();
   assert_no_malloc_begin();
   assert_no_free_begin();
   // Check for normal operation (not allowed to alloc).
-  rcl_system_time_point_t now = {0};
+  rcl_time_point_value_t now = 0;
-  ret = rcl_system_time_point_now(&now);
+  ret = rcl_system_time_now(&now);
   assert_no_malloc_end();
   assert_no_realloc_end();
   assert_no_free_end();
   stop_memory_checking();
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
-  EXPECT_NE(now.nanoseconds, 0u);
+  EXPECT_NE(now, 0u);
   // Compare to std::chrono::system_clock time (within a second).
-  now = {0};
+  now = 0;
-  ret = rcl_system_time_point_now(&now);
+  ret = rcl_system_time_now(&now);
   {
     std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
     auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
     int64_t now_ns_int = now_ns.count();
-    int64_t now_diff = now.nanoseconds - now_ns_int;
+    int64_t now_diff = now - now_ns_int;
     const int k_tolerance_ms = 1000;
     EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs";
   }
 }
 
-/* Tests the rcl_steady_time_point_now() function.
+// Tests the rcl_steady_time_now() function.
- */
+TEST_F(TestTimeFixture, test_rcl_steady_time_now) {
-TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) {
   assert_no_realloc_begin();
   rcl_ret_t ret;
   // Check for invalid argument error condition (allowed to alloc).
-  ret = rcl_steady_time_point_now(nullptr);
+  ret = rcl_steady_time_now(nullptr);
   EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
   rcl_reset_error();
   assert_no_malloc_begin();
   assert_no_free_begin();
   // Check for normal operation (not allowed to alloc).
-  rcl_steady_time_point_t now = {0};
+  rcl_time_point_value_t now = 0;
-  ret = rcl_steady_time_point_now(&now);
+  ret = rcl_steady_time_now(&now);
   assert_no_malloc_end();
   assert_no_realloc_end();
   assert_no_free_end();
   stop_memory_checking();
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
-  EXPECT_NE(now.nanoseconds, 0u);
+  EXPECT_NE(now, 0u);
   // Compare to std::chrono::steady_clock difference of two times (within a second).
-  now = {0};
+  now = 0;
-  ret = rcl_steady_time_point_now(&now);
+  ret = rcl_steady_time_now(&now);
   std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now();
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
   // Wait for a little while.
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
   // Then take a new timestamp with each and compare.
-  rcl_steady_time_point_t later;
+  rcl_time_point_value_t later;
-  ret = rcl_steady_time_point_now(&later);
+  ret = rcl_steady_time_now(&later);
   std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now();
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
-  int64_t steady_diff = later.nanoseconds - now.nanoseconds;
+  int64_t steady_diff = later - now;
   int64_t sc_diff =
     std::chrono::duration_cast<std::chrono::nanoseconds>(later_sc - now_sc).count();
   const int k_tolerance_ms = 1;
   EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "steady_clock differs";
 }
+
+// Tests the rcl_set_ros_time_override() function.
+TEST_F(TestTimeFixture, test_rcl_set_ros_time_override) {
+  rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source();
+  assert_no_realloc_begin();
+  rcl_ret_t ret;
+  // Check for invalid argument error condition (allowed to alloc).
+  ret = rcl_set_ros_time_override(nullptr, 0);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
+  rcl_reset_error();
+  bool result;
+  ret = rcl_is_enabled_ros_time_override(nullptr, &result);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
+  rcl_reset_error();
+  ret = rcl_is_enabled_ros_time_override(ros_time_source, nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
+  rcl_reset_error();
+  ret = rcl_is_enabled_ros_time_override(nullptr, nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
+  rcl_reset_error();
+  rcl_time_point_t query_now;
+  bool is_enabled;
+  ret = rcl_is_enabled_ros_time_override(ros_time_source, &is_enabled);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(is_enabled, false);
+  ret = rcl_init_time_point(&query_now, ros_time_source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  assert_no_malloc_begin();
+  assert_no_free_begin();
+  // Check for normal operation (not allowed to alloc).
+  ret = rcl_get_time_point_now(&query_now);
+  assert_no_malloc_end();
+  assert_no_realloc_end();
+  assert_no_free_end();
+  stop_memory_checking();
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_NE(query_now.nanoseconds, 0);
+  // Compare to std::chrono::system_clock time (within a second).
+  ret = rcl_get_time_point_now(&query_now);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  {
+    std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
+    auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
+    int64_t now_ns_int = now_ns.count();
+    int64_t now_diff = query_now.nanoseconds - now_ns_int;
+    const int k_tolerance_ms = 1000;
+    EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
+  }
+  // Test ros time specific APIs
+  rcl_time_point_value_t set_point = 1000000000ull;
+  // Check initialized state
+  ret = rcl_is_enabled_ros_time_override(ros_time_source, &is_enabled);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(is_enabled, false);
+  // set the time point
+  ret = rcl_set_ros_time_override(ros_time_source, set_point);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  // check still disabled
+  ret = rcl_is_enabled_ros_time_override(ros_time_source, &is_enabled);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(is_enabled, false);
+  // get real
+  ret = rcl_get_time_point_now(&query_now);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  {
+    std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
+    auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
+    int64_t now_ns_int = now_ns.count();
+    int64_t now_diff = query_now.nanoseconds - now_ns_int;
+    const int k_tolerance_ms = 1000;
+    EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
+  }
+  // enable
+  ret = rcl_enable_ros_time_override(ros_time_source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  // check enabled
+  ret = rcl_is_enabled_ros_time_override(ros_time_source, &is_enabled);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(is_enabled, true);
+  // get sim
+  ret = rcl_get_time_point_now(&query_now);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(query_now.nanoseconds, set_point);
+  // disable
+  ret = rcl_disable_ros_time_override(ros_time_source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  // check disabled
+  ret = rcl_is_enabled_ros_time_override(ros_time_source, &is_enabled);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(is_enabled, false);
+  // get real
+  ret = rcl_get_time_point_now(&query_now);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  {
+    std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
+    auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
+    int64_t now_ns_int = now_ns.count();
+    int64_t now_diff = query_now.nanoseconds - now_ns_int;
+    const int k_tolerance_ms = 1000;
+    EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
+  }
+}
+
+TEST_F(TestTimeFixture, test_rcl_init_time_source_and_point) {
+  assert_no_realloc_begin();
+  rcl_ret_t ret;
+  // Check for invalid argument error condition (allowed to alloc).
+  ret = rcl_init_ros_time_source(nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
+  rcl_reset_error();
+  // Check for normal operation (not allowed to alloc).
+  rcl_time_source_t source;
+  ret = rcl_init_ros_time_source(&source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  rcl_time_point_t a_time;
+  ret = rcl_init_time_point(&a_time, &source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  rcl_time_point_t default_time;
+  ret = rcl_init_time_point(&default_time, nullptr);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  // assert_no_malloc_begin();
+  // assert_no_free_begin();
+  // // Do stuff in here
+  // assert_no_malloc_end();
+  // assert_no_realloc_end();
+  // assert_no_free_end();
+  // stop_memory_checking();
+  // EXPECT_NE(now.nanoseconds, 0u);
+  // // Compare to std::chrono::system_clock time (within a second).
+  // now = {0};
+  // ret = rcl_system_time_now(&now);
+  // {
+  //   std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
+  //   auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+  // now_sc.time_since_epoch());
+  //   int64_t now_ns_int = now_ns.count();
+  //   int64_t now_diff = now.nanoseconds - now_ns_int;
+  //   const int k_tolerance_ms = 1000;
+  //   EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs";
+  // }
+  ret = rcl_fini_time_point(&a_time);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  ret = rcl_fini_ros_time_source(&source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+}
+
+TEST(rcl_time, time_source_validation) {
+  ASSERT_FALSE(rcl_time_source_valid(NULL));
+  rcl_time_source_t uninitialized;
+  // Not reliably detectable due to random values.
+  // ASSERT_FALSE(rcl_time_source_valid(&uninitialized));
+  rcl_ret_t ret;
+  ret = rcl_init_ros_time_source(&uninitialized);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+}
+
+TEST(rcl_time, default_time_source_instanciation) {
+  rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source();
+  ASSERT_TRUE(rcl_time_source_valid(ros_time_source));
+  rcl_time_source_t * steady_time_source = rcl_get_default_steady_time_source();
+  ASSERT_TRUE(rcl_time_source_valid(steady_time_source));
+  rcl_time_source_t * system_time_source = rcl_get_default_system_time_source();
+  ASSERT_TRUE(rcl_time_source_valid(system_time_source));
+}
+
+TEST(rcl_time, rcl_time_difference) {
+  rcl_ret_t ret;
+  rcl_time_point_t a, b;
+  ret = rcl_init_time_point(&a, nullptr);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  ret = rcl_init_time_point(&b, nullptr);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  a.nanoseconds = 1000;
+  b.nanoseconds = 2000;
+
+  rcl_duration_t d;
+  ret = rcl_init_duration(&d, nullptr);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  ret = rcl_difference_times(&a, &b, &d);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  EXPECT_EQ(d.nanoseconds, 1000);
+  EXPECT_EQ(d.time_source->type, RCL_ROS_TIME);
+
+  ret = rcl_difference_times(&b, &a, &d);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(d.nanoseconds, -1000);
+  EXPECT_EQ(d.time_source->type, RCL_ROS_TIME);
+
+  rcl_time_source_t * system_time_source = rcl_get_default_system_time_source();
+  EXPECT_TRUE(system_time_source != nullptr);
+
+  rcl_time_point_t e;
+  ret = rcl_init_time_point(&e, system_time_source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+}
+
+static bool pre_callback_called = false;
+static bool post_callback_called = false;
+
+void pre_callback(void)
+{
+  pre_callback_called = true;
+  EXPECT_FALSE(post_callback_called);
+}
+void post_callback(void)
+{
+  EXPECT_TRUE(pre_callback_called);
+  post_callback_called = true;
+}
+
+
+TEST(rcl_time, rcl_time_update_callbacks) {
+  rcl_time_source_t * ros_time_source = rcl_get_default_ros_time_source();
+  rcl_time_point_t query_now;
+  rcl_ret_t ret;
+  rcl_time_point_value_t set_point = 1000000000ull;
+
+  ret = rcl_init_time_point(&query_now, ros_time_source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  // set callbacks
+  ros_time_source->pre_update = pre_callback;
+  ros_time_source->post_update = post_callback;
+
+
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+
+  // Query it to do something different. no changes expected
+  ret = rcl_get_time_point_now(&query_now);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+
+  // Set the time before it's enabled. Should be no callbacks
+  ret = rcl_set_ros_time_override(ros_time_source, set_point);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+
+  // enable
+  ret = rcl_enable_ros_time_override(ros_time_source);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+
+  // Set the time now that it's enabled, now get callbacks
+  ret = rcl_set_ros_time_override(ros_time_source, set_point);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+
+  EXPECT_TRUE(pre_callback_called);
+  EXPECT_TRUE(post_callback_called);
+}