rcl_ret_t rcl_timer_call(rcl_timer_t * timer) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); if (rcl_atomic_load_bool(&timer->impl->canceled)) { RCL_SET_ERROR_MSG("timer is canceled"); return RCL_RET_TIMER_CANCELED; } rcl_time_point_value_t now_steady; rcl_ret_t now_ret = rcl_steady_time_now(&now_steady); 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_timer_callback_t typed_callback = (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback); if (typed_callback != NULL) { uint64_t since_last_call = now_steady - previous_ns; typed_callback(timer, since_last_call); } return RCL_RET_OK; }
rcl_ret_t rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); *old_period = rcl_atomic_exchange_uint64_t(&timer->impl->period, new_period); return RCL_RET_OK; }
rcl_ret_t rcl_timer_init( rcl_timer_t * timer, uint64_t period, const rcl_timer_callback_t callback, rcl_allocator_t allocator) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT); if (timer->impl) { RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } rcl_steady_time_point_t now_steady; rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } rcl_timer_impl_t impl = { .callback = ATOMIC_VAR_INIT((uintptr_t)callback), .period = ATOMIC_VAR_INIT(period), .last_call_time = ATOMIC_VAR_INIT(now_steady.nanoseconds), .canceled = ATOMIC_VAR_INIT(false), .allocator = allocator, }; RCL_CHECK_FOR_NULL_WITH_MSG( allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); *timer->impl = impl; return RCL_RET_OK; } rcl_ret_t rcl_timer_fini(rcl_timer_t * timer) { if (!timer || !timer->impl) { return RCL_RET_OK; } // Will return either RCL_RET_OK or RCL_RET_ERROR since the timer is valid. rcl_ret_t result = rcl_timer_cancel(timer); rcl_allocator_t allocator = timer->impl->allocator; allocator.deallocate(timer->impl, allocator.state); return result; } rcl_ret_t rcl_timer_call(rcl_timer_t * timer) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); if (rcl_atomic_load_bool(&timer->impl->canceled)) { RCL_SET_ERROR_MSG("timer is canceled"); return RCL_RET_TIMER_CANCELED; } rcl_steady_time_point_t now_steady; rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } uint64_t previous_ns = rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady.nanoseconds); uint64_t since_last_call = now_steady.nanoseconds - previous_ns; rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback); typed_callback(timer, since_last_call); return RCL_RET_OK; }