diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 1e08e1d8fd..702b224d53 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/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(1e7))); + /** * 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 b34f328690..f46649a77d 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/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() { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 2f188b2d7c..f3969d3886 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #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(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(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(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(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(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(); +}