diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index d56ec36127..c9272b124e 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -138,38 +138,37 @@ class Clock Context::SharedPtr context = contexts::get_global_default_context()); /** - * Check if the clock is valid. + * Check if the clock is started. * - * A valid clock is a clock that has a non-zero time. - * An invalid clock is an uninitialized clock (i.e. not rcl_clock_valid) or a clock with an - * uninitialized time (i.e. with zero time.) + * 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. * - * Note: - * A valid clock must be both rcl_clock_valid and hold a time that is rcl_time_point_valid. - * An invalid clock can potentially become valid if it is rcl_clock_valid. - * - * \return true if clock was or became valid + * \return true if clock is started + * \throws std::runtime_error if the clock is not rcl_clock_valid */ RCLCPP_PUBLIC bool - is_valid(); + started(); /** - * Wait for clock to become valid. + * Wait until clock to start. * + * \rclcpp::Clock::started * \param context the context to wait in - * \return true if clock was or became valid + * \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_for_valid(Context::SharedPtr context = contexts::get_global_default_context()); + wait_until_started(Context::SharedPtr context = contexts::get_global_default_context()); /** * Wait for clock to become valid, with timeout. * * The timeout is waited in system 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). @@ -178,7 +177,7 @@ class Clock */ RCLCPP_PUBLIC bool - wait_for_valid( + 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))); diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index c8a1da0f2b..15533f39ef 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -210,16 +210,6 @@ class Time rcl_clock_type_t get_clock_type() const; - /// Check if time is valid. - /** - * A valid time is a time that is non-zero. - * - * \return true if the time is valid - */ - RCLCPP_PUBLIC - bool - is_valid() const; - private: rcl_time_point_t rcl_time_; friend Clock; // Allow clock to manipulate internal data diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 2460f99111..b5b2f26cbd 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -183,48 +183,34 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context) } bool -Clock::is_valid() +Clock::started() { - // Checks for null pointer, missing get_now() implementation, and RCL_CLOCK_UNINITIALIZED if (!rcl_clock_valid(get_clock_handle())) { - RCUTILS_LOG_WARN("clock is uninitialized and will never become valid"); - return false; - } - - switch (get_clock_type()) { - case RCL_ROS_TIME: - case RCL_STEADY_TIME: - case RCL_SYSTEM_TIME: - return now().is_valid(); - - // By right we shouldn't even get to this block, but these cases are included for completeness - case RCL_CLOCK_UNINITIALIZED: - default: - return false; + throw std::runtime_error("clock is not rcl_clock_valid"); } + return rcl_clock_time_started(get_clock_handle()); } bool -Clock::wait_for_valid(Context::SharedPtr context) +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 (is_valid()) { + if (started()) { return true; } else { - // Wait until the first valid time + // Wait until the first non-zero time return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context); } } bool -Clock::wait_for_valid( +Clock::wait_until_started( const Duration & timeout, Context::SharedPtr context, const Duration & wait_tick_ns) @@ -232,7 +218,6 @@ Clock::wait_for_valid( 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"); } @@ -240,7 +225,7 @@ Clock::wait_for_valid( Clock timeout_clock = Clock(RCL_STEADY_TIME); Time start = timeout_clock.now(); - while (!is_valid() && context->is_valid()) { // Context check checks for rclcpp::shutdown() + while (!started() && context->is_valid()) { // Context check checks for rclcpp::shutdown() if (timeout < wait_tick_ns) { timeout_clock.sleep_for(timeout); } else { @@ -248,11 +233,10 @@ Clock::wait_for_valid( } if (timeout_clock.now() - start > timeout) { - return is_valid(); + return started(); } } - - return is_valid(); + return started(); } diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index 42716ae33d..556a5e69ad 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -233,12 +233,6 @@ Time::get_clock_type() const return rcl_time_.clock_type; } -bool -Time::is_valid() const -{ - return rcl_time_point_valid(const_cast(&rcl_time_)); -} - Time operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs) { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 39b0b84ad2..61ef44f9f4 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -452,18 +452,6 @@ TEST_F(TestTime, test_overflow_underflow_throws) { std::underflow_error("addition leads to int64_t underflow")); } -TEST_F(TestTime, validity) { - EXPECT_FALSE(rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED).is_valid()); - EXPECT_FALSE(rclcpp::Time(0, 0, RCL_SYSTEM_TIME).is_valid()); - EXPECT_FALSE(rclcpp::Time(0, 0, RCL_STEADY_TIME).is_valid()); - EXPECT_FALSE(rclcpp::Time(0, 0, RCL_ROS_TIME).is_valid()); - - EXPECT_TRUE(rclcpp::Time(0, 1, RCL_CLOCK_UNINITIALIZED).is_valid()); - EXPECT_TRUE(rclcpp::Time(0, 1, RCL_SYSTEM_TIME).is_valid()); - EXPECT_TRUE(rclcpp::Time(0, 1, RCL_STEADY_TIME).is_valid()); - EXPECT_TRUE(rclcpp::Time(0, 1, RCL_ROS_TIME).is_valid()); -} - class TestClockSleep : public ::testing::Test { protected: @@ -862,7 +850,7 @@ TEST_F(TestClockSleep, sleep_for_basic_ros) { EXPECT_TRUE(sleep_succeeded); } -class TestClockValid : public ::testing::Test +class TestClockStarted : public ::testing::Test { protected: void SetUp() @@ -876,40 +864,49 @@ class TestClockValid : public ::testing::Test } }; -TEST_F(TestClockValid, validity) { +TEST_F(TestClockStarted, started) { 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, static_cast(1e7)))); + auto ros_clock_handle = ros_clock.get_clock_handle(); + + 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.is_valid()); - EXPECT_TRUE(system_clock.wait_for_valid()); - EXPECT_TRUE(system_clock.wait_for_valid(rclcpp::Duration(0, static_cast(1e7)))); + 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.is_valid()); - EXPECT_TRUE(steady_clock.wait_for_valid()); - EXPECT_TRUE(steady_clock.wait_for_valid(rclcpp::Duration(0, static_cast(1e7)))); + 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); - EXPECT_FALSE(uninit_clock.is_valid()); RCLCPP_EXPECT_THROW_EQ( - uninit_clock.wait_for_valid(rclcpp::Duration(0, static_cast(1e7))), + uninit_clock.started(), std::runtime_error("clock is not rcl_clock_valid")); + EXPECT_FALSE(uninit_clock.started()); + 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(TestClockValid, invalid_timeout) { - auto ros_clock = std::make_shared(RCL_ROS_TIME); - auto ros_clock_handle = ros_clock->get_clock_handle(); +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_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->is_valid()); - EXPECT_FALSE(ros_clock->wait_for_valid(rclcpp::Duration(0, static_cast(1e7)))); + 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)); @@ -917,7 +914,6 @@ TEST_F(TestClockValid, invalid_timeout) { }); // Test rclcpp shutdown escape hatch (otherwise this waits indefinitely) - EXPECT_FALSE(ros_clock->wait_for_valid()); - + EXPECT_FALSE(ros_clock.wait_until_started()); t.join(); }