first work toward adding ros time support, includes unit tests of bas…

…ic functionality.

rcl/include/rcl/time.h

@@ -64,6 +64,24 @@ typedef struct rcl_steady_time_point_t
   uint64_t nanoseconds;
 } rcl_steady_time_point_t;
 
+/// Struct which encapsulates a point in time according to the ros clock.
+/* The ros 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 ros clock will be subject to published simulation_time.
+ *
+ * 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_ros_time_point_t
+{
+  uint64_t nanoseconds;
+} rcl_ros_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.
@@ -123,6 +141,102 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_steady_time_point_now(rcl_steady_time_point_t * now);
 
+/// Retrieve the current time as a rcl_ros_time_point_t struct.
+/* This function returns the time from a ros clock unless simulation_time
+ * is active. In which case it returns the latest simulated time.
+ * The closest equivalent would be to std::chrono::steady_clock::now();
+ *
+ * The resolution is nanoseconds.
+ *
+ * The now argument must point to an allocated rcl_ros_time_point_t struct,
+ * as the result is copied into this variable.
+ *
+ * This function may allocate heap memory when an error occurs.
+ * This function is thread-safe.
+ * This function is lock-free, with an exception on Windows.
+ * 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
+ * \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.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_ros_time_point_now(rcl_ros_time_point_t * now);
+
+
+/// Set the current time as a rcl_ros_time_point_t struct.
+/* This function sets the time for the simulation_time. If unset it will set
+ * simulation_time active.
+ *
+ * This function is intended only to be used by rcl internals to set
+ * the simulated time.
+ *
+ * The resolution is nanoseconds.
+ *
+ * The now argument must point to an allocated rcl_ros_time_point_t struct,
+ * as the value is copied out of this variable.
+ *
+ * This function may allocate heap memory when an error occurs.
+ * This function is thread-safe.
+ * This function is lock-free, with an exception on Windows.
+ * 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[in] now a struct containing the current time to be 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.
+ */
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_set_ros_time_point_now(rcl_ros_time_point_t * now);
+
+
+/// Disable ros time override to simulation time.
+/* This function turns off the simulation time override.
+ * When simulation time is enabled ros time will reflect
+ * the simulated time.
+ * \return RCL_RET_OK if the current time was successfully obtained, or
+ *         RCL_RET_ERROR an unspecified error occur.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_disable_ros_time_point(void);
+
+/// Enable ros time override to simulation time.
+/* This function turns on the simulation time override.
+ * When simulation time is enabled ros time will reflect
+ * the simulated time.
+ * \return RCL_RET_OK if the current time was successfully obtained, or
+ *         RCL_RET_ERROR an unspecified error occur.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_enable_ros_time_point(void);
+
+
+/// Get wiether ros time override to simulation time is enabled.
+/* This function turns on the simulation time override.
+ * When simulation time is enabled ros time will reflect
+ * the simulated time.
+ * \param[out] is_enabled whether simulation time is active.
+ * \return RCL_RET_OK if the value was queried correctly.
+ *         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_ros_time_point_enabled(bool * is_enabled);
+
 #if __cplusplus
 }
 #endif

rcl/src/rcl/time.c

@@ -17,3 +17,54 @@
 #else  // defined(WIN32)
 #include "./time_unix.c"
 #endif  // defined(WIN32)
+
+#include "./stdatomic_helper.h"
+
+
+// Storage for ros time
+static atomic_bool __rcl_ros_time_is_active = ATOMIC_VAR_INIT(false);
+static atomic_uint_least64_t __rcl_ros_time_storage = ATOMIC_VAR_INIT(0);
+
+rcl_ret_t
+rcl_ros_time_point_now(rcl_ros_time_point_t * now)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
+  if (!rcl_atomic_load_bool(&__rcl_ros_time_is_active)) {
+    rcl_system_time_point_t sys_time;
+    rcl_ret_t retcode = rcl_system_time_point_now(&sys_time);
+    now->nanoseconds = sys_time.nanoseconds;
+    return retcode;
+  }
+  now->nanoseconds = rcl_atomic_load_uint64_t(&__rcl_ros_time_storage);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_set_ros_time_point_now(rcl_ros_time_point_t * now)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
+  atomic_store(&__rcl_ros_time_storage, now->nanoseconds);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_disable_ros_time_point(void)
+{
+  atomic_store(&__rcl_ros_time_is_active, false);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_enable_ros_time_point(void)
+{
+  atomic_store(&__rcl_ros_time_is_active, true);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_is_ros_time_point_enabled(bool * is_enabled)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(is_enabled, RCL_RET_INVALID_ARGUMENT);
+  *is_enabled = rcl_atomic_load_bool(&__rcl_ros_time_is_active);
+  return RCL_RET_OK;
+}

rcl/test/rcl/test_time.cpp

@@ -118,3 +118,94 @@ TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) {
   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_ros_time_point_now() function.
+ */
+TEST_F(TestTimeFixture, test_rcl_ros_time_point_now) {
+  assert_no_realloc_begin();
+  rcl_ret_t ret;
+  // Check for invalid argument error condition (allowed to alloc).
+  ret = rcl_ros_time_point_now(nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
+  rcl_reset_error();
+  ret = rcl_is_ros_time_point_enabled(nullptr);
+  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
+  assert_no_malloc_begin();
+  assert_no_free_begin();
+  // Check for normal operation (not allowed to alloc).
+  rcl_ros_time_point_t now = {0};
+  ret = rcl_ros_time_point_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, 0);
+  // Compare to std::chrono::system_clock time (within a second).
+  now = {0};
+  ret = rcl_ros_time_point_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)) << "ros_clock differs";
+  }
+  // Test ros time specific APIs
+  rcl_ros_time_point_t set_point = {1000000000ull};
+  bool is_enabled;
+  // Check initialized state
+  ret = rcl_is_ros_time_point_enabled(&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_point_now(&set_point);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  // check still disabled
+  ret = rcl_is_ros_time_point_enabled(&is_enabled);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(is_enabled, false);
+  rcl_ros_time_point_t retrieved_point;
+  // get real
+  ret = rcl_ros_time_point_now(&retrieved_point);
+  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 = retrieved_point.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_point();
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  // check enabled
+  ret = rcl_is_ros_time_point_enabled(&is_enabled);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(is_enabled, true);
+  // get sim
+  ret = rcl_ros_time_point_now(&retrieved_point);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(retrieved_point.nanoseconds, set_point.nanoseconds);
+  // disable
+  ret = rcl_disable_ros_time_point();
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  // check disabled
+  ret = rcl_is_ros_time_point_enabled(&is_enabled);
+  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
+  EXPECT_EQ(is_enabled, false);
+  // get real
+  ret = rcl_ros_time_point_now(&retrieved_point);
+  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 = retrieved_point.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";
+  }
+}