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

implement allocator, node, and rcl functions

Browse source
This commit is contained in:
William Woodall 2015-11-24 03:45:40 -08:00
parent bac3b210d7
commit 2f029b560e
5 changed files with 408 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

37
rcl/src/rcl/allocator.c Normal file
View file

@ -0,0 +1,37 @@
// 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.
#include <stdlib.h>
#include "rcl/allocator.h"
static void *
__default_allocate(size_t size, void * state)
{
(void)state; // unused
return malloc(size);
}
static void
__default_deallocate(void * pointer, void * state)
{
(void)state; // unused
return free(pointer);
}
rcl_allocator_t
rcl_get_default_allocator()
{
return {__default_allocate, __default_deallocate, NULL};
}

48
rcl/src/rcl/common.c Normal file
View file

@ -0,0 +1,48 @@
// 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 "./common.h"
#include <stdlib.h>
#if defined(WIN32)
static char __env_buffer[1024];
#endif
rcl_ret_t
rcl_impl_getenv(const char * env_name, char ** env_value)
{
env_value = NULL;
#if !defined(WIN32)
*env_value = getenv(env_name);
#else
size_t required_size;
error_t ret = getenv_s(&required_size, __env_buffer, sizeof(__env_buffer), env_name);
if (ret != 0) {
RCL_SET_ERROR_MSG("value in env variable too large to read in");
return RCL_RET_ERROR;
}
env_value = __env_buffer;
#endif
return RCL_RET_OK;
}
#if __cplusplus
}
#endif

42
rcl/src/rcl/common.h 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.
#ifndef RCL__COMMON_H_
#define RCL__COMMON_H_
#if __cplusplus
extern "C"
{
#endif
#include "rcl/error_handling.h"
#include "rcl/types.h"
#define RCL_CHECK_PARAMETER_FOR_NULL(parameter, error_return_type) \
RCL_CHECK_FOR_NULL_WITH_MSG(parameter, #parameter " parameter is null", return error_return_type)
#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \
RCL_SET_ERROR_MSG(msg); \
error_statement; \
}
// This value put in env_value is only valid until the next call to rcl_impl_getenv (on Windows).
rcl_ret_t
rcl_impl_getenv(const char * env_name, char ** env_value);
#if __cplusplus
}
#endif
#endif // RCL__COMMON_H_

173
rcl/src/rcl/node.c Normal file
View file

@ -0,0 +1,173 @@
// 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 <limits.h>
#include <stdlib.h>
#include <string.h>
#include "rcl/node.h"
#include "rcl/rcl.h"
#include "rmw/rmw.h"
#include "./common.h"
typedef struct rcl_node_impl_t {
char * name;
rcl_node_options_t options;
rmw_node_t * rmw_node_handle;
uint64_t rcl_instance_id;
} rcl_node_impl_t;
rcl_node_t
rcl_get_uninitialized_node()
{
return {0};
}
rcl_ret_t
rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options)
{
size_t domain_id = 0;
char * ros_domain_id;
rcl_ret_t ret;
RCL_CHECK_PARAMETER_FOR_NULL(name, RCL_RET_ERROR);
RCL_CHECK_PARAMETER_FOR_NULL(options, RCL_RET_ERROR);
RCL_CHECK_PARAMETER_FOR_NULL(node, RCL_RET_ERROR);
if (node->impl) {
RCL_SET_ERROR_MSG(
"either double call to rcl_node_init or rcl_get_uninitialized_node was not used");
return RCL_RET_ERROR;
}
const rcl_allocator_t * allocator = &options->allocator;
// Allocate space for the implementation struct.
RCL_CHECK_FOR_NULL_WITH_MSG(allocator->allocate, "allocate not set", return RCL_RET_ERROR);
node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_ERROR);
// Initialize node impl.
// node name
size_t name_len = strlen(name);
if (name_len == 0) {
RCL_SET_ERROR_MSG("node name cannot be empty string");
goto fail;
}
node->impl->name = (char *)allocator->allocate(strlen(name), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl->name, "allocating memory failed", goto fail);
strncpy(node->impl->name, name, strlen(name));
// node options
node->impl->options = *options;
// node rmw_node_handle
// First determine the ROS_DOMAIN_ID.
// The result of rcl_impl_getenv on Windows is only valid until the next call to rcl_impl_getenv.
ret = rcl_impl_getenv("ROS_DOMAIN_ID", &ros_domain_id);
if (ret != RCL_RET_OK) {
goto fail;
}
if (ros_domain_id) {
auto number = strtoul(ros_domain_id, NULL, 0);
if (number == ULONG_MAX) {
RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number");
goto fail;
}
domain_id = (size_t)number;
}
node->impl->rmw_node_handle = rmw_create_node(name, domain_id);
RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail);
node->impl->rcl_instance_id = rcl_get_instance_id();
return RCL_RET_OK;
fail:
if (node->impl->name) {
allocator->deallocate(node->impl->name, allocator->state);
}
if (node->impl) {
allocator->deallocate(node->impl, allocator->state);
}
return RCL_RET_ERROR;
}
rcl_ret_t
rcl_node_fini(rcl_node_t * node)
{
RCL_CHECK_PARAMETER_FOR_NULL(node, RCL_RET_ERROR);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return RCL_RET_ERROR);
rcl_ret_t result = RCL_RET_OK;
rmw_ret_t node_ret = rmw_destroy_node(node->impl->rmw_node_handle);
if (node_ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
result = RCL_RET_ERROR;
}
rcl_allocator_t allocator = node->impl->options.allocator;
allocator.deallocate(node->impl->name, allocator.state);
allocator.deallocate(node->impl, allocator.state);
return result;
}
rcl_node_options_t
rcl_node_get_default_options()
{
return {false, rcl_get_default_allocator()};
}
const char *
rcl_node_get_name(const rcl_node_t * node)
{
RCL_CHECK_PARAMETER_FOR_NULL(node, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
return NULL;
}
return node->impl->name;
}
const rcl_node_options_t *
rcl_node_get_options(const rcl_node_t * node)
{
RCL_CHECK_PARAMETER_FOR_NULL(node, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
return NULL;
}
return &node->impl->options;
}
rmw_node_t *
rcl_node_get_rmw_node_handle(const rcl_node_t * node)
{
RCL_CHECK_PARAMETER_FOR_NULL(node, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
return NULL;
}
return node->impl->rmw_node_handle;
}
uint64_t
rcl_node_get_rcl_instance_id(const rcl_node_t * node)
{
RCL_CHECK_PARAMETER_FOR_NULL(node, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
return node->impl->rcl_instance_id;
}
#if __cplusplus
}
#endif

108
rcl/src/rcl/rcl.c Normal file
View file

@ -0,0 +1,108 @@
// 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/rcl.h"
#include <stdatomic.h>
#include <string.h>
#include "rcl/error_handling.h"
static atomic_bool __rcl_is_initialized = false;
static rcl_allocator_t __rcl_allocator = {0};
static int __rcl_argc = 0;
static char ** __rcl_argv = NULL;
static atomic_uint_fast64_t __rcl_instance_id = 0;
static uint64_t __rcl_next_unique_id = 0;
static inline void
__clean_up_init()
{
if (__rcl_argv) {
for (size_t i = 0; i < __rcl_argc; ++i) {
if (__rcl_argv[i]) {
__rcl_allocator.deallocate(__rcl_argv[i], __rcl_allocator.state);
}
}
__rcl_allocator.deallocate(__rcl_argv, __rcl_allocator.state);
}
atomic_store(&__rcl_instance_id, 0);
atomic_store(&__rcl_is_initialized, false);
}
rcl_ret_t
rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
{
bool was_initialized = atomic_exchange(&__rcl_is_initialized, true);
if (was_initialized) {
RCL_SET_ERROR_MSG("rcl_init called while already initialized");
return RCL_RET_ALREADY_INIT;
}
// 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");
goto fail;
}
memset(__rcl_argv, 0, sizeof(char **) * argc);
for (size_t i = 0; i < argc; ++i) {
__rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state);
strcpy(__rcl_argv[i], argv[i]);
}
atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id);
if (atomic_load(&__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 RCL_RET_ERROR;
}
rcl_ret_t
rcl_fini()
{
if (!atomic_load(&__rcl_is_initialized)) {
RCL_SET_ERROR_MSG("rcl_fini called before rcl_init");
return RCL_RET_NOT_INIT;
}
__clean_up_init();
return RCL_RET_OK;
}
uint64_t
rcl_get_instance_id()
{
return atomic_load(&__rcl_instance_id);
}
bool
rcl_ok()
{
return atomic_load(&__rcl_is_initialized);
}
#if __cplusplus
}
#endif