Add Clock::sleep_until method (#1748)

emersonknapp · web-flow · commit 81df5843f321 · 2021-09-24T15:15:23.000-04:00
* Add Clock::sleep_until method

Handle all 3 clock types, and edge cases for shutdown and enabling/disabling ROS time

Signed-off-by: Emerson Knapp &lt;eknapp@amazon.com&gt;
diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp
@@ -78,6 +78,25 @@ class Clock
   Time
   now();
 
+  /**
+   * Sleep until a specified Time, according to clock type.
+   *
+   * Notes for RCL_ROS_TIME clock type:
+   *   - Can sleep forever if ros time is active and received clock never reaches `until`
+   *   - If ROS time enabled state changes during the sleep, this method will immediately return
+   *     false. There is not a consistent choice of sleeping time when the time source changes,
+   *     so this is up to the caller to call again if needed.
+   *
+   * \param until absolute time according to current clock type to sleep until.
+   * \return true immediately if `until` is in the past
+   * \return true when the time `until` is reached
+   * \return false if time cannot be reached reliably, for example from shutdown or a change
+   *    of time source.
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_until(Time until);
+
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *
diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp
@@ -14,10 +14,13 @@
 
 #include "rclcpp/clock.hpp"
 
+#include <condition_variable>
 #include <memory>
 #include <thread>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/utilities.hpp"
 
 #include "rcutils/logging_macros.h"
 
@@ -47,6 +50,8 @@ class Clock::Impl
   rcl_clock_t rcl_clock_;
   rcl_allocator_t allocator_;
   std::mutex clock_mutex_;
+  std::condition_variable cv_;
+  rclcpp::OnShutdownCallbackHandle shutdown_cb_;
 };
 
 JumpHandler::JumpHandler(
@@ -59,9 +64,18 @@ JumpHandler::JumpHandler(
 {}
 
 Clock::Clock(rcl_clock_type_t clock_type)
-: impl_(new Clock::Impl(clock_type)) {}
+: impl_(new Clock::Impl(clock_type))
+{
+  impl_->shutdown_cb_ = rclcpp::contexts::get_global_default_context()->add_on_shutdown_callback(
+    [this]() {
+      impl_->cv_.notify_all();
+    });
+}
 
-Clock::~Clock() {}
+Clock::~Clock()
+{
+  rclcpp::contexts::get_global_default_context()->remove_on_shutdown_callback(impl_->shutdown_cb_);
+}
 
 Time
 Clock::now()
@@ -76,6 +90,85 @@ Clock::now()
   return now;
 }
 
+bool
+Clock::sleep_until(Time until)
+{
+  const auto this_clock_type = get_clock_type();
+  if (until.get_clock_type() != this_clock_type) {
+    RCUTILS_LOG_ERROR(
+      "sleep_until Time clock type %d does not match this clock's type %d.",
+      until.get_clock_type(), this_clock_type);
+    return false;
+  }
+  bool time_source_changed = false;
+
+  if (this_clock_type == RCL_STEADY_TIME) {
+    auto steady_time = std::chrono::steady_clock::time_point(
+      std::chrono::nanoseconds(until.nanoseconds()));
+
+    // loop over spurious wakeups but notice shutdown
+    std::unique_lock lock(impl_->clock_mutex_);
+    while (now() < until && rclcpp::ok()) {
+      impl_->cv_.wait_until(lock, steady_time);
+    }
+  } else if (this_clock_type == RCL_SYSTEM_TIME) {
+    auto system_time = std::chrono::time_point<
+      std::chrono::system_clock, std::chrono::nanoseconds>(
+      std::chrono::nanoseconds(until.nanoseconds()));
+
+    // loop over spurious wakeups but notice shutdown
+    std::unique_lock lock(impl_->clock_mutex_);
+    while (now() < until && rclcpp::ok()) {
+      impl_->cv_.wait_until(lock, system_time);
+    }
+  } else if (this_clock_type == RCL_ROS_TIME) {
+    // Install jump handler for any amount of time change, for two purposes:
+    // - if ROS time is active, check if time reached on each new clock sample
+    // - Trigger via on_clock_change to detect if time source changes, to invalidate sleep
+    rcl_jump_threshold_t threshold;
+    threshold.on_clock_change = true;
+    threshold.min_backward.nanoseconds = 0;
+    threshold.min_forward.nanoseconds = 0;
+    auto clock_handler = create_jump_callback(
+      []() {},
+      [this](const rcl_time_jump_t &) {impl_->cv_.notify_all();},
+      threshold);
+
+    try {
+      if (!ros_time_is_active()) {
+        auto system_time = std::chrono::time_point<
+          std::chrono::system_clock, std::chrono::nanoseconds>(
+          std::chrono::nanoseconds(until.nanoseconds()));
+
+        // loop over spurious wakeups but notice shutdown or time source change
+        std::unique_lock lock(impl_->clock_mutex_);
+        while (now() < until && rclcpp::ok() && !ros_time_is_active()) {
+          impl_->cv_.wait_until(lock, system_time);
+        }
+        time_source_changed = ros_time_is_active();
+      } else {
+        // RCL_ROS_TIME with ros_time_is_active.
+        // Just wait without "until" because installed
+        // jump callbacks wake the cv on every new sample.
+        std::unique_lock lock(impl_->clock_mutex_);
+        while (now() < until && rclcpp::ok() && ros_time_is_active()) {
+          impl_->cv_.wait(lock);
+        }
+        time_source_changed = !ros_time_is_active();
+      }
+    } catch (...) {
+      RCUTILS_LOG_ERROR("Unexpected exception from ros_time_is_active()");
+      return false;
+    }
+  }
+
+  if (!rclcpp::ok() || time_source_changed) {
+    return false;
+  }
+
+  return now() >= until;
+}
+
 bool
 Clock::ros_time_is_active()
 {
diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp
@@ -17,13 +17,15 @@
 #include <algorithm>
 #include <chrono>
 #include <limits>
+#include <memory>
 #include <string>
 
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/time.hpp"
+#include "rclcpp/time_source.hpp"
 #include "rclcpp/utilities.hpp"
 
 #include "../utils/rclcpp_gtest_macros.hpp"
@@ -447,3 +449,154 @@ TEST_F(TestTime, test_overflow_underflow_throws) {
     test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1),
     std::underflow_error("addition leads to int64_t underflow"));
 }
+
+class TestClockSleep : public ::testing::Test
+{
+protected:
+  void SetUp()
+  {
+    // Shutdown in case there was a dangling global context from other test fixtures
+    rclcpp::shutdown();
+    rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("clock_sleep_node");
+    param_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+    ASSERT_TRUE(param_client->wait_for_service(5s));
+  }
+
+  void TearDown()
+  {
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::SyncParametersClient::SharedPtr param_client;
+};
+
+TEST_F(TestClockSleep, bad_clock_type) {
+  rclcpp::Clock clock(RCL_SYSTEM_TIME);
+  rclcpp::Time steady_until(12345, 0, RCL_STEADY_TIME);
+  ASSERT_FALSE(clock.sleep_until(steady_until));
+
+  rclcpp::Time ros_until(54321, 0, RCL_ROS_TIME);
+  ASSERT_FALSE(clock.sleep_until(ros_until));
+}
+
+TEST_F(TestClockSleep, sleep_until_basic_system) {
+  static const auto MILLION = 1000L * 1000L;
+  const auto milliseconds = 300;
+  rclcpp::Clock clock(RCL_SYSTEM_TIME);
+  auto delay = rclcpp::Duration(0, milliseconds * MILLION);
+  auto sleep_until = clock.now() + delay;
+
+  auto start = std::chrono::system_clock::now();
+  ASSERT_TRUE(clock.sleep_until(sleep_until));
+  auto end = std::chrono::system_clock::now();
+
+  EXPECT_GE(clock.now(), sleep_until);
+  EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds));
+}
+
+TEST_F(TestClockSleep, sleep_until_basic_steady) {
+  static const auto MILLION = 1000L * 1000L;
+  const auto milliseconds = 300;
+  rclcpp::Clock clock(RCL_STEADY_TIME);
+  auto delay = rclcpp::Duration(0, milliseconds * MILLION);
+  auto sleep_until = clock.now() + delay;
+
+  auto steady_start = std::chrono::steady_clock::now();
+  ASSERT_TRUE(clock.sleep_until(sleep_until));
+  auto steady_end = std::chrono::steady_clock::now();
+
+  EXPECT_GE(clock.now(), sleep_until);
+  EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds));
+}
+
+TEST_F(TestClockSleep, sleep_until_steady_past_returns_immediately) {
+  rclcpp::Clock clock(RCL_STEADY_TIME);
+  auto until = clock.now() - rclcpp::Duration(1000, 0);
+  // This should return immediately, other possible behavior might be sleep forever and timeout
+  ASSERT_TRUE(clock.sleep_until(until));
+}
+
+TEST_F(TestClockSleep, sleep_until_system_past_returns_immediately) {
+  rclcpp::Clock clock(RCL_SYSTEM_TIME);
+  auto until = clock.now() - rclcpp::Duration(1000, 0);
+  // This should return immediately, other possible behavior might be sleep forever and timeout
+  ASSERT_TRUE(clock.sleep_until(until));
+}
+
+TEST_F(TestClockSleep, sleep_until_ros_time_enable_interrupt) {
+  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  rclcpp::TimeSource time_source;
+  time_source.attachNode(node);
+  time_source.attachClock(clock);
+
+  // 5 second timeout, but it should be interrupted right away
+  const auto until = clock->now() + rclcpp::Duration(5, 0);
+
+  // Try sleeping with ROS time off, then turn it on to interrupt
+  bool sleep_succeeded = true;
+  auto sleep_thread = std::thread(
+    [clock, until, &sleep_succeeded]() {
+      sleep_succeeded = clock->sleep_until(until);
+    });
+  // yield execution long enough to let the sleep thread get to waiting on the condition variable
+  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  auto set_parameters_results = param_client->set_parameters(
+    {rclcpp::Parameter("use_sim_time", true)});
+  for (auto & result : set_parameters_results) {
+    ASSERT_TRUE(result.successful);
+  }
+  sleep_thread.join();
+  EXPECT_FALSE(sleep_succeeded);
+}
+
+TEST_F(TestClockSleep, sleep_until_ros_time_disable_interrupt) {
+  param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)});
+  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  rclcpp::TimeSource time_source;
+  time_source.attachNode(node);
+  time_source.attachClock(clock);
+
+  // /clock shouldn't be publishing, shouldn't be possible to reach timeout
+  const auto until = clock->now() + rclcpp::Duration(600, 0);
+
+  // Try sleeping with ROS time off, then turn it on to interrupt
+  bool sleep_succeeded = true;
+  auto sleep_thread = std::thread(
+    [clock, until, &sleep_succeeded]() {
+      sleep_succeeded = clock->sleep_until(until);
+    });
+  // yield execution long enough to let the sleep thread get to waiting on the condition variable
+  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  auto set_parameters_results = param_client->set_parameters(
+    {rclcpp::Parameter("use_sim_time", false)});
+  for (auto & result : set_parameters_results) {
+    ASSERT_TRUE(result.successful);
+  }
+  sleep_thread.join();
+  EXPECT_FALSE(sleep_succeeded);
+}
+
+TEST_F(TestClockSleep, sleep_until_shutdown_interrupt) {
+  param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)});
+  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  rclcpp::TimeSource time_source;
+  time_source.attachNode(node);
+  time_source.attachClock(clock);
+
+  // the timeout doesn't matter here - no /clock is being published, so it should never wake
+  const auto until = clock->now() + rclcpp::Duration(600, 0);
+
+  bool sleep_succeeded = true;
+  auto sleep_thread = std::thread(
+    [clock, until, &sleep_succeeded]() {
+      sleep_succeeded = clock->sleep_until(until);
+    });
+  // yield execution long enough to let the sleep thread get to waiting on the condition variable
+  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  rclcpp::shutdown();
+  sleep_thread.join();
+  EXPECT_FALSE(sleep_succeeded);
+}
diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp
@@ -742,3 +742,36 @@ TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) {
   // Node should have get out of timer callback
   ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen());
 }
+
+TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) {
+  SimClockPublisherNode pub_node;
+  pub_node.SpinNode();
+
+  node->set_parameter({"use_sim_time", true});
+  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  rclcpp::TimeSource time_source;
+  time_source.attachNode(node);
+  time_source.attachClock(clock);
+
+  // Wait until time source has definitely received a first ROS time from the pub node
+  {
+    rcl_jump_threshold_t threshold;
+    threshold.on_clock_change = false;
+    threshold.min_backward.nanoseconds = 0;
+    threshold.min_forward.nanoseconds = 0;
+
+    std::condition_variable cv;
+    std::mutex mutex;
+    auto handler = clock->create_jump_callback(
+      []() {},
+      [&cv](const rcl_time_jump_t &) {cv.notify_all();},
+      threshold);
+    std::unique_lock lock(mutex);
+    cv.wait(lock);
+  }
+
+  auto now = clock->now();
+  // Any amount of time will do, just need to make sure that we awake and return true
+  auto until = now + rclcpp::Duration(0, 500);
+  EXPECT_TRUE(clock->sleep_until(until));
+}
