static void
__clean_up_init()
{
if (__rcl_argv) {
int i;
for (i = 0; i < __rcl_argc; ++i) {
if (__rcl_argv[i]) {
// Use the old allocator.
__rcl_allocator.deallocate(__rcl_argv[i], __rcl_allocator.state);
}
}
// Use the old allocator.
__rcl_allocator.deallocate(__rcl_argv, __rcl_allocator.state);
}
__rcl_argc = 0;
__rcl_argv = NULL;
rcl_atomic_store(&__rcl_instance_id, 0);
rcl_atomic_store(&__rcl_is_initialized, false);
}
rcl_wait_set_t
rcl_get_zero_initialized_wait_set()
{
static rcl_wait_set_t null_wait_set = {
.subscriptions = NULL,
.size_of_subscriptions = 0,
.guard_conditions = NULL,
.size_of_guard_conditions = 0,
.timers = NULL,
.size_of_timers = 0,
.impl = NULL,
};
return null_wait_set;
}
static bool
__wait_set_is_valid(const rcl_wait_set_t * wait_set)
{
return wait_set && wait_set->impl;
}
static void
__wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator)
{
if (wait_set->subscriptions) {
rcl_ret_t ret = rcl_wait_set_resize_subscriptions(wait_set, 0);
(void)ret; // NO LINT
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->guard_conditions) {
rcl_ret_t ret = rcl_wait_set_resize_guard_conditions(wait_set, 0);
(void)ret; // NO LINT
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->timers) {
rcl_ret_t ret = rcl_wait_set_resize_timers(wait_set, 0);
(void)ret; // NO LINT
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->impl) {
allocator.deallocate(wait_set->impl, allocator.state);
wait_set->impl = NULL;
}
}
rcl_ret_t
rcl_wait_set_init(
rcl_wait_set_t * wait_set,
size_t number_of_subscriptions,
size_t number_of_guard_conditions,
size_t number_of_timers,
rcl_allocator_t allocator)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
if (__wait_set_is_valid(wait_set)) {
RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized.");
return RCL_RET_ALREADY_INIT;
}
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);
RCL_CHECK_FOR_NULL_WITH_MSG(
allocator.reallocate, "reallocate not set", return RCL_RET_INVALID_ARGUMENT);
// Allocate space for the implementation struct.
wait_set->impl = (rcl_wait_set_impl_t *)allocator.allocate(
sizeof(rcl_wait_set_impl_t), allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG(
wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
wait_set->impl->rmw_subscriptions.subscribers = NULL;
wait_set->impl->rmw_subscriptions.subscriber_count = 0;
wait_set->impl->rmw_guard_conditions.guard_conditions = NULL;
wait_set->impl->rmw_guard_conditions.guard_condition_count = 0;
// Initialize subscription space.
rcl_ret_t ret;
if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_subscriptions(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Initialize guard condition space.
ret = rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions);
if (ret != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_guard_conditions(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Initialize timer space.
ret = rcl_wait_set_resize_timers(wait_set, number_of_timers);
if (ret != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_timers(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Set allocator.
wait_set->impl->allocator = allocator;
return RCL_RET_OK;
fail:
__wait_set_clean_up(wait_set, allocator);
return fail_ret;
}
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;
}
