Implement rcl_clock_time_started

rcl/include/rcl/time.h

@@ -168,6 +168,32 @@ typedef struct rcl_time_point_s
 // } rcl_rate_t;
 // TODO(tfoote) integrate rate and timer implementations
 
+/// Check if the clock has started.
+/**
+ * This function returns true if the clock contains a time point value
+ * that is non-zero.
+ * Note that if data is uninitialized it may give a false positive.
+ *
+ * This function is primarily used to check if a clock using ROS time
+ * has started. This is because it is possible that a simulator might be
+ * initialized paused, causing ROS time to be 0 until it is unpaused.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] clock the handle to the clock which is being queried
+ * \return true if the clock has started, otherwise return false.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+bool
+rcl_clock_time_started(rcl_clock_t * clock);
+
 /// Check if the clock has valid values.
 /**
  * This function returns true if the time source appears to be valid.

rcl/src/rcl/time.c

@@ -72,6 +72,16 @@ rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time)
   return RCL_RET_OK;
 }
 
+bool
+rcl_clock_time_started(rcl_clock_t * clock)
+{
+  rcl_time_point_value_t query_now;
+  if (rcl_clock_get_now(clock, &query_now) == RCL_RET_OK) {
+    return query_now > 0;
+  }
+  return false;
+}
+
 bool
 rcl_clock_valid(rcl_clock_t * clock)
 {

rcl/test/rcl/test_time.cpp

@@ -318,6 +318,38 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) {
   }
 }
 
+TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_time_started) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  {
+    rcl_clock_t ros_clock;
+    rcl_ret_t ret = rcl_clock_init(RCL_ROS_TIME, &ros_clock, &allocator);
+    // At this point, the ROS clock is reading system time since the ROS time override isn't on
+    // So we expect it to be started (it's extremely unlikely that system time is at epoch start)
+    ASSERT_TRUE(rcl_clock_time_started(&ros_clock));
+    ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&ros_clock)) << rcl_get_error_string().str;
+    ASSERT_FALSE(rcl_clock_time_started(&ros_clock));
+    ret = rcl_set_ros_time_override(&ros_clock, 1);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+    ASSERT_TRUE(rcl_clock_time_started(&ros_clock));
+    ret = rcl_clock_fini(&ros_clock);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  }
+  {
+    rcl_clock_t system_clock;
+    rcl_ret_t ret = rcl_clock_init(RCL_SYSTEM_TIME, &system_clock, &allocator);
+    ASSERT_TRUE(rcl_clock_time_started(&system_clock));  // As long as system time isn't 0
+    ret = rcl_clock_fini(&system_clock);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  }
+  {
+    rcl_clock_t steady_clock;
+    rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &steady_clock, &allocator);
+    ASSERT_TRUE(rcl_clock_time_started(&steady_clock));
+    ret = rcl_clock_fini(&steady_clock);
+    EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+  }
+}
+
 TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) {
   rcl_ret_t ret;
   rcl_time_point_t a, b;