Implement validity checks for rclcpp::Clock

Signed-off-by: methylDragon <methylDragon@gmail.com>

rclcpp/include/rclcpp/clock.hpp

@@ -137,6 +137,48 @@ class Clock
     Duration rel_time,
     Context::SharedPtr context = contexts::get_global_default_context());
 
+  /**
+   * Check if the clock is valid.
+   *
+   * An invalid clock is either a clock that is RCL_CLOCK_UNINITIALIZED or is an RCL_ROS_TIME
+   * clock that is using ros time (ros_time_is_active()) but with time 0.
+   *
+   * \return true if clock was or became valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_valid();
+
+  /**
+   * Wait for clock to become valid.
+   *
+   * \param timeout the maximum time to wait for.
+   * \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
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_for_valid(
+    Context::SharedPtr context=contexts::get_global_default_context(),
+    Duration wait_tick_ns=Duration(0, 1e7));
+
+  /**
+   * Wait for clock to become valid, with timeout.
+   *
+   * The timeout is waited in system time.
+   *
+   * \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 the clock is active
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_for_valid(
+    const rclcpp::Duration& timeout,
+    Context::SharedPtr context=contexts::get_global_default_context(),
+    Duration wait_tick_ns=Duration(0, 1e7));
+
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *

rclcpp/src/rclcpp/clock.cpp

@@ -182,6 +182,60 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context)
   return sleep_until(now() + rel_time, context);
 }
 
+bool
+Clock::is_valid()
+{
+  switch (get_clock_type())
+  {
+    case RCL_ROS_TIME:
+    case RCL_STEADY_TIME:
+    case RCL_SYSTEM_TIME:
+      return now().nanoseconds() > 0;
+
+    case RCL_CLOCK_UNINITIALIZED:
+    default:
+      return false;
+  }
+}
+
+bool
+Clock::wait_for_valid(Context::SharedPtr context, Duration wait_tick_ns)
+{
+  return wait_for_valid(Duration(0, 0), context, wait_tick_ns);
+}
+
+bool
+Clock::wait_for_valid(
+  const Duration& timeout,
+  Context::SharedPtr context,
+  Duration wait_tick_ns)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+
+  if (get_clock_type() == RCL_CLOCK_UNINITIALIZED) {
+    throw std::runtime_error(
+      "clock cannot be waited on as its clock_type is RCL_CLOCK_UNINITIALIZED");
+  }
+
+  Clock timeout_clock = Clock(RCL_SYSTEM_TIME);
+  Time start = timeout_clock.now();
+
+  while (!is_valid() && rclcpp::ok(context)) {
+   timeout_clock.sleep_for(Duration(wait_tick_ns));
+   if (timeout > rclcpp::Duration(0, 0) && (timeout_clock.now() - start > timeout)) {
+     return false;
+   }
+  }
+
+  if (!rclcpp::ok(context)) {
+   return false;
+  }
+  return true;
+}
+
+
 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,217 @@ TEST_F(TestClockSleep, sleep_for_basic_ros) {
   sleep_thread.join();
   EXPECT_TRUE(sleep_succeeded);
 }
+
+class TestClockValid : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+};
+
+void spin_until_time(
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Node::SharedPtr node,
+  std::chrono::nanoseconds end_time,
+  bool expect_time_update)
+{
+  // Call spin_once on the node until either:
+  // 1) We see the ros_clock's simulated time change to the expected end_time
+  // -or-
+  // 2) 1 second has elapsed in the real world
+  // If 'expect_time_update' is True, and we timed out waiting for simulated time to
+  // update, we'll have the test fail
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+
+  auto start = std::chrono::system_clock::now();
+  while (std::chrono::system_clock::now() < (start + 1s)) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+
+    executor.spin_once(10ms);
+
+    if (clock->now().nanoseconds() >= end_time.count()) {
+      return;
+    }
+  }
+
+  if (expect_time_update) {
+    // If we were expecting ROS clock->now to be updated and we didn't take the early return from
+    // the loop up above, that's a failure
+    ASSERT_TRUE(false) << "Timed out waiting for ROS time to update";
+  }
+}
+
+void spin_until_ros_time_updated(
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Node::SharedPtr node,
+  rclcpp::ParameterValue value)
+{
+  // Similar to above:  Call spin_once until we see the clock's ros_time_is_active method
+  // match the ParameterValue
+  // Unlike spin_until_time, there aren't any test cases where we don't expect the value to
+  // update.  In the event that the ParameterValue is not set, we'll pump messages for a full second
+  // but we don't cause the test to fail
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+
+  auto start = std::chrono::system_clock::now();
+  while (std::chrono::system_clock::now() < (start + 2s)) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+
+    executor.spin_once(10ms);
+
+    // In the case where we didn't intend to change the parameter, we'll still pump
+    if (value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      continue;
+    }
+
+    if (clock->ros_time_is_active() == value.get<bool>()) {
+      return;
+    }
+  }
+}
+
+void trigger_clock_changes(
+  rclcpp::Node::SharedPtr node,
+  std::shared_ptr<rclcpp::Clock> clock,
+  bool expect_time_update = true,
+  bool zero = false)
+{
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock", 10);
+
+  for (int i = 0; i < 5; ++i) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+    rosgraph_msgs::msg::Clock msg;
+
+    if (zero) {
+      msg.clock.sec = 0;
+      msg.clock.nanosec = 0;
+    } else {
+      msg.clock.sec = i;
+      msg.clock.nanosec = 1000;
+    }
+    clock_pub->publish(msg);
+
+    // workaround.  Long-term, there can be a more elegant fix where we hook a future up
+    // to a clock change callback and spin until future complete, but that's an upstream
+    // change
+    if (zero) {
+      spin_until_time(
+        clock,
+        node,
+        std::chrono::nanoseconds(0),
+        expect_time_update
+      );
+    }
+    else {
+      spin_until_time(
+        clock,
+        node,
+        std::chrono::seconds(i) + std::chrono::nanoseconds(1000),
+        expect_time_update
+      );
+    }
+  }
+}
+
+void set_use_sim_time_parameter(
+  rclcpp::Node::SharedPtr node,
+  rclcpp::ParameterValue value,
+  rclcpp::Clock::SharedPtr clock)
+{
+  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+
+  using namespace std::chrono_literals;
+  EXPECT_TRUE(parameters_client->wait_for_service(2s));
+  auto set_parameters_results = parameters_client->set_parameters(
+  {
+    rclcpp::Parameter("use_sim_time", value)
+  });
+  for (auto & result : set_parameters_results) {
+    EXPECT_TRUE(result.successful);
+  }
+
+  // Same as above - workaround for a little bit of asynchronous behavior.  The sim_time paramater
+  // is set synchronously, but the way the ros clock gets notified involves a pub/sub that happens
+  // AFTER the synchronous notification that the parameter was set.  This may also get fixed
+  // upstream
+  spin_until_ros_time_updated(clock, node, value);
+}
+
+TEST_F(TestClockValid, validity) {
+  rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  EXPECT_TRUE(ros_clock.is_valid());
+  EXPECT_TRUE(ros_clock.wait_for_valid());
+  EXPECT_TRUE(ros_clock.wait_for_valid(rclcpp::Duration(0, 1e7)));
+
+  rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
+  EXPECT_TRUE(system_clock.is_valid());
+  EXPECT_TRUE(system_clock.wait_for_valid());
+  EXPECT_TRUE(system_clock.wait_for_valid(rclcpp::Duration(0, 1e7)));
+
+  rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+  EXPECT_TRUE(steady_clock.is_valid());
+  EXPECT_TRUE(steady_clock.wait_for_valid());
+  EXPECT_TRUE(steady_clock.wait_for_valid(rclcpp::Duration(0, 1e7)));
+
+  rclcpp::Clock uninit_clock(RCL_CLOCK_UNINITIALIZED);
+  EXPECT_FALSE(uninit_clock.is_valid());
+  RCLCPP_EXPECT_THROW_EQ(
+    uninit_clock.wait_for_valid(rclcpp::Duration(0, 1e7)),
+    std::runtime_error("clock cannot be waited on as its clock_type is RCL_CLOCK_UNINITIALIZED"));
+}
+
+TEST_F(TestClockValid, invalid_timeout) {
+  // We need to borrow some logic from the time_source tests to set clock time to (0, 0)
+  rclcpp::TimeSource ts(node);
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  ts.attachClock(ros_clock);
+  trigger_clock_changes(node, ros_clock, false);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  trigger_clock_changes(node, ros_clock, true, true);  // Force to time zero
+
+  EXPECT_FALSE(ros_clock->is_valid());
+  EXPECT_FALSE(ros_clock->wait_for_valid(rclcpp::Duration(0, 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_for_valid(rclcpp::Duration(0, 0)));
+
+  t.join();
+}