Skip to content

Commit

Permalink
initialize timer with clock
Browse files Browse the repository at this point in the history
  • Loading branch information
@dirk-thomas
dirk-thomas committed Jul 23, 2018
1 parent d3a734f commit 252bdaf
Show file tree
Hide file tree
Showing 4 changed files with 56 additions and 15 deletions.
1 change: 1 addition & 0 deletions rcl/include/rcl/timer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,7 @@ RCL_WARN_UNUSED
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);
Expand Down
19 changes: 12 additions & 7 deletions rcl/src/rcl/timer.c
View file
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 = clock->get_now(clock->data, &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 Down Expand Up @@ -105,18 +110,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 = timer->impl->clock->get_now(timer->impl->clock->data, &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
36 changes: 30 additions & 6 deletions rcl/test/rcl/test_timer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,13 +50,19 @@ class TestTimerFixture : public ::testing::Test

TEST_F(TestTimerFixture, test_two_timers) {
rcl_ret_t ret;

rcl_clock_t clock;
rcl_allocator_t allocator = rcl_get_default_allocator();
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_timer_t timer = rcl_get_zero_initialized_timer();
rcl_timer_t timer2 = rcl_get_zero_initialized_timer();

ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

ret = rcl_timer_init(&timer2, RCL_MS_TO_NS(20), nullptr, rcl_get_default_allocator());
ret = rcl_timer_init(&timer2, &clock, RCL_MS_TO_NS(20), nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
Expand Down Expand Up @@ -95,13 +101,19 @@ TEST_F(TestTimerFixture, test_two_timers) {

TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) {
rcl_ret_t ret;

rcl_clock_t clock;
rcl_allocator_t allocator = rcl_get_default_allocator();
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_timer_t timer = rcl_get_zero_initialized_timer();
rcl_timer_t timer2 = rcl_get_zero_initialized_timer();

ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

ret = rcl_timer_init(&timer2, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
ret = rcl_timer_init(&timer2, &clock, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
Expand Down Expand Up @@ -140,9 +152,15 @@ TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) {

TEST_F(TestTimerFixture, test_timer_not_ready) {
rcl_ret_t ret;

rcl_clock_t clock;
rcl_allocator_t allocator = rcl_get_default_allocator();
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_timer_t timer = rcl_get_zero_initialized_timer();

ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
Expand Down Expand Up @@ -175,9 +193,15 @@ TEST_F(TestTimerFixture, test_timer_not_ready) {

TEST_F(TestTimerFixture, test_canceled_timer) {
rcl_ret_t ret;

rcl_clock_t clock;
rcl_allocator_t allocator = rcl_get_default_allocator();
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_timer_t timer = rcl_get_zero_initialized_timer();

ret = rcl_timer_init(&timer, 500, nullptr, rcl_get_default_allocator());
ret = rcl_timer_init(&timer, &clock, 500, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

ret = rcl_timer_cancel(&timer);
Expand Down
15 changes: 13 additions & 2 deletions rcl/test/rcl/test_wait.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,8 +103,13 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) {
ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_clock_t clock;
rcl_allocator_t allocator = rcl_get_default_allocator();
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_timer_t timer = rcl_get_zero_initialized_timer();
ret = rcl_timer_init(&timer, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_wait_set_add_timer(&wait_set, &timer);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
Expand Down Expand Up @@ -205,8 +210,14 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) {
ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_clock_t clock;
rcl_allocator_t allocator = rcl_get_default_allocator();
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_timer_t canceled_timer = rcl_get_zero_initialized_timer();
ret = rcl_timer_init(&canceled_timer, RCL_MS_TO_NS(1), nullptr, rcl_get_default_allocator());
ret = rcl_timer_init(
&canceled_timer, &clock, RCL_MS_TO_NS(1), nullptr, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
ret = rcl_timer_cancel(&canceled_timer);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
Expand Down

0 comments on commit 252bdaf

Please sign in to comment.