Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

initialize timer with clock #272

Merged
merged 11 commits into from
Jul 28, 2018
8 changes: 4 additions & 4 deletions rcl/include/rcl/time.h
Original file line number Diff line number Diff line change
Expand Up @@ -272,20 +272,20 @@ rcl_ret_t
rcl_difference_times(
rcl_time_point_t * start, rcl_time_point_t * finish, rcl_duration_t * delta);

/// Fill the time point with the current value of the associated clock.
/// Fill the time point value with the current value of the associated clock.
/**
* This function will populate the data of the time_point object with the
* This function will populate the data of the time_point_value object with the
* current value from it's associated time abstraction.
* \param[in] clock The time source from which to set the value.
* \param[out] time_point The time_point to populate.
* \param[out] time_point_value The time_point value to populate.
* \return `RCL_RET_OK` if the last call time was retrieved successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point);
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value);
Copy link
Member

@dhood dhood Jul 27, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

new functionality in rclpy is relying on the existing signature: https://github.com/ros2/rclpy/pull/209/files#diff-80dda03110b5be606b823d6b0558b5c2R3218 (CI wouldn't have picked up on that because the PR was merged after it). Sorry about that, I didn't realise that merge would cause conflict here (I understood we were going to add a new signature rather than modifying the existing one). I would make a PR for it but ros2/rclpy#211 is already connected and needs to be rebased first

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for pointing it out - no problem. I rebased the rclpy PR and updated it in ros2/rclpy@f938bac to use the modified API.



/// Enable the ROS time abstraction override.
Expand Down
47 changes: 42 additions & 5 deletions rcl/include/rcl/timer.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ extern "C"
#include "rcl/types.h"
#include "rmw/rmw.h"

struct rcl_clock_t;
struct rcl_timer_impl_t;

/// Structure which encapsulates a ROS Timer.
Expand Down Expand Up @@ -60,7 +61,7 @@ rcl_get_zero_initialized_timer(void);

/// Initialize a timer.
/**
* A timer consists of a callback function and a period.
* A timer consists of a clock, a callback function and a period.
* A timer can be added to a wait set and waited on, such that the wait set
* will wake up when a timer is ready to be executed.
*
Expand All @@ -77,6 +78,9 @@ rcl_get_zero_initialized_timer(void);
* Calling this function on a timer struct which has been allocated but not
* zero initialized is undefined behavior.
*
* The clock handle must be a pointer to an initialized rcl_clock_t struct.
* The life time of the clock must exceed the life time of the timer.
*
* The period is a duration (rather an absolute time in the future).
* If the period is `0` then it will always be ready.
*
Expand All @@ -101,9 +105,14 @@ rcl_get_zero_initialized_timer(void);
* // Optionally reconfigure, cancel, or reset the timer...
* }
*
* rcl_clock_t clock;
* rcl_allocator_t allocator = rcl_get_default_allocator();
* rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
* // ... error handling
*
* rcl_timer_t timer = rcl_get_zero_initialized_timer();
* rcl_ret_t ret = rcl_timer_init(
* &timer, RCL_MS_TO_NS(100), my_timer_callback, rcl_get_default_allocator());
* ret = rcl_timer_init(
* &timer, &clock, RCL_MS_TO_NS(100), my_timer_callback, allocator);
* // ... error handling, use the timer with a wait set, or poll it manually, then cleanup
* ret = rcl_timer_fini(&timer);
* // ... error handling
Expand All @@ -123,6 +132,7 @@ rcl_get_zero_initialized_timer(void);
* <i>[3] if `atomic_is_lock_free()` returns true for `atomic_bool`</i>
*
* \param[inout] timer the timer handle to be initialized
* \param[in] clock the clock providing the current time
* \param[in] period the duration between calls to the callback in nanoseconds
* \param[in] callback the user defined function to be called every period
* \param[in] allocator the allocator to use for allocations
Expand All @@ -137,6 +147,7 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_init(
rcl_timer_t * timer,
rcl_clock_t * clock,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

docblock for this function needs to be updated

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Absolutely, packages on-top of rclcpp also haven't been updated yet. Will put it back in review when that is the case and the docblocks are updated too.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated the docblock in e1a400e.

int64_t period,
const rcl_timer_callback_t callback,
rcl_allocator_t allocator);
Expand Down Expand Up @@ -216,6 +227,32 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_call(rcl_timer_t * timer);

/// Retrieve the clock of the timer.
/**
* This function retrieves the clock pointer and copies it into the given variable.
*
* The clock argument must be a pointer to an already allocated rcl_clock_t *.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | Yes
* Uses Atomics | No
* Lock-Free | Yes
*
* \param[in] timer the handle to the timer which is being queried
* \param[out] clock the rcl_clock_t * in which the clock is stored
* \return `RCL_RET_OK` if the period was retrieved successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_TIMER_INVALID` if the timer is invalid, or
* \return `RCL_RET_ERROR` an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock);

/// Calculates whether or not the timer should be called.
/**
* The result is true if the time until next call is less than, or equal to, 0
Expand Down Expand Up @@ -315,7 +352,7 @@ rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, int64_t * time_sin

/// Retrieve the period of the timer.
/**
* This function retrieves the period and copies it into the give variable.
* This function retrieves the period and copies it into the given variable.
*
* The period argument must be a pointer to an already allocated int64_t.
*
Expand Down Expand Up @@ -343,7 +380,7 @@ rcl_timer_get_period(const rcl_timer_t * timer, int64_t * period);
/// Exchange the period of the timer and return the previous period.
/**
* This function exchanges the period in the timer and copies the old one into
* the give variable.
* the given variable.
*
* Exchanging (changing) the period will not affect already waiting wait sets.
*
Expand Down
10 changes: 6 additions & 4 deletions rcl/src/rcl/time.c
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,7 @@ rcl_ret_t
rcl_clock_fini(
rcl_clock_t * clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
switch (clock->type) {
case RCL_ROS_TIME:
return rcl_ros_clock_fini(clock);
Expand Down Expand Up @@ -226,12 +227,13 @@ rcl_difference_times(
}

rcl_ret_t
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point)
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value)
{
RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(
time_point_value, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
if (clock->type && clock->get_now) {
return clock->get_now(clock->data,
&(time_point->nanoseconds));
return clock->get_now(clock->data, time_point_value);
}
RCL_SET_ERROR_MSG(
"clock is not initialized or does not have get_now registered.",
Expand Down
31 changes: 24 additions & 7 deletions rcl/src/rcl/timer.c
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ extern "C"

typedef struct rcl_timer_impl_t
{
// The clock providing time.
rcl_clock_t * clock;
// The user supplied callback.
atomic_uintptr_t callback;
// This is a duration in nanoseconds.
Expand All @@ -50,26 +52,29 @@ rcl_get_zero_initialized_timer()
rcl_ret_t
rcl_timer_init(
rcl_timer_t * timer,
rcl_clock_t * clock,
int64_t period,
const rcl_timer_callback_t callback,
rcl_allocator_t allocator)
{
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, allocator);
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, allocator);
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing timer with period: %" PRIu64 "ns", period)
if (timer->impl) {
RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator);
return RCL_RET_ALREADY_INIT;
}
rcl_time_point_value_t now_steady;
rcl_ret_t now_ret = rcutils_steady_time_now(&now_steady);
rcl_time_point_value_t now;
rcl_ret_t now_ret = rcl_clock_get_now(clock, &now);
if (now_ret != RCL_RET_OK) {
return now_ret; // rcl error state should already be set.
}
rcl_timer_impl_t impl;
impl.clock = clock;
atomic_init(&impl.callback, (uintptr_t)callback);
atomic_init(&impl.period, period);
atomic_init(&impl.last_call_time, now_steady);
atomic_init(&impl.last_call_time, now);
atomic_init(&impl.canceled, false);
impl.allocator = allocator;
timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
Expand All @@ -92,6 +97,18 @@ rcl_timer_fini(rcl_timer_t * timer)
return result;
}

RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID, rcl_get_default_allocator());
*clock = timer->impl->clock;
return RCL_RET_OK;
}

rcl_ret_t
rcl_timer_call(rcl_timer_t * timer)
{
Expand All @@ -105,18 +122,18 @@ rcl_timer_call(rcl_timer_t * timer)
RCL_SET_ERROR_MSG("timer is canceled", *allocator);
return RCL_RET_TIMER_CANCELED;
}
rcl_time_point_value_t now_steady;
rcl_ret_t now_ret = rcutils_steady_time_now(&now_steady);
rcl_time_point_value_t now;
rcl_ret_t now_ret = rcl_clock_get_now(timer->impl->clock, &now);
if (now_ret != RCL_RET_OK) {
return now_ret; // rcl error state should already be set.
}
rcl_time_point_value_t previous_ns =
rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady);
rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now);
rcl_timer_callback_t typed_callback =
(rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);

if (typed_callback != NULL) {
int64_t since_last_call = now_steady - previous_ns;
int64_t since_last_call = now - previous_ns;
typed_callback(timer, since_last_call);
}
return RCL_RET_OK;
Expand Down
14 changes: 7 additions & 7 deletions rcl/test/rcl/test_time.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
ret = rcl_is_enabled_ros_time_override(nullptr, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
rcl_time_point_t query_now;
rcl_time_point_value_t query_now;
bool is_enabled;
ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
Expand All @@ -86,15 +86,15 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
ret = rcl_clock_get_now(&ros_clock, &query_now);
});
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_NE(query_now.nanoseconds, 0u);
EXPECT_NE(query_now, 0u);
// Compare to std::chrono::system_clock time (within a second).
ret = rcl_clock_get_now(&ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
{
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
int64_t now_ns_int = now_ns.count();
int64_t now_diff = query_now.nanoseconds - now_ns_int;
int64_t now_diff = query_now - now_ns_int;
const int k_tolerance_ms = 1000;
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
}
Expand All @@ -118,7 +118,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
int64_t now_ns_int = now_ns.count();
int64_t now_diff = query_now.nanoseconds - now_ns_int;
int64_t now_diff = query_now - now_ns_int;
const int k_tolerance_ms = 1000;
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
}
Expand All @@ -132,7 +132,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
// get sim
ret = rcl_clock_get_now(&ros_clock, &query_now);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_EQ(query_now.nanoseconds, set_point);
EXPECT_EQ(query_now, set_point);
// disable
ret = rcl_disable_ros_time_override(&ros_clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
Expand All @@ -147,7 +147,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
int64_t now_ns_int = now_ns.count();
int64_t now_diff = query_now.nanoseconds - now_ns_int;
int64_t now_diff = query_now - now_ns_int;
const int k_tolerance_ms = 1000;
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
}
Expand Down Expand Up @@ -352,7 +352,7 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) {
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
rcl_time_point_t query_now;
rcl_time_point_value_t query_now;
rcl_ret_t ret;
rcl_time_point_value_t set_point = 1000000000ull;

Expand Down
Loading