initialize timer with clock

rcl/include/rcl/time.h

@@ -272,20 +272,20 @@ 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.
+/// Fill the time point value with the current value of the associated clock.
 /**
- * This function will populate the data of the time_point object with the
+ * This function will populate the data of the time_point_value object with the
  * current value from it's associated time abstraction.
  * \param[in] clock The time source from which to set the value.
- * \param[out] time_point The time_point to populate.
+ * \param[out] time_point_value The time_point value to populate.
  * \return `RCL_RET_OK` if the last call time was retrieved successfully, or
  * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
  * \return `RCL_RET_ERROR` an unspecified error occur.
  */
 RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
-rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point);
+rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value);
 
 
 /// Enable the ROS time abstraction override.

rcl/include/rcl/timer.h

@@ -28,6 +28,7 @@ extern "C"
 #include "rcl/types.h"
 #include "rmw/rmw.h"
 
+struct rcl_clock_t;
 struct rcl_timer_impl_t;
 
 /// Structure which encapsulates a ROS Timer.
@@ -60,7 +61,7 @@ rcl_get_zero_initialized_timer(void);
 
 /// Initialize a timer.
 /**
- * A timer consists of a callback function and a period.
+ * A timer consists of a clock, a callback function and a period.
  * A timer can be added to a wait set and waited on, such that the wait set
  * will wake up when a timer is ready to be executed.
  *
@@ -77,6 +78,9 @@ rcl_get_zero_initialized_timer(void);
  * Calling this function on a timer struct which has been allocated but not
  * zero initialized is undefined behavior.
  *
+ * The clock handle must be a pointer to an initialized rcl_clock_t struct.
+ * The life time of the clock must exceed the life time of the timer.
+ *
  * The period is a duration (rather an absolute time in the future).
  * If the period is `0` then it will always be ready.
  *
@@ -101,9 +105,14 @@ rcl_get_zero_initialized_timer(void);
  *   // Optionally reconfigure, cancel, or reset the timer...
  * }
  *
+ * rcl_clock_t clock;
+ * rcl_allocator_t allocator = rcl_get_default_allocator();
+ * rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
+ * // ... error handling
+ *
  * rcl_timer_t timer = rcl_get_zero_initialized_timer();
- * rcl_ret_t ret = rcl_timer_init(
- *   &timer, RCL_MS_TO_NS(100), my_timer_callback, rcl_get_default_allocator());
+ * ret = rcl_timer_init(
+ *   &timer, &clock, RCL_MS_TO_NS(100), my_timer_callback, allocator);
  * // ... error handling, use the timer with a wait set, or poll it manually, then cleanup
  * ret = rcl_timer_fini(&timer);
  * // ... error handling
@@ -124,6 +133,7 @@ rcl_get_zero_initialized_timer(void);
  *
  * \param[inout] timer the timer handle to be initialized
  * \param[in] period the duration between calls to the callback in nanoseconds
+ * \param[in] clock the clock providing the current time
  * \param[in] callback the user defined function to be called every period
  * \param[in] allocator the allocator to use for allocations
  * \return `RCL_RET_OK` if the timer was initialized successfully, or
@@ -137,6 +147,7 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_timer_init(
   rcl_timer_t * timer,
+  rcl_clock_t * clock,
   int64_t period,
   const rcl_timer_callback_t callback,
   rcl_allocator_t allocator);
@@ -216,6 +227,32 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_timer_call(rcl_timer_t * timer);
 
+/// Retrieve the clock of the timer.
+/**
+ * This function retrieves the clock pointer and copies it into the give variable.
+ *
+ * The clock argument must be a pointer to an already allocated rcl_clock_t *.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] timer the handle to the timer which is being queried
+ * \param[out] clock the rcl_clock_t * in which the clock is stored
+ * \return `RCL_RET_OK` if the period was retrieved successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_TIMER_INVALID` if the timer is invalid, or
+ * \return `RCL_RET_ERROR` an unspecified error occur.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock);
+
 /// Calculates whether or not the timer should be called.
 /**
  * The result is true if the time until next call is less than, or equal to, 0

rcl/src/rcl/time.c

@@ -110,6 +110,7 @@ rcl_ret_t
 rcl_clock_fini(
   rcl_clock_t * clock)
 {
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   switch (clock->type) {
     case RCL_ROS_TIME:
       return rcl_ros_clock_fini(clock);
@@ -226,12 +227,13 @@ rcl_difference_times(
 }
 
 rcl_ret_t
-rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point)
+rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ARGUMENT_FOR_NULL(
+    time_point_value, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (clock->type && clock->get_now) {
-    return clock->get_now(clock->data,
-             &(time_point->nanoseconds));
+    return clock->get_now(clock->data, time_point_value);
   }
   RCL_SET_ERROR_MSG(
     "clock is not initialized or does not have get_now registered.",

rcl/src/rcl/timer.c

@@ -28,6 +28,8 @@ extern "C"
 
 typedef struct rcl_timer_impl_t
 {
+  // The clock providing time.
+  rcl_clock_t * clock;
   // The user supplied callback.
   atomic_uintptr_t callback;
   // This is a duration in nanoseconds.
@@ -50,26 +52,29 @@ rcl_get_zero_initialized_timer()
 rcl_ret_t
 rcl_timer_init(
   rcl_timer_t * timer,
+  rcl_clock_t * clock,
   int64_t period,
   const rcl_timer_callback_t callback,
   rcl_allocator_t allocator)
 {
   RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, allocator);
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing timer with period: %" PRIu64 "ns", period)
   if (timer->impl) {
     RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator);
     return RCL_RET_ALREADY_INIT;
   }
-  rcl_time_point_value_t now_steady;
-  rcl_ret_t now_ret = rcutils_steady_time_now(&now_steady);
+  rcl_time_point_value_t now;
+  rcl_ret_t now_ret = rcl_clock_get_now(clock, &now);
   if (now_ret != RCL_RET_OK) {
     return now_ret;  // rcl error state should already be set.
   }
   rcl_timer_impl_t impl;
+  impl.clock = clock;
   atomic_init(&impl.callback, (uintptr_t)callback);
   atomic_init(&impl.period, period);
-  atomic_init(&impl.last_call_time, now_steady);
+  atomic_init(&impl.last_call_time, now);
   atomic_init(&impl.canceled, false);
   impl.allocator = allocator;
   timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
@@ -92,6 +97,18 @@ rcl_timer_fini(rcl_timer_t * timer)
   return result;
 }
 
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID, rcl_get_default_allocator());
+  *clock = timer->impl->clock;
+  return RCL_RET_OK;
+}
+
 rcl_ret_t
 rcl_timer_call(rcl_timer_t * timer)
 {
@@ -105,18 +122,18 @@ rcl_timer_call(rcl_timer_t * timer)
     RCL_SET_ERROR_MSG("timer is canceled", *allocator);
     return RCL_RET_TIMER_CANCELED;
   }
-  rcl_time_point_value_t now_steady;
-  rcl_ret_t now_ret = rcutils_steady_time_now(&now_steady);
+  rcl_time_point_value_t now;
+  rcl_ret_t now_ret = rcl_clock_get_now(timer->impl->clock, &now);
   if (now_ret != RCL_RET_OK) {
     return now_ret;  // rcl error state should already be set.
   }
   rcl_time_point_value_t previous_ns =
-    rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady);
+    rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now);
   rcl_timer_callback_t typed_callback =
     (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
 
   if (typed_callback != NULL) {
-    int64_t since_last_call = now_steady - previous_ns;
+    int64_t since_last_call = now - previous_ns;
     typed_callback(timer, since_last_call);
   }
   return RCL_RET_OK;

rcl/test/rcl/test_time.cpp

@@ -76,7 +76,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
   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;
+  rcl_time_point_value_t query_now;
   bool is_enabled;
   ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
@@ -86,15 +86,15 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
     ret = rcl_clock_get_now(&ros_clock, &query_now);
   });
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
-  EXPECT_NE(query_now.nanoseconds, 0u);
+  EXPECT_NE(query_now, 0u);
   // Compare to std::chrono::system_clock time (within a second).
   ret = rcl_clock_get_now(&ros_clock, &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;
+    int64_t now_diff = query_now - now_ns_int;
     const int k_tolerance_ms = 1000;
     EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
   }
@@ -118,7 +118,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
     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;
+    int64_t now_diff = query_now - now_ns_int;
     const int k_tolerance_ms = 1000;
     EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
   }
@@ -132,7 +132,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
   // get sim
   ret = rcl_clock_get_now(&ros_clock, &query_now);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
-  EXPECT_EQ(query_now.nanoseconds, set_point);
+  EXPECT_EQ(query_now, set_point);
   // disable
   ret = rcl_disable_ros_time_override(&ros_clock);
   EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
@@ -147,7 +147,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
     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;
+    int64_t now_diff = query_now - now_ns_int;
     const int k_tolerance_ms = 1000;
     EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
   }
@@ -352,7 +352,7 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) {
     reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
   rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
   EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
-  rcl_time_point_t query_now;
+  rcl_time_point_value_t query_now;
   rcl_ret_t ret;
   rcl_time_point_value_t set_point = 1000000000ull;
 

rcl/test/rcl/test_timer.cpp

@@ -50,13 +50,19 @@ class TestTimerFixture : public ::testing::Test
 
 TEST_F(TestTimerFixture, test_two_timers) {
   rcl_ret_t ret;
+
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
   rcl_timer_t timer = rcl_get_zero_initialized_timer();
   rcl_timer_t timer2 = rcl_get_zero_initialized_timer();
 
-  ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
+  ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 
-  ret = rcl_timer_init(&timer2, RCL_MS_TO_NS(20), nullptr, rcl_get_default_allocator());
+  ret = rcl_timer_init(&timer2, &clock, RCL_MS_TO_NS(20), nullptr, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
@@ -91,17 +97,26 @@ TEST_F(TestTimerFixture, test_two_timers) {
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
   EXPECT_FALSE(is_ready);
   ASSERT_EQ(1, nonnull_timers);
+
+  ret = rcl_clock_fini(&clock);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 }
 
 TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) {
   rcl_ret_t ret;
+
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
   rcl_timer_t timer = rcl_get_zero_initialized_timer();
   rcl_timer_t timer2 = rcl_get_zero_initialized_timer();
 
-  ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
+  ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 
-  ret = rcl_timer_init(&timer2, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
+  ret = rcl_timer_init(&timer2, &clock, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
@@ -136,13 +151,22 @@ TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) {
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
   EXPECT_FALSE(is_ready);
   ASSERT_EQ(1, nonnull_timers);
+
+  ret = rcl_clock_fini(&clock);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 }
 
 TEST_F(TestTimerFixture, test_timer_not_ready) {
   rcl_ret_t ret;
+
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
   rcl_timer_t timer = rcl_get_zero_initialized_timer();
 
-  ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
+  ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
@@ -171,13 +195,22 @@ TEST_F(TestTimerFixture, test_timer_not_ready) {
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
   EXPECT_FALSE(is_ready);
   ASSERT_EQ(0, nonnull_timers);
+
+  ret = rcl_clock_fini(&clock);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 }
 
 TEST_F(TestTimerFixture, test_canceled_timer) {
   rcl_ret_t ret;
+
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
   rcl_timer_t timer = rcl_get_zero_initialized_timer();
 
-  ret = rcl_timer_init(&timer, 500, nullptr, rcl_get_default_allocator());
+  ret = rcl_timer_init(&timer, &clock, 500, nullptr, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 
   ret = rcl_timer_cancel(&timer);
@@ -209,4 +242,7 @@ TEST_F(TestTimerFixture, test_canceled_timer) {
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
   EXPECT_FALSE(is_ready);
   ASSERT_EQ(0, nonnull_timers);
+
+  ret = rcl_clock_fini(&clock);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 }