Merge pull request #12 from ros2/ros_time

first work toward adding ros time support

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);
+}