rcl_ret_t rcl_init(int argc, char ** argv, rcl_allocator_t allocator) { rcl_ret_t fail_ret = RCL_RET_ERROR; if (argc > 0) { RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT); } RCL_CHECK_FOR_NULL_WITH_MSG( allocator.allocate, "invalid allocator, allocate not set", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( allocator.deallocate, "invalid allocator, deallocate not set", return RCL_RET_INVALID_ARGUMENT); if (rcl_atomic_exchange_bool(&__rcl_is_initialized, true)) { RCL_SET_ERROR_MSG("rcl_init called while already initialized"); return RCL_RET_ALREADY_INIT; } // There is a race condition between the time __rcl_is_initialized is set true, // and when the allocator is set, in which rcl_shutdown() could get rcl_ok() as // true and try to use the allocator, but it isn't set yet... // A very unlikely race condition, but it is possile I think. // I've documented that rcl_init() and rcl_shutdown() are not thread-safe with each other. __rcl_allocator = allocator; // Set the new allocator. // TODO(wjwwood): Remove rcl specific command line arguments. // For now just copy the argc and argv. __rcl_argc = argc; __rcl_argv = (char **)__rcl_allocator.allocate(sizeof(char *) * argc, __rcl_allocator.state); if (!__rcl_argv) { RCL_SET_ERROR_MSG("allocation failed"); fail_ret = RCL_RET_BAD_ALLOC; goto fail; } memset(__rcl_argv, 0, sizeof(char **) * argc); int i; for (i = 0; i < argc; ++i) { __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state); memcpy(__rcl_argv[i], argv[i], strlen(argv[i])); } rcl_atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id); if (rcl_atomic_load_uint64_t(&__rcl_instance_id) == 0) { // Roll over occurred. __rcl_next_unique_id--; // roll back to avoid the next call succeeding. RCL_SET_ERROR_MSG("unique rcl instance ids exhausted"); goto fail; } return RCL_RET_OK; fail: __clean_up_init(); 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); if (timer->impl) { RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } 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_timer_impl_t impl; atomic_init(&impl.callback, (uintptr_t)callback); atomic_init(&impl.period, period); atomic_init(&impl.last_call_time, now_steady); atomic_init(&impl.canceled, false); impl.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_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; }