From 2f029b560e217f3bcace6b04fab6e64b061292bc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 24 Nov 2015 03:45:40 -0800 Subject: [PATCH] implement allocator, node, and rcl functions --- rcl/src/rcl/allocator.c | 37 +++++++++ rcl/src/rcl/common.c | 48 +++++++++++ rcl/src/rcl/common.h | 42 ++++++++++ rcl/src/rcl/node.c | 173 ++++++++++++++++++++++++++++++++++++++++ rcl/src/rcl/rcl.c | 108 +++++++++++++++++++++++++ 5 files changed, 408 insertions(+) create mode 100644 rcl/src/rcl/allocator.c create mode 100644 rcl/src/rcl/common.c create mode 100644 rcl/src/rcl/common.h create mode 100644 rcl/src/rcl/node.c create mode 100644 rcl/src/rcl/rcl.c diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c new file mode 100644 index 0000000..b4caf3e --- /dev/null +++ b/rcl/src/rcl/allocator.c @@ -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 + +#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}; +} diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c new file mode 100644 index 0000000..563921c --- /dev/null +++ b/rcl/src/rcl/common.c @@ -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 + +#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 diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h new file mode 100644 index 0000000..c62260b --- /dev/null +++ b/rcl/src/rcl/common.h @@ -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_ diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c new file mode 100644 index 0000000..6d2d19c --- /dev/null +++ b/rcl/src/rcl/node.c @@ -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 +#include +#include + +#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 diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c new file mode 100644 index 0000000..a8488ca --- /dev/null +++ b/rcl/src/rcl/rcl.c @@ -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 +#include + +#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