Clock has multiple time jump callbacks (#284)

* Clock has multiple time jump callbacks

Replaces pre_update and post_update
Adds callbacks when ROS time is activated or deactivated
Add rcl_clock_change_t, rcl_time_jump_t, rcl_jump_threshold_t
Add rcl_clock_add_jump_callback()
Add rcl_clock_remove_jump_callback()

* Fini generic clock after confirming correct clock type

* test_time no ASAN detections

rcl/include/rcl/time.h

@@ -54,12 +54,73 @@ typedef enum rcl_clock_type_t
   RCL_STEADY_TIME
 } rcl_clock_type_t;
 
+/// A duration of time, measured in nanoseconds and its source.
+typedef struct rcl_duration_t
+{
+  rcl_duration_value_t nanoseconds;
+} rcl_duration_t;
+
+/// Enumeration to describe the type of time jump.
+typedef enum rcl_clock_change_t
+{
+  /// The source before and after the jump is ROS_TIME.
+  RCL_ROS_TIME_NO_CHANGE = 1,
+  /// The source switched to ROS_TIME from SYSTEM_TIME.
+  RCL_ROS_TIME_ACTIVATED = 2,
+  /// The source switched to SYSTEM_TIME from ROS_TIME.
+  RCL_ROS_TIME_DEACTIVATED = 3,
+  /// The source before and after the jump is SYSTEM_TIME.
+  RCL_SYSTEM_TIME_NO_CHANGE = 4
+} rcl_clock_change_t;
+
+/// Struct to describe a jump in time.
+typedef struct rcl_time_jump_t
+{
+  /// Indicate whether or not the source of time changed.
+  rcl_clock_change_t clock_change;
+  /// The new time minus the last time before the jump.
+  rcl_duration_t delta;
+} rcl_time_jump_t;
+
+/// Signature of a time jump callback.
+/// \param[in] time_jump A description of the jump in time.
+/// \param[in] before_jump Every jump callback is called twice: once before the clock changes and
+/// once after. This is true the first call and false the second.
+/// \param[in] user_data A pointer given at callback registration which is passed to the callback.
+typedef void (* rcl_jump_callback_t)(
+  const struct rcl_time_jump_t * time_jump,
+  bool before_jump,
+  void * user_data);
+
+/// Describe the prerequisites for calling a time jump callback.
+typedef struct rcl_jump_threshold_t
+{
+  /// True to call callback when the clock type changes.
+  bool on_clock_change;
+  /// A positive duration indicating the minimum jump forwards to be considered exceeded, or zero
+  /// to disable.
+  rcl_duration_t min_forward;
+  /// A negative duration indicating the minimum jump backwards to be considered exceeded, or zero
+  /// to disable.
+  rcl_duration_t min_backward;
+} rcl_jump_threshold_t;
+
+/// Struct to describe an added callback.
+typedef struct rcl_jump_callback_info_t
+{
+  rcl_jump_callback_t callback;
+  rcl_jump_threshold_t threshold;
+  void * user_data;
+} rcl_jump_callback_info_t;
+
 /// Encapsulation of a time source.
 typedef struct rcl_clock_t
 {
   enum rcl_clock_type_t type;
-  void (* pre_update)(void);
+  /// An array of added jump callbacks.
-  void (* post_update)(void);
+  rcl_jump_callback_info_t * jump_callbacks;
+  /// Number of callbacks in jump_callbacks.
+  size_t num_jump_callbacks;
   rcl_ret_t (* get_now)(void * data, rcl_time_point_value_t * now);
   // void (*set_now) (rcl_time_point_value_t);
   void * data;
@@ -73,12 +134,6 @@ typedef struct rcl_time_point_t
   rcl_clock_type_t clock_type;
 } rcl_time_point_t;
 
-/// A duration of time, measured in nanoseconds and its source.
-typedef struct rcl_duration_t
-{
-  rcl_duration_value_t nanoseconds;
-} rcl_duration_t;
-
 // typedef struct rcl_rate_t
 // {
 //   rcl_time_point_value_t trigger_time;
@@ -358,6 +413,44 @@ rcl_ret_t
 rcl_set_ros_time_override(
   rcl_clock_t * clock, rcl_time_point_value_t time_value);
 
+/// Add a callback to be called when a time jump exceeds a threshold.
+/**
+ * The callback is called twice when the threshold is exceeded: once before the clock is
+ * updated, and once after.
+ * The user_data pointer is passed to the callback as the last argument.
+ * A callback and user_data pair must be unique among the callbacks added to a clock.
+ *
+ * \param[in] clock A clock to add a jump callback to.
+ * \param[in] threshold Criteria indicating when to call the callback.
+ * \param[in] callback A callback to call.
+ * \param[in] user_data A pointer to be passed to the callback.
+ * \return `RCL_RET_OK` if the callback was added successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_ERROR` an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_clock_add_jump_callback(
+  rcl_clock_t * clock, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback,
+  void * user_data);
+
+/// Remove a previously added time jump callback.
+/**
+ * \param[in] clock The clock to remove a jump callback from.
+ * \param[in] threshold Criteria indicating when to call callback.
+ * \param[in] callback The callback to call.
+ * \param[in] user_data A pointer to be passed to the callback.
+ * \return `RCL_RET_OK` if the callback was added successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_ERROR` the callback was not found or an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_clock_remove_jump_callback(
+  rcl_clock_t * clock, rcl_jump_callback_t callback, void * user_data);
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/time.c

@@ -52,8 +52,8 @@ void
 rcl_init_generic_clock(rcl_clock_t * clock)
 {
   clock->type = RCL_CLOCK_UNINITIALIZED;
-  clock->pre_update = NULL;
+  clock->jump_callbacks = NULL;
-  clock->post_update = NULL;
+  clock->num_jump_callbacks = 0u;
   clock->get_now = NULL;
   clock->data = NULL;
 }
@@ -106,6 +106,18 @@ rcl_clock_init(
   }
 }
 
+void
+_rcl_clock_generic_fini(
+  rcl_clock_t * clock)
+{
+  // Internal function; assume caller has already checked that clock is valid.
+  if (clock->num_jump_callbacks > 0) {
+    clock->num_jump_callbacks = 0;
+    clock->allocator.deallocate(clock->jump_callbacks, clock->allocator.state);
+    clock->jump_callbacks = NULL;
+  }
+}
+
 rcl_ret_t
 rcl_clock_fini(
   rcl_clock_t * clock)
@@ -155,6 +167,7 @@ rcl_ros_clock_fini(
     RCL_SET_ERROR_MSG("clock not of type RCL_ROS_TIME", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
+  _rcl_clock_generic_fini(clock);
   if (!clock->data) {
     RCL_SET_ERROR_MSG("clock data invalid", rcl_get_default_allocator());
     return RCL_RET_ERROR;
@@ -186,6 +199,7 @@ rcl_steady_clock_fini(
     RCL_SET_ERROR_MSG("clock not of type RCL_STEADY_TIME", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
+  _rcl_clock_generic_fini(clock);
   return RCL_RET_OK;
 }
 
@@ -212,6 +226,7 @@ rcl_system_clock_fini(
     RCL_SET_ERROR_MSG("clock not of type RCL_SYSTEM_TIME", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
+  _rcl_clock_generic_fini(clock);
   return RCL_RET_OK;
 }
 
@@ -249,6 +264,27 @@ rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value
   return RCL_RET_ERROR;
 }
 
+void
+_rcl_clock_call_callbacks(
+  rcl_clock_t * clock, const rcl_time_jump_t * time_jump, bool before_jump)
+{
+  // Internal function; assume parameters are valid.
+  bool is_clock_change = time_jump->clock_change == RCL_ROS_TIME_ACTIVATED ||
+    time_jump->clock_change == RCL_ROS_TIME_DEACTIVATED;
+  for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) {
+    rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]);
+    if (
+      (is_clock_change && info->threshold.on_clock_change) ||
+      (time_jump->delta.nanoseconds < 0 &&
+      time_jump->delta.nanoseconds <= info->threshold.min_backward.nanoseconds) ||
+      (time_jump->delta.nanoseconds > 0 &&
+      time_jump->delta.nanoseconds >= info->threshold.min_forward.nanoseconds))
+    {
+      info->callback(time_jump, before_jump, info->user_data);
+    }
+  }
+}
+
 rcl_ret_t
 rcl_enable_ros_time_override(rcl_clock_t * clock)
 {
@@ -264,7 +300,14 @@ rcl_enable_ros_time_override(rcl_clock_t * clock)
       rcl_get_default_allocator())
     return RCL_RET_ERROR;
   }
-  storage->active = true;
+  if (!storage->active) {
+    rcl_time_jump_t time_jump;
+    time_jump.delta.nanoseconds = 0;
+    time_jump.clock_change = RCL_ROS_TIME_ACTIVATED;
+    _rcl_clock_call_callbacks(clock, &time_jump, true);
+    storage->active = true;
+    _rcl_clock_call_callbacks(clock, &time_jump, false);
+  }
   return RCL_RET_OK;
 }
 
@@ -284,7 +327,14 @@ rcl_disable_ros_time_override(rcl_clock_t * clock)
       rcl_get_default_allocator())
     return RCL_RET_ERROR;
   }
-  storage->active = false;
+  if (storage->active) {
+    rcl_time_jump_t time_jump;
+    time_jump.delta.nanoseconds = 0;
+    time_jump.clock_change = RCL_ROS_TIME_DEACTIVATED;
+    _rcl_clock_call_callbacks(clock, &time_jump, true);
+    storage->active = false;
+    _rcl_clock_call_callbacks(clock, &time_jump, false);
+  }
   return RCL_RET_OK;
 }
 
@@ -323,14 +373,107 @@ rcl_set_ros_time_override(
       "Clock is not of type RCL_ROS_TIME, cannot set time override.", rcl_get_default_allocator())
     return RCL_RET_ERROR;
   }
-  rcl_ros_clock_storage_t * storage = \
+  rcl_time_jump_t time_jump;
-    (rcl_ros_clock_storage_t *)clock->data;
+  rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
-  if (storage->active && clock->pre_update) {
+  if (storage->active) {
-    clock->pre_update();
+    time_jump.clock_change = RCL_ROS_TIME_NO_CHANGE;
+    rcl_time_point_value_t current_time;
+    rcl_ret_t ret = rcl_get_ros_time(storage, &current_time);
+    if (RCL_RET_OK != ret) {
+      return ret;
+    }
+    time_jump.delta.nanoseconds = time_value - current_time;
+    _rcl_clock_call_callbacks(clock, &time_jump, true);
   }
   rcl_atomic_store(&(storage->current_time), time_value);
-  if (storage->active && clock->post_update) {
+  if (storage->active) {
-    clock->post_update();
+    _rcl_clock_call_callbacks(clock, &time_jump, false);
   }
   return RCL_RET_OK;
 }
+
+rcl_ret_t
+rcl_clock_add_jump_callback(
+  rcl_clock_t * clock, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback,
+  void * user_data)
+{
+  // Make sure parameters are valid
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&(clock->allocator), "invalid allocator",
+    return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT, clock->allocator);
+  if (threshold.min_forward.nanoseconds < 0) {
+    RCL_SET_ERROR_MSG("forward jump threshold must be positive or zero", clock->allocator);
+    return RCL_RET_INVALID_ARGUMENT;
+  }
+  if (threshold.min_backward.nanoseconds > 0) {
+    RCL_SET_ERROR_MSG("backward jump threshold must be negative or zero", clock->allocator);
+    return RCL_RET_INVALID_ARGUMENT;
+  }
+
+  // Callback/user_data pair must be unique
+  for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) {
+    const rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]);
+    if (info->callback == callback && info->user_data == user_data) {
+      RCL_SET_ERROR_MSG("callback/user_data are already added to this clock", clock->allocator);
+      return RCL_RET_ERROR;
+    }
+  }
+
+  // Add the new callback, increasing the size of the callback list
+  rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate(
+    clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * (clock->num_jump_callbacks + 1),
+    clock->allocator.state);
+  if (NULL == callbacks) {
+    RCL_SET_ERROR_MSG("Failed to realloc jump callbacks", clock->allocator);
+    return RCL_RET_BAD_ALLOC;
+  }
+  clock->jump_callbacks = callbacks;
+  clock->jump_callbacks[clock->num_jump_callbacks].callback = callback;
+  clock->jump_callbacks[clock->num_jump_callbacks].threshold = threshold;
+  clock->jump_callbacks[clock->num_jump_callbacks].user_data = user_data;
+  ++(clock->num_jump_callbacks);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_clock_remove_jump_callback(
+  rcl_clock_t * clock, rcl_jump_callback_t callback, void * user_data)
+{
+  // Make sure parameters are valid
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&(clock->allocator), "invalid allocator",
+    return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT, clock->allocator);
+
+  // Delete callback if found, moving all callbacks after back one
+  bool found_callback = false;
+  for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) {
+    const rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]);
+    if (found_callback) {
+      clock->jump_callbacks[cb_idx - 1] = *info;
+    } else if (info->callback == callback && info->user_data == user_data) {
+      found_callback = true;
+    }
+  }
+  if (!found_callback) {
+    RCL_SET_ERROR_MSG("jump callback was not found", clock->allocator);
+    return RCL_RET_ERROR;
+  }
+
+  // Shrink size of the callback array
+  if (clock->num_jump_callbacks == 1) {
+    clock->allocator.deallocate(clock->jump_callbacks, clock->allocator.state);
+  } else {
+    rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate(
+      clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * (clock->num_jump_callbacks - 1),
+      clock->allocator.state);
+    if (NULL == callbacks) {
+      RCL_SET_ERROR_MSG("Failed to shrink jump callbacks", clock->allocator);
+      return RCL_RET_BAD_ALLOC;
+    }
+    clock->jump_callbacks = callbacks;
+  }
+  --(clock->num_jump_callbacks);
+  return RCL_RET_OK;
+}

rcl/test/rcl/test_time.cpp

@@ -60,7 +60,10 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
   rcl_clock_t ros_clock;
   rcl_allocator_t allocator = rcl_get_default_allocator();
   rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
-  EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string_safe();
+  });
 
   rcl_ret_t ret;
   // Check for invalid argument error condition (allowed to alloc).
@@ -169,11 +172,17 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_init_for_clock_a
   // Check for normal operation (not allowed to alloc).
   rcl_clock_t source;
   ret = rcl_ros_clock_init(&source, &allocator);
-  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&source)) << rcl_get_error_string_safe();
+  });
 
   rcl_clock_t ros_clock;
   rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
-  EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string_safe();
+  });
 }
 
 TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_ros_clock_initially_zero) {
@@ -197,26 +206,45 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), clock_validation) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
   rcl_ret_t ret;
   ret = rcl_ros_clock_init(&uninitialized, &allocator);
-  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&uninitialized)) << rcl_get_error_string_safe();
+  });
 }
 
 TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_clock_instanciation) {
   rcl_clock_t ros_clock;
   rcl_allocator_t allocator = rcl_get_default_allocator();
   rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
-  EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string_safe();
+  });
   ASSERT_TRUE(rcl_clock_valid(&ros_clock));
 
   rcl_clock_t * steady_clock = reinterpret_cast<rcl_clock_t *>(
     allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(steady_clock, allocator.state);
+  });
+
   retval = rcl_steady_clock_init(steady_clock, &allocator);
   EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_steady_clock_fini(steady_clock)) << rcl_get_error_string_safe();
+  });
   ASSERT_TRUE(rcl_clock_valid(steady_clock));
 
   rcl_clock_t * system_clock = reinterpret_cast<rcl_clock_t *>(
     allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(system_clock, allocator.state);
+  });
   retval = rcl_system_clock_init(system_clock, &allocator);
   EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_system_clock_fini(system_clock)) << rcl_get_error_string_safe();
+  });
   ASSERT_TRUE(rcl_clock_valid(system_clock));
 }
 
@@ -263,8 +291,14 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
   rcl_clock_t * ros_clock =
     reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(ros_clock, allocator.state);
+  });
   rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
-  EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(ros_clock)) << rcl_get_error_string_safe();
+  });
   EXPECT_TRUE(ros_clock != nullptr);
   EXPECT_TRUE(ros_clock->data != nullptr);
   EXPECT_EQ(ros_clock->type, RCL_ROS_TIME);
@@ -292,8 +326,14 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference_signed) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
   rcl_clock_t * ros_clock =
     reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(ros_clock, allocator.state);
+  });
   rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
-  EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(ros_clock)) << rcl_get_error_string_safe();
+  });
 
   rcl_time_point_t a, b;
   a.nanoseconds = RCL_S_TO_NS(0LL) + 0LL;
@@ -339,19 +379,22 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference_signed) {
   }
 }
 
-
 static bool pre_callback_called = false;
 static bool post_callback_called = false;
 
-void pre_callback(void)
+void clock_callback(
+  const struct rcl_time_jump_t * time_jump,
+  bool before_jump,
+  void * user_data)
 {
-  pre_callback_called = true;
+  if (before_jump) {
-  EXPECT_FALSE(post_callback_called);
+    pre_callback_called = true;
-}
+    EXPECT_FALSE(post_callback_called);
-void post_callback(void)
+  } else {
-{
+    EXPECT_TRUE(pre_callback_called);
-  EXPECT_TRUE(pre_callback_called);
+    post_callback_called = true;
-  post_callback_called = true;
+  }
+  *(static_cast<rcl_time_jump_t *>(user_data)) = *time_jump;
 }
 
 void reset_callback_triggers(void)
@@ -360,48 +403,270 @@ void reset_callback_triggers(void)
   post_callback_called = false;
 }
 
-TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) {
+TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_clock_change_callbacks) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
   rcl_clock_t * ros_clock =
     reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(ros_clock, allocator.state);
+  });
   rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
-  EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock));
+  });
   rcl_time_point_value_t query_now;
   rcl_ret_t ret;
-  rcl_time_point_value_t set_point = 1000000000ull;
 
   // set callbacks
-  ros_clock->pre_update = pre_callback;
+  rcl_time_jump_t time_jump;
-  ros_clock->post_update = post_callback;
+  rcl_jump_threshold_t threshold;
+  threshold.on_clock_change = true;
+  threshold.min_forward.nanoseconds = 0;
+  threshold.min_backward.nanoseconds = 0;
+  ASSERT_EQ(RCL_RET_OK,
+    rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) <<
+    rcl_get_error_string_safe();
+  reset_callback_triggers();
 
-  EXPECT_FALSE(pre_callback_called);
+  // Query time, no changes expected.
-  EXPECT_FALSE(post_callback_called);
-
-  // Query it to do something different. no changes expected
   ret = rcl_clock_get_now(ros_clock, &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
+  // Clock change callback called when ROS time is enabled
-  ret = rcl_set_ros_time_override(ros_clock, 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_clock);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_TRUE(pre_callback_called);
+  EXPECT_TRUE(post_callback_called);
+  EXPECT_EQ(RCL_ROS_TIME_ACTIVATED, time_jump.clock_change);
+  reset_callback_triggers();
 
+  // Clock change callback not called because ROS time is already enabled.
+  ret = rcl_enable_ros_time_override(ros_clock);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+  reset_callback_triggers();
+
+  // Clock change callback called when ROS time is disabled
+  ret = rcl_disable_ros_time_override(ros_clock);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_TRUE(pre_callback_called);
+  EXPECT_TRUE(post_callback_called);
+  EXPECT_EQ(RCL_ROS_TIME_DEACTIVATED, time_jump.clock_change);
+  reset_callback_triggers();
+
+  // Clock change callback not called because ROS time is already disabled.
+  ret = rcl_disable_ros_time_override(ros_clock);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+  reset_callback_triggers();
+}
+
+TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_forward_jump_callbacks) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_clock_t * ros_clock =
+    reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(ros_clock, allocator.state);
+  });
+  rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
+  ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock));
+  });
+
+  rcl_ret_t ret;
+  rcl_time_point_value_t set_point1 = 1000L * 1000L * 1000L;
+  rcl_time_point_value_t set_point2 = 2L * 1000L * 1000L * 1000L;
+
+  rcl_time_jump_t time_jump;
+  rcl_jump_threshold_t threshold;
+  threshold.on_clock_change = false;
+  threshold.min_forward.nanoseconds = 1;
+  threshold.min_backward.nanoseconds = 0;
+  ASSERT_EQ(RCL_RET_OK,
+    rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) <<
+    rcl_get_error_string_safe();
+  reset_callback_triggers();
+
+  // Set the time before it's enabled. Should be no callbacks
+  ret = rcl_set_ros_time_override(ros_clock, set_point1);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+
+  // enable no callbacks
+  ret = rcl_enable_ros_time_override(ros_clock);
+  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_clock, set_point);
+  ret = rcl_set_ros_time_override(ros_clock, set_point2);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
-
   EXPECT_TRUE(pre_callback_called);
   EXPECT_TRUE(post_callback_called);
+  EXPECT_EQ(set_point2 - set_point1, time_jump.delta.nanoseconds);
+  EXPECT_EQ(RCL_ROS_TIME_NO_CHANGE, time_jump.clock_change);
+  reset_callback_triggers();
+
+  // Setting same value as previous time, not a jump
+  ret = rcl_set_ros_time_override(ros_clock, set_point2);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+
+  // disable no callbacks
+  ret = rcl_disable_ros_time_override(ros_clock);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+}
+
+TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_backward_jump_callbacks) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_clock_t * ros_clock =
+    reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(ros_clock, allocator.state);
+  });
+  rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
+  ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock));
+  });
+  rcl_ret_t ret;
+  rcl_time_point_value_t set_point1 = 1000000000;
+  rcl_time_point_value_t set_point2 = 2000000000;
+
+  rcl_time_jump_t time_jump;
+  rcl_jump_threshold_t threshold;
+  threshold.on_clock_change = false;
+  threshold.min_forward.nanoseconds = 0;
+  threshold.min_backward.nanoseconds = -1;
+  ASSERT_EQ(RCL_RET_OK,
+    rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) <<
+    rcl_get_error_string_safe();
+  reset_callback_triggers();
+
+  // Set the time before it's enabled. Should be no callbacks
+  ret = rcl_set_ros_time_override(ros_clock, set_point2);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+
+  // enable no callbacks
+  ret = rcl_enable_ros_time_override(ros_clock);
+  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_clock, set_point1);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_TRUE(pre_callback_called);
+  EXPECT_TRUE(post_callback_called);
+  EXPECT_EQ(set_point1 - set_point2, time_jump.delta.nanoseconds);
+  EXPECT_EQ(RCL_ROS_TIME_NO_CHANGE, time_jump.clock_change);
+  reset_callback_triggers();
+
+  // Setting same value as previous time, not a jump
+  ret = rcl_set_ros_time_override(ros_clock, set_point1);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+
+  // disable no callbacks
+  ret = rcl_disable_ros_time_override(ros_clock);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_FALSE(pre_callback_called);
+  EXPECT_FALSE(post_callback_called);
+}
+
+TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_add_jump_callback) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_clock_t * clock =
+    reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(clock, allocator.state);
+  });
+  rcl_ret_t retval = rcl_ros_clock_init(clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(clock));
+  });
+
+  rcl_jump_threshold_t threshold;
+  threshold.on_clock_change = false;
+  threshold.min_forward.nanoseconds = 0;
+  threshold.min_backward.nanoseconds = 0;
+  rcl_jump_callback_t cb = reinterpret_cast<rcl_jump_callback_t>(0xBEEF);
+  void * user_data = reinterpret_cast<void *>(0xCAFE);
+
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_clock_add_jump_callback(clock, threshold, NULL, NULL));
+  rcl_reset_error();
+
+  EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, NULL)) <<
+    rcl_get_error_string_safe();
+  EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(clock, threshold, cb, NULL));
+  rcl_reset_error();
+
+  EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data)) <<
+    rcl_get_error_string_safe();
+  EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(clock, threshold, cb, user_data));
+  rcl_reset_error();
+
+  EXPECT_EQ(2u, clock->num_jump_callbacks);
+}
+
+TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_remove_jump_callback) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_clock_t * clock =
+    reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    allocator.deallocate(clock, allocator.state);
+  });
+  rcl_ret_t retval = rcl_ros_clock_init(clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(clock));
+  });
+
+  rcl_jump_threshold_t threshold;
+  threshold.on_clock_change = false;
+  threshold.min_forward.nanoseconds = 0;
+  threshold.min_backward.nanoseconds = 0;
+  rcl_jump_callback_t cb = reinterpret_cast<rcl_jump_callback_t>(0xBEEF);
+  void * user_data1 = reinterpret_cast<void *>(0xCAFE);
+  void * user_data2 = reinterpret_cast<void *>(0xFACE);
+  void * user_data3 = reinterpret_cast<void *>(0xBEAD);
+  void * user_data4 = reinterpret_cast<void *>(0xDEED);
+
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_clock_remove_jump_callback(clock, NULL, NULL));
+  rcl_reset_error();
+  EXPECT_EQ(RCL_RET_ERROR, rcl_clock_remove_jump_callback(clock, cb, NULL));
+  rcl_reset_error();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data1)) <<
+    rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data2)) <<
+    rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data3)) <<
+    rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data4)) <<
+    rcl_get_error_string_safe();
+  EXPECT_EQ(4u, clock->num_jump_callbacks);
+
+  EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data3));
+  EXPECT_EQ(3u, clock->num_jump_callbacks);
+  EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data4));
+  EXPECT_EQ(2u, clock->num_jump_callbacks);
+  EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data1));
+  EXPECT_EQ(1u, clock->num_jump_callbacks);
+  EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data2));
+  EXPECT_EQ(0u, clock->num_jump_callbacks);
 }