niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

some small API changes and initial wait impl

Browse source
This commit is contained in:
William Woodall 2015-12-04 09:37:18 -08:00
parent ad21736113
commit 3e5298bac0
10 changed files with 239 additions and 73 deletions
Download patch file Download diff file Expand all files Collapse all files

1
rcl/CMakeLists.txt
View file

@ -25,6 +25,7 @@ set(${PROJECT_NAME}_sources
src/rcl/rcl.c
src/rcl/subscription.c
src/rcl/wait.c
src/rcl/time.c
)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources})
ament_target_dependencies(${PROJECT_NAME}

2
rcl/include/rcl/guard_condition.h
View file

@ -141,7 +141,7 @@ rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition);
* \return rmw guard_condition handle if successful, otherwise NULL
*/
rmw_guard_condition_t *
rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition);
rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition);
#if __cplusplus
}

6
rcl/include/rcl/node.h
View file

@ -29,12 +29,6 @@ extern "C"
struct rcl_node_impl_t;
/// Handle for a ROS node.
/* This handle can be in one a few stats, which are referred to below:
* - uninitialized: memory allocated, but not initialized to anything
* - invalid: memory allocated but not usable
* - either zero initialized or shutdown (rcl_fini() or rcl_node_fini())
* - valid: memory allocated and usable
*/
typedef struct rcl_node_t {
/// Private implementation pointer.
struct rcl_node_impl_t * impl;

4
rcl/include/rcl/subscription.h
View file

@ -239,7 +239,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription);
* \return options struct if successful, otherwise NULL
*/
const rcl_subscription_options_t *
rcl_subscription_get_options(rcl_subscription_t * subscription);
rcl_subscription_get_options(const rcl_subscription_t * subscription);
/// Return the rmw subscription handle.
/* The handle returned is a pointer to the internally held rmw handle.
@ -257,7 +257,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription);
* \return rmw subscription handle if successful, otherwise NULL
*/
rmw_subscription_t *
rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription);
rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription);
#if __cplusplus
}

14
rcl/include/rcl/time.h
View file

@ -22,8 +22,22 @@ extern "C"
#include "rmw/rmw.h"
#define RCL_S_TO_NS(seconds) seconds * 1000000000
#define RCL_MS_TO_NS(milliseconds) milliseconds * 1000000
#define RCL_US_TO_NS(microseconds) microseconds * 1000
#define RCL_NS_TO_S(nanoseconds) nanoseconds / 1000000000
#define RCL_NS_TO_MS(nanoseconds) nanoseconds / 1000000
#define RCL_NS_TO_US(nanoseconds) nanoseconds / 1000
typedef rmw_time_t rcl_time_t;
rcl_time_t
rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds);
rcl_time_t
rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds);
#if __cplusplus
}
#endif

32
rcl/include/rcl/wait.h
View file

@ -25,7 +25,6 @@ extern "C"
#include "rcl/subscription.h"
#include "rcl/guard_condition.h"
#include "rcl/time.h"
#include "rcl/types.h"
struct rcl_wait_set_impl_t;
@ -35,11 +34,9 @@ typedef struct rcl_wait_set_t {
/// Storage for subscription pointers.
const rcl_subscription_t ** subscriptions;
size_t size_of_subscriptions;
size_t __current_subscription_offset;
/// Storage for guard condition pointers.
const rcl_guard_condition_t ** guard_conditions;
size_t size_of_guard_conditions;
size_t __current_guard_condition_offset;
/// Allocator for storage.
rcl_allocator_t allocator;
/// If set to true, actions like add_subscription will fail until cleared.
@ -240,8 +237,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
* // ... error handling
* ret = rcl_wait_set_add_guard_condition(&wait_set, &gc1);
* // ... error handling
* rcl_time_t timeout = {1, 0}; // 1 second and 0 nanoseconds.
* ret = rcl_wait(&wait_set, &timeout);
* ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1000)); // 1000ms == 1s, passed as ns
* if (ret == RCL_RET_TIMEOUT) {
* continue;
* }
@ -271,14 +267,14 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
* Passing an uninitialized (zero initialized) wait set struct will fail.
* Passing a wait set struct with uninitialized memory is undefined behavior.
*
* If the timeout pointer is NULL then this function will block indefinitely
* until something in the wait set is valid or it is interrupted.
* If the timeout contains 0 for seconds and nanoseconds this function will be
* non-blocking; checking what's ready but not waiting if nothing is ready yet.
* If the timeout contains a non-zero time then this function will return after
* that period of time has elapsed if something in the wait set does not become
* ready before then.
* The timeout's value is not changed.
* The unit of timeout is nanoseconds.
* If the timeout is negative then this function will block indefinitely until
* something in the wait set is valid or it is interrupted.
* If the timeout is 0 then this function will be non-blocking; checking what's
* ready now, but not waiting if nothing is ready yet.
* If the timeout is greater than 0 then this function will return after
* that period of time has elapsed or the wait set becomes ready, which ever
* comes first.
* Passing a timeout struct with uninitialized memory is undefined behavior.
*
* \TODO(wjwwood) this function should probably be thread-safe with itself but
@ -288,16 +284,16 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
* the given wait sets are not the same and non-overlapping in contents.
*
* \param[inout] wait_set the set of things to be waited on and to be pruned if not ready
* \param[in] timeout the time duration to wait something in the wait set to be ready
* \param[in] timeout the duration to wait for the wait set to be ready, in nanoseconds
* \return RCL_RET_OK something in the wait set became ready, or
* RCL_RET_TIMEOUT timeout expired before something was ready, or
* RCL_RET_NOT_INIT wait set is zero initialized, or
* RCL_RET_WAIT_SET_EMPTY wait set contains no items, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or
* RCL_RET_WAIT_SET_EMPTY if the wait set contains no items, or
* RCL_RET_TIMEOUT if the timeout expired before something was ready, or
* RCL_RET_ERROR an unspecified error occur.
*/
rcl_ret_t
rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout);
rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout);
#if __cplusplus
}

2
rcl/src/rcl/guard_condition.c
View file

@ -137,7 +137,7 @@ rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition)
}
rmw_guard_condition_t *
rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition)
rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition)
{
RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(

4
rcl/src/rcl/subscription.c
View file

@ -157,7 +157,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription)
}
const rcl_subscription_options_t *
rcl_subscription_get_options(rcl_subscription_t * subscription)
rcl_subscription_get_options(const rcl_subscription_t * subscription)
{
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
@ -166,7 +166,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription)
}
rmw_subscription_t *
rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription)
rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription)
{
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(

42
rcl/src/rcl/time.c Normal file
View file

@ -0,0 +1,42 @@
// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#if __cplusplus
extern "C"
{
#endif
#include "rcl/time.h"
rcl_time_t
rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds)
{
rcl_time_t result;
result.sec = RCL_NS_TO_S(nanoseconds);
result.nsec = nanoseconds % 1000000000;
return result;
}
rcl_time_t
rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds)
{
rcl_time_t result;
result.sec = RCL_NS_TO_S(nanoseconds);
result.nsec = nanoseconds % 1000000000;
return result;
}
#if __cplusplus
}
#endif

193
rcl/src/rcl/wait.c
View file

@ -19,15 +19,21 @@ extern "C"
#include "rcl/wait.h"
#include <assert.h>
#include <stdatomic.h>
#include <stdbool.h>
#include <string.h>
#include "rcl/error_handling.h"
#include "rcl/time.h"
#include "rmw/rmw.h"
#include "./common.h"
typedef struct rcl_wait_set_impl_t {
void * place_holder; // To prevent size differences between C and C++.
size_t subscription_index;
rmw_subscriptions_t rmw_subscriptions;
size_t guard_condition_index;
rmw_guard_conditions_t rmw_guard_conditions;
} rcl_wait_set_impl_t;
rcl_wait_set_t
@ -43,6 +49,21 @@ __wait_set_is_valid(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_wait_set_resize_subscriptions(wait_set, 0);
}
if (wait_set->guard_conditions) {
rcl_wait_set_resize_guard_conditions(wait_set, 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,
@ -68,26 +89,42 @@ rcl_wait_set_init(
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);
// Right now, the implementation struct is unused, but checked for NULL, so set it.
wait_set->impl = (rcl_wait_set_impl_t *)0xDEADBEEF;
// 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_wait_set_resize_subscriptions(wait_set, number_of_subscriptions);
rcl_wait_set_clear_subscriptions(wait_set);
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.
rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions);
rcl_wait_set_clear_guard_conditions(wait_set);
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;
}
// Set allocator.
wait_set->allocator = allocator;
// Initialize pruned.
wait_set->pruned = false;
return RCL_RET_OK;
fail:
if (wait_set->subscriptions) {
allocator.deallocate(wait_set->subscriptions, allocator.state);
}
if (wait_set->guard_conditions) {
allocator.deallocate(wait_set->guard_conditions, allocator.state);
}
__wait_set_clean_up(wait_set, allocator);
return fail_ret;
}
@ -97,61 +134,83 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set)
rcl_ret_t result = RCL_RET_OK;
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
if (__wait_set_is_valid(wait_set)) {
if (wait_set->subscriptions) {
wait_set->allocator.deallocate(wait_set->subscriptions, wait_set->allocator.state);
}
if (wait_set->guard_conditions) {
wait_set->allocator.deallocate(wait_set->guard_conditions, wait_set->allocator.state);
}
if (wait_set->impl) {
wait_set->impl = NULL;
}
__wait_set_clean_up(wait_set, wait_set->allocator);
}
return result;
}
#define SET_ADD(Type) \
#define SET_ADD(Type, RMWStorage) \
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
RCL_CHECK_ARGUMENT_FOR_NULL(Type, 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; \
} \
if (!(wait_set->__current_##Type##_offset < wait_set->size_of_##Type##s)) { \
if (!(wait_set->impl->Type##_index < wait_set->size_of_##Type##s)) { \
RCL_SET_ERROR_MSG(#Type "s set is full"); \
return RCL_RET_WAIT_SET_FULL; \
} \
wait_set->Type##s[wait_set->__current_##Type##_offset++] = Type; \
size_t current_index = wait_set->impl->Type##_index++; \
wait_set->Type##s[current_index] = Type; \
/* Also place into rmw storage. */ \
rmw_##Type##_t * rmw_handle = rcl_##Type##_get_rmw_##Type##_handle(Type); \
RCL_CHECK_FOR_NULL_WITH_MSG( \
rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR); \
wait_set->impl->RMWStorage[current_index] = rmw_handle->data; \
return RCL_RET_OK;
#define SET_CLEAR(Type) \
#define SET_CLEAR(Type, RMWStorage, RMWCount) \
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, 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; \
} \
memset(wait_set->Type##s, 0, sizeof(rcl_##Type##_t *) * wait_set->size_of_##Type##s); \
wait_set->__current_##Type##_offset = 0; \
wait_set->impl->Type##_index = 0; \
/* Also clear the rmw storage. */ \
memset( \
wait_set->impl->RMWStorage, \
0, \
sizeof(rmw_subscription_t *) * wait_set->impl->RMWCount); \
return RCL_RET_OK;
#define SET_RESIZE(Type) \
#define SET_RESIZE(Type, RMWStorage, RMWCount) \
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
RCL_CHECK_FOR_NULL_WITH_MSG( \
wait_set->impl, "wait set is invalid", return RCL_RET_WAIT_SET_INVALID); \
if (size == wait_set->size_of_##Type##s) { \
return RCL_RET_OK; \
} \
if (size == 0) { \
if (wait_set->Type##s) { \
wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \
} \
wait_set->Type##s = NULL; \
return RCL_RET_OK; \
} \
/* Also deallocate the rmw storage. */ \
if (wait_set->impl->RMWStorage) { \
wait_set->allocator.deallocate(wait_set->impl->RMWStorage, wait_set->allocator.state); \
wait_set->impl->RMWStorage = NULL; \
} \
} \
else { \
wait_set->size_of_##Type##s = 0; \
wait_set->Type##s = (const rcl_##Type##_t **)wait_set->allocator.reallocate( \
wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \
RCL_CHECK_FOR_NULL_WITH_MSG( \
wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \
wait_set->size_of_##Type##s = size; \
/* Also resize the rmw storage. */ \
wait_set->impl->RMWCount = 0; \
wait_set->impl->RMWStorage = (void **)wait_set->allocator.reallocate( \
wait_set->impl->RMWStorage, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \
if (!wait_set->impl->RMWStorage) { \
wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \
wait_set->size_of_##Type##s = 0; \
RCL_SET_ERROR_MSG("allocating memory failed"); \
return RCL_RET_BAD_ALLOC; \
} \
wait_set->impl->RMWCount = size; \
} \
return RCL_RET_OK;
rcl_ret_t
@ -159,19 +218,25 @@ rcl_wait_set_add_subscription(
rcl_wait_set_t * wait_set,
const rcl_subscription_t * subscription)
{
SET_ADD(subscription)
SET_ADD(subscription, rmw_subscriptions.subscribers)
}
rcl_ret_t
rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set)
{
SET_CLEAR(subscription)
SET_CLEAR(
subscription,
rmw_subscriptions.subscribers,
rmw_subscriptions.subscriber_count)
}
rcl_ret_t
rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size)
{
SET_RESIZE(subscription)
SET_RESIZE(
subscription,
rmw_subscriptions.subscribers,
rmw_subscriptions.subscriber_count)
}
rcl_ret_t
@ -179,24 +244,78 @@ rcl_wait_set_add_guard_condition(
rcl_wait_set_t * wait_set,
const rcl_guard_condition_t * guard_condition)
{
SET_ADD(guard_condition)
SET_ADD(guard_condition, rmw_guard_conditions.guard_conditions)
}
rcl_ret_t
rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set)
{
SET_CLEAR(guard_condition)
SET_CLEAR(
guard_condition,
rmw_guard_conditions.guard_conditions,
rmw_guard_conditions.guard_condition_count)
}
rcl_ret_t
rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size)
{
SET_RESIZE(guard_condition)
SET_RESIZE(
guard_condition,
rmw_guard_conditions.guard_conditions,
rmw_guard_conditions.guard_condition_count)
}
rcl_ret_t
rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout)
rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
{
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(timeout, 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;
}
if (wait_set->size_of_subscriptions == 0 && wait_set->size_of_guard_conditions == 0) {
RCL_SET_ERROR_MSG("wait set is empty");
return RCL_RET_WAIT_SET_EMPTY;
}
// Create dummy sets for currently unsupported wait-ables.
static rmw_services_t dummy_services = {0, NULL};
static rmw_clients_t dummy_clients = {0, NULL};
rmw_time_t rmw_timeout = rcl_time_t_from_int64_t_nanoseconds(timeout);
// Wait.
rmw_ret_t ret = rmw_wait(
&wait_set->impl->rmw_subscriptions,
&wait_set->impl->rmw_guard_conditions,
&dummy_services,
&dummy_clients,
&rmw_timeout
);
// Check for timeout.
if (ret == RMW_RET_TIMEOUT) {
// Assume none were set (because timeout was reached first), and clear all.
rcl_wait_set_clear_subscriptions(wait_set);
rcl_wait_set_clear_guard_conditions(wait_set);
return RCL_RET_TIMEOUT;
}
// Check for error.
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
return RCL_RET_ERROR;
}
// Set corresponding rcl subscription handles NULL.
for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
assert(i < wait_set->impl->rmw_subscriptions.subscriber_count); // Defensive.
if (!wait_set->impl->rmw_subscriptions.subscribers[i]) {
wait_set->subscriptions[i] = NULL;
}
}
// Set corresponding rcl guard_condition handles NULL.
for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) {
assert(i < wait_set->impl->rmw_guard_conditions.guard_condition_count); // Defensive.
if (!wait_set->impl->rmw_guard_conditions.guard_conditions[i]) {
wait_set->guard_conditions[i] = NULL;
}
}
return RCL_RET_OK;
}