Implement validity checks for rclcpp::Clock (ros2#2040) (ros2#2210)

(cherry picked from commit c091fe1)

Co-authored-by: methylDragon <methylDragon@gmail.com>

rclcpp/include/rclcpp/clock.hpp

@@ -137,6 +137,51 @@ class Clock
     Duration rel_time,
     Context::SharedPtr context = contexts::get_global_default_context());
 
+  /**
+   * Check if the clock is started.
+   *
+   * A started clock is a clock that reflects non-zero time.
+   * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and
+   * nothing has been published on the clock topic yet.
+   *
+   * \return true if clock is started
+   * \throws std::runtime_error if the clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  started();
+
+  /**
+   * Wait until clock to start.
+   *
+   * \rclcpp::Clock::started
+   * \param context the context to wait in
+   * \return true if clock was already started or became started
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Wait for clock to start, with timeout.
+   *
+   * The timeout is waited in steady time.
+   *
+   * \rclcpp::Clock::started
+   * \param timeout the maximum time to wait for.
+   * \param context the context to wait in.
+   * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds).
+   * \return true if clock was or became valid
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(
+    const rclcpp::Duration & timeout,
+    Context::SharedPtr context = contexts::get_global_default_context(),
+    const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
+
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *

rclcpp/src/rclcpp/clock.cpp

@@ -182,6 +182,71 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context)
   return sleep_until(now() + rel_time, context);
 }
 
+bool
+Clock::started()
+{
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock is not rcl_clock_valid");
+  }
+  return rcl_clock_time_started(get_clock_handle());
+}
+
+bool
+Clock::wait_until_started(Context::SharedPtr context)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  if (started()) {
+    return true;
+  } else {
+    // Wait until the first non-zero time
+    return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context);
+  }
+}
+
+bool
+Clock::wait_until_started(
+  const Duration & timeout,
+  Context::SharedPtr context,
+  const Duration & wait_tick_ns)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  Clock timeout_clock = Clock(RCL_STEADY_TIME);
+  Time start = timeout_clock.now();
+
+  // Check if the clock has started every wait_tick_ns nanoseconds
+  // Context check checks for rclcpp::shutdown()
+  while (!started() && context->is_valid()) {
+    if (timeout < wait_tick_ns) {
+      timeout_clock.sleep_for(timeout);
+    } else {
+      Duration time_left = start + timeout - timeout_clock.now();
+      if (time_left > wait_tick_ns) {
+        timeout_clock.sleep_for(Duration(wait_tick_ns));
+      } else {
+        timeout_clock.sleep_for(time_left);
+      }
+    }
+
+    if (timeout_clock.now() - start > timeout) {
+      return started();
+    }
+  }
+  return started();
+}
+
+
 bool
 Clock::ros_time_is_active()
 {

rclcpp/test/rclcpp/test_time.cpp

@@ -19,6 +19,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <thread>
 
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
@@ -848,3 +849,72 @@ TEST_F(TestClockSleep, sleep_for_basic_ros) {
   sleep_thread.join();
   EXPECT_TRUE(sleep_succeeded);
 }
+
+class TestClockStarted : public ::testing::Test
+{
+protected:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestClockStarted, started) {
+  // rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  // auto ros_clock_handle = ros_clock.get_clock_handle();
+  //
+  // // 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)
+  // EXPECT_TRUE(ros_clock.started());
+  // EXPECT_TRUE(ros_clock.wait_until_started());
+  // EXPECT_TRUE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
+  // EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle));
+  // EXPECT_TRUE(ros_clock.ros_time_is_active());
+  // EXPECT_FALSE(ros_clock.started());
+  // EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 1));
+  // EXPECT_TRUE(ros_clock.started());
+  //
+  // rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
+  // EXPECT_TRUE(system_clock.started());
+  // EXPECT_TRUE(system_clock.wait_until_started());
+  // EXPECT_TRUE(system_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
+  //
+  // rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+  // EXPECT_TRUE(steady_clock.started());
+  // EXPECT_TRUE(steady_clock.wait_until_started());
+  // EXPECT_TRUE(steady_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
+  //
+  // rclcpp::Clock uninit_clock(RCL_CLOCK_UNINITIALIZED);
+  // RCLCPP_EXPECT_THROW_EQ(
+  //   uninit_clock.started(), std::runtime_error("clock is not rcl_clock_valid"));
+  // RCLCPP_EXPECT_THROW_EQ(
+  //   uninit_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))),
+  //   std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"));
+}
+
+TEST_F(TestClockStarted, started_timeout) {
+  rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  auto ros_clock_handle = ros_clock.get_clock_handle();
+
+  EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle));
+  EXPECT_TRUE(ros_clock.ros_time_is_active());
+
+  EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 0));
+
+  EXPECT_FALSE(ros_clock.started());
+  EXPECT_FALSE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
+
+  std::thread t([]() {
+      std::this_thread::sleep_for(std::chrono::seconds(1));
+      rclcpp::shutdown();
+    });
+
+  // Test rclcpp shutdown escape hatch (otherwise this waits indefinitely)
+  EXPECT_FALSE(ros_clock.wait_until_started());
+  t.join();
+}