Skip to content

Commit

Permalink
Update docstrings and refine code
Browse files Browse the repository at this point in the history
Signed-off-by: methylDragon <methylDragon@gmail.com>
  • Loading branch information
methylDragon committed Nov 3, 2022
1 parent 6a54627 commit a0091d0
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 14 deletions.
16 changes: 14 additions & 2 deletions rclcpp/include/rclcpp/clock.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand All @@ -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
Expand All @@ -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
Expand Down
37 changes: 25 additions & 12 deletions rclcpp/src/rclcpp/clock.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand All @@ -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();
}


Expand Down

0 comments on commit a0091d0

Please sign in to comment.