diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 9d2ef45b4d..f70e5efeea 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/contexts/default_context.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "rclcpp/visibility_control.hpp" @@ -78,6 +79,30 @@ 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. + * \param context the rclcpp context the clock should use to check that ROS is still initialized. + * \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. + * \throws std::runtime_error if the context is invalid + * \throws std::runtime_error if `until` has a different clock type from this clock + */ + RCLCPP_PUBLIC + bool + sleep_until( + Time until, + Context::SharedPtr context = contexts::get_global_default_context()); + /** * 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 index e9207e6f31..c9e283ebba 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,11 +14,14 @@ #include "rclcpp/clock.hpp" +#include #include #include #include "rclcpp/exceptions.hpp" +#include "rclcpp/utilities.hpp" +#include "rcpputils/scope_exit.hpp" #include "rcutils/logging_macros.h" namespace rclcpp @@ -76,6 +79,99 @@ Clock::now() return now; } +bool +Clock::sleep_until(Time until, Context::SharedPtr context) +{ + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + const auto this_clock_type = get_clock_type(); + if (until.get_clock_type() != this_clock_type) { + throw std::runtime_error("until's clock type does not match this clock's type"); + } + bool time_source_changed = false; + + std::condition_variable cv; + + // Wake this thread if the context is shutdown + rclcpp::OnShutdownCallbackHandle shutdown_cb_handle = context->add_on_shutdown_callback( + [&cv]() { + cv.notify_one(); + }); + // No longer need the shutdown callback when this function exits + auto callback_remover = rcpputils::scope_exit( + [context, &shutdown_cb_handle]() { + context->remove_on_shutdown_callback(shutdown_cb_handle); + }); + + 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 && context->is_valid()) { + cv.wait_until(lock, steady_time); + } + } else if (this_clock_type == RCL_SYSTEM_TIME) { + auto system_time = std::chrono::system_clock::time_point( + // Cast because system clock resolution is too big for nanoseconds on some systems + std::chrono::duration_cast( + std::chrono::nanoseconds(until.nanoseconds()))); + + // loop over spurious wakeups but notice shutdown + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && context->is_valid()) { + 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; + // 0 is disable, so -1 and 1 are smallest possible time changes + threshold.min_backward.nanoseconds = -1; + threshold.min_forward.nanoseconds = 1; + auto clock_handler = create_jump_callback( + nullptr, + [&cv, &time_source_changed](const rcl_time_jump_t & jump) { + if (jump.clock_change != RCL_ROS_TIME_NO_CHANGE) { + time_source_changed = true; + } + cv.notify_one(); + }, + threshold); + + if (!ros_time_is_active()) { + auto system_time = std::chrono::system_clock::time_point( + // Cast because system clock resolution is too big for nanoseconds on some systems + std::chrono::duration_cast( + 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 && context->is_valid() && !time_source_changed) { + cv.wait_until(lock, system_time); + } + } 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 && context->is_valid() && !time_source_changed) { + cv.wait(lock); + } + } + } + + if (!context->is_valid() || 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 index f7889e96f6..26bfe034eb 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "rcl/error_handling.h" @@ -24,7 +25,9 @@ #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" #include "rclcpp/utilities.hpp" +#include "rcutils/time.h" #include "../utils/rclcpp_gtest_macros.hpp" @@ -91,8 +94,8 @@ TEST_F(TestTime, time_sources) { EXPECT_NE(0u, steady_now.nanosec); } -static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; -static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000; +static const int64_t HALF_SEC_IN_NS = RCUTILS_MS_TO_NS(500); +static const int64_t ONE_SEC_IN_NS = RCUTILS_MS_TO_NS(1000); static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; TEST_F(TestTime, conversions) { @@ -447,3 +450,222 @@ 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("clock_sleep_node"); + param_client = std::make_shared(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); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(steady_until), + std::runtime_error("until's clock type does not match this clock's type")); + + rclcpp::Time ros_until(54321, 0, RCL_ROS_TIME); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(ros_until), + std::runtime_error("until's clock type does not match this clock's type")); +} + +TEST_F(TestClockSleep, invalid_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now(); + + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, nullptr), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto uninitialized_context = std::make_shared(); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, uninitialized_context), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto shutdown_context = std::make_shared(); + shutdown_context->init(0, nullptr); + shutdown_context->shutdown("i am a teapot"); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, shutdown_context), + std::runtime_error("context cannot be slept with because it's invalid")); +} + +TEST_F(TestClockSleep, non_global_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now() + rclcpp::Duration(0, 1); + + auto non_global_context = std::make_shared(); + non_global_context->init(0, nullptr); + ASSERT_TRUE(clock.sleep_until(until, non_global_context)); +} + +TEST_F(TestClockSleep, sleep_until_basic_system) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto delay = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + 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) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_STEADY_TIME); + auto delay = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + 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(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(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(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); +} + +TEST_F(TestClockSleep, sleep_until_basic_ros) { + rclcpp::Clock clock(RCL_ROS_TIME); + rcl_clock_t * rcl_clock = clock.get_clock_handle(); + + ASSERT_EQ(RCL_ROS_TIME, clock.get_clock_type()); + + // Not zero, because 0 means time not initialized + const rcl_time_point_value_t start_time = 1337; + const rcl_time_point_value_t end_time = start_time + 1; + + // Initialize time + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(rcl_clock)); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, start_time)); + + const auto until = rclcpp::Time(end_time, RCL_ROS_TIME); + + bool sleep_succeeded = false; + 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)); + + // False because still sleeping + EXPECT_FALSE(sleep_succeeded); + + // Jump time to the end + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, end_time)); + ASSERT_EQ(until, clock.now()); + + sleep_thread.join(); + EXPECT_TRUE(sleep_succeeded); +} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index bbf5d6bc31..4a572c7a70 100644 --- 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(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 = -1; + threshold.min_forward.nanoseconds = 1; + + std::condition_variable cv; + std::mutex mutex; + auto handler = clock->create_jump_callback( + nullptr, + [&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)); +}