diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 9d3d585753..197b684cf7 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -140,8 +140,18 @@ class Clock /** * Check if the clock is valid. * - * An invalid clock is either a clock that is RCL_CLOCK_UNINITIALIZED or a clock with zero-time. - * In other words, an uninitialized clock. + * 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.) + * + * Note: + * This notion of validity is different from the rcl notion of validity (rcl_clock_valid)! + * A clock that is rcl_clock_valid is a clock that can be used to get a time (i.e. not + * NULL, not RCL_CLOCK_UNINITIALIZED, or with a missing get_now() method.) + * + * A clock that is valid in the sense codified by this method is a clock with non-zero time. + * Consequently, a clock that is not rcl_clock_valid can never become valid, since it cannot + * be used to obtain a time. * * \return true if clock was or became valid */ @@ -154,6 +164,7 @@ class Clock * * \param context the context to wait in. * \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 @@ -168,6 +179,7 @@ class Clock * \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 + * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid */ RCLCPP_PUBLIC bool diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 70fd248be8..6ada751a43 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -185,12 +185,19 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context) bool Clock::is_valid() { + // 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().nanoseconds() > 0; + // By right we shouldn't even get to this block, but these cases are included for completeness case RCL_CLOCK_UNINITIALIZED: default: return false; @@ -200,13 +207,20 @@ Clock::is_valid() bool Clock::wait_for_valid(Context::SharedPtr context) { - if (get_clock_type() == RCL_CLOCK_UNINITIALIZED) { - throw std::runtime_error( - "clock cannot be waited on as its clock_type is RCL_CLOCK_UNINITIALIZED"); + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); } - // Wait until the first valid time - return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context); + 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()) { + return true; + } else { + // Wait until the first valid time + return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context); + } } bool @@ -219,22 +233,21 @@ Clock::wait_for_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"); + 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_SYSTEM_TIME); + Clock timeout_clock = Clock(RCL_STEADY_TIME); Time start = timeout_clock.now(); - while (!is_valid() && context->is_valid()) { + while (!is_valid() && context->is_valid()) { // Context check checks for rclcpp::shutdown() timeout_clock.sleep_for(Duration(wait_tick_ns)); if (timeout_clock.now() - start > timeout) { - return false; + return is_valid(); } } - return context->is_valid(); + return is_valid(); }