Add rcutils_raw_steady_time_now method for slew-free clock (ros2#507) (ros2#514)

(cherry picked from commit 89264ce)

Signed-off-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>

Author: mergify[bot]

README.md

@@ -54,6 +54,7 @@ The API is a combination of parts:
 - Portable implementations of "get system time" and "get steady time":
   - rcutils_system_time_now()
   - rcutils_steady_time_now()
+  - rcutils_raw_steady_time_now()
   - rcutils/time.h
 - Some useful data structures:
   - A "string array" data structure (analogous to `std::vector<std::string>`):

include/rcutils/time.h

@@ -104,6 +104,33 @@ RCUTILS_WARN_UNUSED
 rcutils_ret_t
 rcutils_steady_time_now(rcutils_time_point_value_t * now);
 
+/// Retrieve the current time as a rcutils_time_point_value_t object.
+/**
+ * This function returns the time from a monotonically increasing slew-free clock.
+ *
+ * The resolution (e.g. nanoseconds vs microseconds) is not guaranteed.
+ *
+ * The now argument must point to an allocated rcutils_time_point_value_t object,
+ * as the result is copied into this variable.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[out] now a struct in which the current time is stored
+ * \return #RCUTILS_RET_OK if the current time was successfully obtained, or
+ * \return #RCUTILS_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCUTILS_RET_ERROR if an unspecified error occur.
+ */
+RCUTILS_PUBLIC
+RCUTILS_WARN_UNUSED
+rcutils_ret_t
+rcutils_raw_steady_time_now(rcutils_time_point_value_t * now);
+
 /// Return a time point as nanoseconds in a string.
 /**
  * The number is always fixed width, with left padding zeros up to the maximum

src/time_unix.c

@@ -16,6 +16,7 @@
 # error time_unix.c is not intended to be used with win32 based systems
 #endif  // defined(_WIN32)
 
+#include "rcutils/logging_macros.h"
 #include "rcutils/time.h"
 
 #if defined(__MACH__) && defined(__APPLE__)
@@ -98,3 +99,30 @@ rcutils_steady_time_now(rcutils_time_point_value_t * now)
   *now = RCUTILS_S_TO_NS((int64_t)timespec_now.tv_sec) + timespec_now.tv_nsec;
   return RCUTILS_RET_OK;
 }
+
+rcutils_ret_t
+rcutils_raw_steady_time_now(rcutils_time_point_value_t * now)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(now, RCUTILS_RET_INVALID_ARGUMENT);
+  struct timespec timespec_now;
+
+#if defined(CLOCK_MONOTONIC_RAW)
+  clockid_t monotonic_raw_clock = CLOCK_MONOTONIC_RAW;
+#else
+  clockid_t monotonic_raw_clock = CLOCK_MONOTONIC;
+  RCUTILS_LOG_WARN_ONCE(
+    "CLOCK_MONOTONIC_RAW is not supported by the platform, using CLOCK_MONOTONIC "
+    "instead. This may not provide the desired raw steady time behavior.");
+#endif
+
+  if (clock_gettime(monotonic_raw_clock, &timespec_now) < 0) {
+    RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to get raw steady time: %d", errno);
+    return RCUTILS_RET_ERROR;
+  }
+  if (would_be_negative(&timespec_now)) {
+    RCUTILS_SET_ERROR_MSG("unexpected negative time");
+    return RCUTILS_RET_ERROR;
+  }
+  *now = RCUTILS_S_TO_NS((int64_t)timespec_now.tv_sec) + timespec_now.tv_nsec;
+  return RCUTILS_RET_OK;
+}

src/time_win32.c

@@ -84,6 +84,15 @@ rcutils_steady_time_now(rcutils_time_point_value_t * now)
   return RCUTILS_RET_OK;
 }
 
+rcutils_ret_t
+rcutils_raw_steady_time_now(rcutils_time_point_value_t * now)
+{
+  // On Windows, there is no difference between steady and raw steady time.
+  // The QueryPerformanceCounter function provides a high-resolution timer
+  // that is not affected by system clock changes.
+  return rcutils_steady_time_now(now);
+}
+
 #ifdef __cplusplus
 }
 #endif

test/test_time.cpp

@@ -191,6 +191,47 @@ TEST_F(TestTimeFixture, test_rcutils_steady_time_now) {
     llabs(steady_diff - sc_diff), RCUTILS_MS_TO_NS(k_tolerance_ms)) << "steady_clock differs";
 }
 
+// Tests the rcutils_raw_steady_time_now() function.
+TEST_F(TestTimeFixture, test_rcutils_raw_steady_time_now) {
+  rcutils_ret_t ret;
+  // Check for invalid argument error condition (allowed to alloc).
+  ret = rcutils_raw_steady_time_now(nullptr);
+  EXPECT_EQ(ret, RCUTILS_RET_INVALID_ARGUMENT) << rcutils_get_error_string().str;
+  rcutils_reset_error();
+  // Check for normal operation (not allowed to alloc).
+  rcutils_time_point_value_t now = 0;
+  EXPECT_NO_MEMORY_OPERATIONS(
+  {
+    ret = rcutils_raw_steady_time_now(&now);
+  });
+  EXPECT_EQ(ret, RCUTILS_RET_OK) << rcutils_get_error_string().str;
+  EXPECT_NE(0u, now);
+  // Compare to std::chrono::steady_clock difference of two times (within a second).
+  now = 0;
+  EXPECT_NO_MEMORY_OPERATIONS(
+  {
+    ret = rcutils_raw_steady_time_now(&now);
+  });
+  std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now();
+  EXPECT_EQ(ret, RCUTILS_RET_OK) << rcutils_get_error_string().str;
+  // Wait for a little while.
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  // Then take a new timestamp with each and compare.
+  rcutils_time_point_value_t later;
+  EXPECT_NO_MEMORY_OPERATIONS(
+  {
+    ret = rcutils_raw_steady_time_now(&later);
+  });
+  std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now();
+  EXPECT_EQ(ret, RCUTILS_RET_OK) << rcutils_get_error_string().str;
+  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), RCUTILS_MS_TO_NS(k_tolerance_ms)) << "steady_clock differs";
+}
+
 #if !defined(_WIN32)
 
 TEST_F(TestTimeFixture, test_rcutils_with_bad_system_clocks) {
@@ -211,6 +252,10 @@ TEST_F(TestTimeFixture, test_rcutils_with_bad_system_clocks) {
     ret = rcutils_steady_time_now(&now);
     EXPECT_EQ(RCUTILS_RET_ERROR, ret);
     rcutils_reset_error();
+
+    ret = rcutils_raw_steady_time_now(&now);
+    EXPECT_EQ(RCUTILS_RET_ERROR, ret);
+    rcutils_reset_error();
   }
   {
     auto mock = mocking_utils::patch(
@@ -229,6 +274,10 @@ TEST_F(TestTimeFixture, test_rcutils_with_bad_system_clocks) {
     ret = rcutils_steady_time_now(&now);
     EXPECT_EQ(RCUTILS_RET_ERROR, ret);
     rcutils_reset_error();
+
+    ret = rcutils_raw_steady_time_now(&now);
+    EXPECT_EQ(RCUTILS_RET_ERROR, ret);
+    rcutils_reset_error();
   }
 }