rcl_ret_t
rcl_take_request(
const rcl_service_t * service,
rmw_request_id_t * request_header,
void * ros_request)
{
RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(
service->impl, "service is invalid", return RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT);
bool taken = false;
if (rmw_take_request(
service->impl->rmw_handle, request_header, ros_request, &taken) != RMW_RET_OK)
{
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
return RCL_RET_ERROR;
}
if (!taken) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
return RCL_RET_CLIENT_TAKE_FAILED;
}
return RCL_RET_OK;
}
rcl_ret_t
rcl_service_init(
rcl_service_t * service,
const rcl_node_t * node,
const rosidl_service_type_support_t * type_support,
const char * service_name,
const rcl_service_options_t * options)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
if (!node->impl) {
RCL_SET_ERROR_MSG("invalid node");
return RCL_RET_NODE_INVALID;
}
RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
if (service->impl) {
RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized");
return RCL_RET_ALREADY_INIT;
}
const rcl_allocator_t * allocator = &options->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);
// Allocate space for the implementation struct.
service->impl = (rcl_service_impl_t *)allocator->allocate(
sizeof(rcl_service_impl_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
service->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
// Fill out implementation struct.
// rmw handle (create rmw service)
// TODO(wjwwood): pass along the allocator to rmw when it supports it
service->impl->rmw_handle = rmw_create_service(
rcl_node_get_rmw_handle(node),
type_support,
service_name,
&options->qos);
if (!service->impl->rmw_handle) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
goto fail;
}
// options
service->impl->options = *options;
return RCL_RET_OK;
fail:
if (service->impl) {
allocator->deallocate(service->impl, allocator->state);
}
return fail_ret;
}
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_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_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_ret_t
rcl_shutdown()
{
if (!rcl_ok()) {
RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init");
return RCL_RET_NOT_INIT;
}
__clean_up_init();
return RCL_RET_OK;
}
rcl_ret_t
rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator)
{
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT);
if (!__wait_set_is_valid(wait_set)) {
RCL_SET_ERROR_MSG("wait set is invalid");
return RCL_RET_WAIT_SET_INVALID;
}
*allocator = wait_set->impl->allocator;
return RCL_RET_OK;
}
rcl_ret_t
rcl_service_fini(rcl_service_t * service, rcl_node_t * node)
{
rcl_ret_t result = RCL_RET_OK;
RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
if (service->impl) {
rmw_ret_t ret =
rmw_destroy_service(service->impl->rmw_handle);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
result = RCL_RET_ERROR;
}
rcl_allocator_t allocator = service->impl->options.allocator;
allocator.deallocate(service->impl, allocator.state);
}
return result;
}
rcl_ret_t
rcl_send_response(
const rcl_service_t * service,
rmw_request_id_t * request_header,
void * ros_response)
{
RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(
service->impl, "service is invalid", return RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT);
if (rmw_send_response(
service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK)
{
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
return RCL_RET_ERROR;
}
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;
}
