Add new rcutils_raw_steady_time_now method changes

saikishor · saikishor · commit 885e12aff0de · 2025-07-16T22:41:38.000+02:00
diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h
@@ -57,6 +57,11 @@ typedef rcutils_duration_value_t rcl_duration_value_t;
  * RCL_SYSTEM_TIME reports the same value as the system clock.
  *
  * RCL_STEADY_TIME reports a value from a monotonically increasing clock.
+ *
+ * RCL_RAW_STEADY_TIME reports a value from a monotonic clock that is not
+ * adjusted for time jumps, such as those caused by NTP synchronization.
+ * This clock is not suitable for measuring elapsed time, but can be used to
+ * measure time intervals without being affected by system time changes.
  */
 typedef enum rcl_clock_type_e
 {
@@ -67,7 +72,9 @@ typedef enum rcl_clock_type_e
   /// Use system time
   RCL_SYSTEM_TIME,
   /// Use a steady clock time
-  RCL_STEADY_TIME
+  RCL_STEADY_TIME,
+  /// Use a monotonic slew-free steady clock time
+  RCL_RAW_STEADY_TIME
 } rcl_clock_type_t;
 
 /// A duration of time, measured in nanoseconds and its source.
@@ -406,6 +413,69 @@ rcl_ret_t
 rcl_steady_clock_fini(
   rcl_clock_t * clock);
 
+/// Initialize a clock as a #RCL_RAW_STEADY_TIME time source.
+/**
+ * This will allocate all necessary internal structures, and initialize variables.
+ * It is specifically setting up a #RCL_RAW_STEADY_TIME time source.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No [1]
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * <i>[1] Function is reentrant, but concurrent calls on the same `clock` object are not safe.
+ *        Thread-safety is also affected by that of the `allocator` object.</i>
+ *
+ * \param[in] clock the handle to the clock which is being initialized
+ * \param[in] allocator The allocator to use for allocations
+ * \return #RCL_RET_OK if the time source was successfully initialized, 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_raw_steady_clock_init(
+  rcl_clock_t * clock,
+  rcl_allocator_t * allocator);
+
+/// Finalize a clock as a #RCL_RAW_STEADY_TIME time source.
+/**
+ * Finalize the clock as a #RCL_RAW_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.
+ *
+ * This function is not thread-safe with any other function operating on the same
+ * clock object.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No [1]
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * <i>[1] Function is reentrant, but concurrent calls on the same `clock` object are not safe.
+ *        Thread-safety is also affected by that of the `allocator` object associated with the
+ *        `clock` object.</i>
+ *
+ * \param[in] clock the handle to the clock which is being initialized
+ * \return #RCL_RET_OK if the time source was successfully finalized, 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_raw_steady_clock_fini(
+  rcl_clock_t * clock);
+
 /// Initialize a clock as a #RCL_SYSTEM_TIME time source.
 /**
  * Initialize the clock as a #RCL_SYSTEM_TIME time source.
diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c
@@ -39,6 +39,14 @@ rcl_get_steady_time(void * data, rcl_time_point_value_t * current_time)
   return rcutils_steady_time_now(current_time);
 }
 
+// Implementation only
+static rcl_ret_t
+rcl_get_raw_steady_time(void * data, rcl_time_point_value_t * current_time)
+{
+  (void)data;  // unused
+  return rcutils_raw_steady_time_now(current_time);
+}
+
 // Implementation only
 static rcl_ret_t
 rcl_get_system_time(void * data, rcl_time_point_value_t * current_time)
@@ -111,6 +119,8 @@ rcl_clock_init(
       return rcl_system_clock_init(clock, allocator);
     case RCL_STEADY_TIME:
       return rcl_steady_clock_init(clock, allocator);
+    case RCL_RAW_STEADY_TIME:
+      return rcl_raw_steady_clock_init(clock, allocator);
     default:
       return RCL_RET_INVALID_ARGUMENT;
   }
@@ -142,6 +152,8 @@ rcl_clock_fini(
       return rcl_system_clock_fini(clock);
     case RCL_STEADY_TIME:
       return rcl_steady_clock_fini(clock);
+    case RCL_RAW_STEADY_TIME:
+      return rcl_raw_steady_clock_fini(clock);
     case RCL_CLOCK_UNINITIALIZED:
     // fall through
     default:
@@ -213,6 +225,32 @@ rcl_steady_clock_fini(
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+rcl_raw_steady_clock_init(
+  rcl_clock_t * clock,
+  rcl_allocator_t * allocator)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT);
+  rcl_init_generic_clock(clock, allocator);
+  clock->get_now = rcl_get_raw_steady_time;
+  clock->type = RCL_RAW_STEADY_TIME;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_raw_steady_clock_fini(
+  rcl_clock_t * clock)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT);
+  if (clock->type != RCL_RAW_STEADY_TIME) {
+    RCL_SET_ERROR_MSG("clock not of type RCL_RAW_STEADY_TIME");
+    return RCL_RET_ERROR;
+  }
+  rcl_clock_generic_fini(clock);
+  return RCL_RET_OK;
+}
+
 rcl_ret_t
 rcl_system_clock_init(
   rcl_clock_t * clock,
diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c
@@ -544,18 +544,27 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     }
   }
 
-  int64_t min_next_call_time[RCL_STEADY_TIME + 1];
-  rcl_clock_t * clocks[RCL_STEADY_TIME + 1] = {NULL, NULL, NULL, NULL};
+  int64_t min_next_call_time[RCL_RAW_STEADY_TIME + 1];
+  rcl_clock_t * clocks[RCL_RAW_STEADY_TIME + 1] = {NULL, NULL, NULL, NULL};
 
   // asserts to make sure nobody changes the ordering of RCL_ROS_TIME,
-  // RCL_SYSTEM_TIME and RCL_STEADY_TIME
-  static_assert(RCL_ROS_TIME < RCL_STEADY_TIME + 1, "RCL_ROS_TIME won't fit in the array");
-  static_assert(RCL_SYSTEM_TIME < RCL_STEADY_TIME + 1, "RCL_SYSTEM_TIME won't fit in the array");
-  static_assert(RCL_STEADY_TIME < RCL_STEADY_TIME + 1, "RCL_STEADY_TIME won't fit in the array");
+  // RCL_SYSTEM_TIME, RCL_STEADY_TIME and RCL_RAW_STEADY_TIME
+  static_assert(RCL_ROS_TIME < RCL_RAW_STEADY_TIME + 1, "RCL_ROS_TIME won't fit in the array");
+  static_assert(
+    RCL_SYSTEM_TIME < RCL_RAW_STEADY_TIME + 1,
+    "RCL_SYSTEM_TIME won't fit in the array");
+  static_assert(
+    RCL_STEADY_TIME < RCL_RAW_STEADY_TIME + 1,
+    "RCL_STEADY_TIME won't fit in the array");
+  static_assert(
+    RCL_RAW_STEADY_TIME < RCL_RAW_STEADY_TIME + 1,
+    "RCL_RAW_STEADY_TIME won't fit in the array");
 
   min_next_call_time[RCL_ROS_TIME] = INT64_MAX;
   min_next_call_time[RCL_SYSTEM_TIME] = INT64_MAX;
   min_next_call_time[RCL_STEADY_TIME] = INT64_MAX;
+  min_next_call_time[RCL_RAW_STEADY_TIME] = INT64_MAX;
+
 
   if (!is_non_blocking) {
     for (size_t t_idx = 0; t_idx < wait_set->impl->timer_index; ++t_idx) {
@@ -623,7 +632,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     int64_t min_timeout = has_valid_timeout ? timeout : INT64_MAX;
 
     // determine the min timeout of all clocks
-    for (size_t i = RCL_ROS_TIME; i <= RCL_STEADY_TIME; i++) {
+    for (size_t i = RCL_ROS_TIME; i <= RCL_RAW_STEADY_TIME; i++) {
       if (clocks[i] == NULL) {
         continue;
       }
diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp
@@ -239,6 +239,22 @@ TEST(rcl_time, default_clock_instanciation) {
   });
   ASSERT_TRUE(rcl_clock_valid(steady_clock));
 
+  auto * raw_steady_clock = static_cast<rcl_clock_t *>(
+    allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state));
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    allocator.deallocate(raw_steady_clock, allocator.state);
+  });
+  retval = rcl_raw_steady_clock_init(raw_steady_clock, &allocator);
+  EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str;
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(
+      RCL_RET_OK, rcl_raw_steady_clock_fini(
+        raw_steady_clock)) << rcl_get_error_string().str;
+  });
+  ASSERT_TRUE(rcl_clock_valid(raw_steady_clock));
+
   auto * system_clock = static_cast<rcl_clock_t *>(
     allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state));
   OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
@@ -272,6 +288,9 @@ TEST(rcl_time, specific_clock_instantiation) {
     EXPECT_EQ(
       rcl_steady_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str;
     rcl_reset_error();
+    EXPECT_EQ(
+      rcl_raw_steady_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str;
+    rcl_reset_error();
     EXPECT_EQ(
       rcl_system_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str;
     rcl_reset_error();
@@ -303,6 +322,15 @@ TEST(rcl_time, specific_clock_instantiation) {
     ret = rcl_clock_fini(&steady_clock);
     EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
   }
+  {
+    rcl_clock_t raw_steady_clock;
+    rcl_ret_t ret = rcl_clock_init(RCL_RAW_STEADY_TIME, &raw_steady_clock, &allocator);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    EXPECT_EQ(raw_steady_clock.type, RCL_RAW_STEADY_TIME) <<
+      "Expected time source of type RCL_RAW_STEADY_TIME";
+    ret = rcl_clock_fini(&raw_steady_clock);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  }
   {
     rcl_clock_t fail_clock;
     rcl_clock_type_t undefined_type = (rcl_clock_type_t) 130;
@@ -344,6 +372,14 @@ TEST(rcl_time, rcl_clock_time_started) {
     ret = rcl_clock_fini(&steady_clock);
     EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
   }
+  {
+    rcl_clock_t raw_steady_clock;
+    rcl_ret_t ret = rcl_clock_init(RCL_RAW_STEADY_TIME, &raw_steady_clock, &allocator);
+    ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    ASSERT_TRUE(rcl_clock_time_started(&raw_steady_clock));
+    ret = rcl_clock_fini(&raw_steady_clock);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  }
 }
 
 TEST(rcl_time, rcl_time_difference) {
