From c24a214403b40137c6e309190d90c4c7e4732c5b Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 1 Dec 2015 08:10:40 -0800 Subject: [PATCH] more changes (I will squash this commit) specifically I: - used more descriptive error codes - added some of the CMake logic - More .c files - Changed some of the documentation to be clearer --- rcl/CMakeLists.txt | 29 +++++- rcl/include/rcl/guard_condition.h | 22 +++-- rcl/include/rcl/node.h | 58 ++++++++++-- rcl/include/rcl/publisher.h | 4 - rcl/include/rcl/rcl.h | 14 +-- rcl/include/rcl/subscription.h | 16 +++- rcl/include/rcl/types.h | 3 + rcl/include/rcl/wait.h | 13 ++- rcl/package.xml | 5 + rcl/src/rcl/allocator.c | 8 +- rcl/src/rcl/common.h | 4 +- rcl/src/rcl/guard_condition.c | 150 ++++++++++++++++++++++++++++++ rcl/src/rcl/node.c | 66 +++++++------ rcl/src/rcl/publisher.c | 91 ++++++++++++++++++ rcl/src/rcl/rcl.c | 4 +- rcl/src/rcl/subscription.c | 26 ++++++ rcl/src/rcl/wait.c | 24 +++++ 17 files changed, 470 insertions(+), 67 deletions(-) create mode 100644 rcl/src/rcl/guard_condition.c create mode 100644 rcl/src/rcl/publisher.c create mode 100644 rcl/src/rcl/subscription.c create mode 100644 rcl/src/rcl/wait.c diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 8e83769..79c5af4 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -3,11 +3,36 @@ cmake_minimum_required(VERSION 2.8.3) project(rcl) find_package(ament_cmake REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(rmw REQUIRED) +find_package(rosidl_generator_cpp REQUIRED) -ament_export_dependencies(rmw) - +ament_export_dependencies(rmw rosidl_generator_cpp) ament_export_include_directories(include) +include_directories(include) + +if(NOT WIN32) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c11 -Wall -Wextra") +endif() + +set(${PROJECT_NAME}_sources + src/rcl/allocator.c + src/rcl/common.c + src/rcl/guard_condition.c + src/rcl/node.c + src/rcl/publisher.c + src/rcl/rcl.c + src/rcl/subscription.c + src/rcl/wait.c +) +add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources}) +ament_target_dependencies(${PROJECT_NAME} + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" +) + if(AMENT_ENABLE_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index b4a9b82..7e976c3 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -72,10 +72,16 @@ rcl_get_zero_initialized_guard_condition(); * * This function is not thread-safe. * + * \TODO(wjwwood): does this function need a node to be passed to it? (same for fini) + * * \param[inout] guard_condition preallocated guard_condition structure * \param[in] node valid rcl node handle * \param[in] options the guard_condition's options - * \return RMW_RET_OK if guard_condition was initialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if guard_condition was initialized successfully, or + * RCL_RET_ALREADY_INIT if the guard condition is already initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_guard_condition_init( @@ -83,16 +89,18 @@ rcl_guard_condition_init( const rcl_node_t * node, const rcl_guard_condition_options_t * options); -/// Deinitialize a rcl_guard_condition_t. +/// Finalize a rcl_guard_condition_t. /* After calling, calls to rcl_trigger_guard_condition will fail when using * this guard condition. * However, the given node handle is still valid. * * This function is not thread-safe. * - * \param[inout] guard_condition handle to the guard_condition to be deinitialized + * \param[inout] guard_condition handle to the guard_condition to be finalized * \param[in] node handle to the node used to create the guard_condition - * \return RMW_RET_OK if guard_condition was deinitialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if guard_condition was finalized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node); @@ -104,7 +112,7 @@ rcl_guard_condition_get_default_options(); /// Trigger a rcl guard condition. /* This function can fail, and therefore return NULL, if the: * - guard condition is NULL - * - guard condition is invalid (never called init, called fini, or invalid node) + * - guard condition is invalid (never called init or called fini) * * A guard condition can be triggered from any thread. * @@ -112,7 +120,9 @@ rcl_guard_condition_get_default_options(); * with rcl_guard_condition_fini() on the same guard condition. * * \param[in] guard_condition handle to the guard_condition to be triggered - * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if the guard condition was triggered, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition); diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 9d5f706..d1ad319 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -23,6 +23,9 @@ extern "C" #include "rcl/allocator.h" #include "rcl/types.h" +// Intentional underflow to get max size_t. +#define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID (size_t)-1 + struct rcl_node_impl_t; /// Handle for a ROS node. @@ -42,6 +45,16 @@ typedef struct rcl_node_options_t { // rmw_qos_profile_t parameter_qos; /// If true, no parameter infrastructure will be setup. bool no_parameters; + /// If set, then this value overrides the ROS_DOMAIN_ID environment variable. + /* It defaults to RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID, which will cause the + * node to use the ROS domain ID set in the ROS_DOMAIN_ID environment + * variable, or on some systems 0 if the environment variable is not set. + * + * \TODO(wjwwood): Should we put a limit on the ROS_DOMAIN_ID value, that way + * we can have a safe value for the default + * RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID? (currently max size_t) + */ + size_t domain_id; /// Custom allocator used for incidental allocations, e.g. node name string. rcl_allocator_t allocator; } rcl_node_options_t; @@ -66,11 +79,13 @@ rcl_get_zero_initialized_node(); * * A node contains infrastructure for ROS parameters, which include advertising * publishers and service servers. - * So this function will create those external parameter interfaces even if + * This function will create those external parameter interfaces even if * parameters are not used later. * - * This function should be called on a rcl_node_t exactly once, and calling it - * multiple times is undefined behavior. + * The rcl_node_t given must be allocated and zero initalized. + * Passing an rcl_node_t which has already had this function called on it, more + * recently than rcl_node_fini, will fail. + * An allocated rcl_node_t with uninitialized memory is undefined behavior. * * Expected usage: * @@ -89,22 +104,28 @@ rcl_get_zero_initialized_node(); * \param[inout] node a preallocated node structure * \param[in] name the name of the node * \param[in] options the node options; pass null to use default options - * \return RMW_RET_OK if node was initialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if node was initialized successfully, or + * RCL_RET_ALREADY_INIT if the node has already be initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options); -/// Deinitialize a rcl_node_t. +/// Finalized a rcl_node_t. /* Shuts down any automatically created infrastructure and deallocates memory. - * After calling, the rcl_node_t can be safely released. + * After calling, the rcl_node_t can be safely deallocated. * * Any middleware primitives created by the user, e.g. publishers, services, etc., * are invalid after deinitialization. * * This function is not thread-safe. * - * \param[in] node handle to the node to be deinitialized - * \return RMW_RET_OK if node was deinitialized successfully, otherwise RMW_RET_ERROR + * \param[in] node handle to the node to be finalized + * \return RCL_RET_OK if node was finalized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_node_fini(rcl_node_t * node); @@ -145,6 +166,27 @@ rcl_node_get_name(const rcl_node_t * node); const rcl_node_options_t * rcl_node_get_options(const rcl_node_t * node); +/// Return the ROS domain ID that the node is using. +/* This function returns the ROS domain ID that the node is in. + * + * This function should be used to determine what domain_id was used rather + * than checking the domin_id field in the node options, because if + * RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID is used when creating the node then + * it is not changed after creation, but this function will return the actual + * domain_id used. + * + * The domain_id field must point to an allocated size_t object to which the + * ROS domain ID will be written. + * + * \param[in] node the handle to the node being queried + * \return RCL_RET_OK if node the domain ID was retrieved successfully, or + * RCL_RET_NODE_INVALID if the node is invalid, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +rcl_ret_t +rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); + /// Return the rmw node handle. /* The handle returned is a pointer to the internally held rmw handle. * This function can fail, and therefore return NULL, if: diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 0ce2548..f4bf6f2 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -256,10 +256,6 @@ rcl_publisher_get_options(rcl_publisher_t * publisher); rmw_publisher_t * rcl_publisher_get_rmw_publisher_handle(rcl_publisher_t * publisher); -int main() { - -} - #if __cplusplus } #endif diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 485375e..b099360 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -29,13 +29,13 @@ extern "C" /* Unless otherwise noted, this must be called before using any rcl functions. * * This function can only be run once after starting the program, and once - * after each call to rcl_fini. + * after each call to rcl_shutdown. * Repeated calls will fail with RCL_RET_ALREADY_INIT. * This function is not thread safe. * - * This function can be called any time after rcl_fini is called, but it + * This function can be called any time after rcl_shutdown is called, but it * cannot be called from within a callback being executed by an rcl executor. - * For example, you can call rcl_fini from within a timer callback, but + * For example, you can call rcl_shutdown from within a timer callback, but * you have to return from the callback, and therefore exit any in-progress * call to a spin function, before calling rcl_init again. * @@ -47,7 +47,7 @@ extern "C" * * \param[in] argc number of strings in argv * \param[in] argv command line arguments; rcl specific arguments are removed - * \param[in] allocator allocator to be used during rcl_init and rcl_fini + * \param[in] allocator allocator to be used during rcl_init and rcl_shutdown * \return RCL_RET_OK if initialization is successful, or * RCL_RET_ALREADY_INIT if rcl_init has already been called, or * RCL_RET_ERROR if an unspecified error occurs. @@ -74,14 +74,14 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); * RCL_RET_NOT_INIT if rcl_init has not yet been called */ rcl_ret_t -rcl_fini(); +rcl_shutdown(); /// Returns an uint64_t number that is unique for the latest rcl_init call. -/* If called before rcl_init or after rcl_fini then 0 will be returned. */ +/* If called before rcl_init or after rcl_shutdown then 0 will be returned. */ uint64_t rcl_get_instance_id(); -/// Return true until rcl_fini is called, then false. +/// Return true until rcl_shutdown is called, then false. /* This function is thread safe. */ bool rcl_ok(); diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index b2dd86f..f008864 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -117,7 +117,10 @@ rcl_get_zero_initialized_subscription(); * \param[in] type_support type support object for the topic's type * \param[in] topic_name the name of the topic * \param[in] options subscription options, including quality of service settings - * \return RMW_RET_OK if subscription was initialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if subscription was initialized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_subscription_init( @@ -140,7 +143,9 @@ rcl_subscription_init( * * \param[inout] subscription handle to the subscription to be deinitialized * \param[in] node handle to the node used to create the subscription - * \return RMW_RET_OK if subscription was deinitialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if subscription was deinitialized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node); @@ -157,7 +162,7 @@ rcl_subscription_get_default_options(); * be checked by this function and therefore no deliberate error will occur. * * TODO(wjwwood) blocking of take? - * TODO(wjwwood) pre- and post-conditions for message ownership? + * TODO(wjwwood) pre-, during-, and post-conditions for message ownership? * TODO(wjwwood) is rcl_take thread-safe? * * The ros_message pointer should point to an already allocated ROS message @@ -184,7 +189,10 @@ rcl_subscription_get_default_options(); * \param[inout] ros_message type-erased ptr to a allocated ROS message * \param[out] taken pointer to a bool, if set to false a message was not taken * \param[out] message_info rmw struct which contains meta-data for the message - * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if the message was published, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_take( diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index cef999e..2dd33fe 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -24,7 +24,10 @@ typedef rmw_ret_t rcl_ret_t; // rcl specific ret codes start at 100 #define RCL_RET_ALREADY_INIT 100 #define RCL_RET_NOT_INIT 101 +#define RCL_RET_BAD_ALLOC 102 +#define RCL_RET_INVALID_ARGUMENT 103 // rcl node specific ret codes in 2XX +#define RCL_RET_NODE_INVALID 200 // rcl publisher specific ret codes in 3XX // rcl subscription specific ret codes in 4XX // rcl service client specific ret codes in 5XX diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 2d415dc..7fc853c 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -45,7 +45,7 @@ typedef struct rcl_wait_set_t { /// If set to true, actions like add_subscription will fail until cleared. bool pruned; /// Implementation specific storage. - rcl_wait_set_impl_t * impl; + struct rcl_wait_set_impl_t * impl; } rcl_wait_set_t; /// Return a rcl_wait_set_t struct with members set to NULL. @@ -87,6 +87,8 @@ rcl_get_zero_initialized_wait_set(); * \param[in] allocator the allocator to use when allocating space in the sets * \return RCL_RET_OK if the wait set is initialized successfully, or * RCL_RET_ALREADY_INIT if the wait set is not zero initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -110,7 +112,9 @@ rcl_wait_set_init( * This function is not thread-safe. * * \param[inout] wait_set the wait set struct to be finalized. - * \return RCL_RET_OK if the finalization was successful, otherwise RCL_RET_ERROR + * \return RCL_RET_OK if the finalization was successful, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_wait_set_fini(rcl_wait_set_t * wait_set); @@ -126,6 +130,7 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set); * \return RCL_RET_OK if added successfully, or * RCL_RET_WAIT_SET_FULL if the subscription set is full, or * RCL_RET_NOT_INIT if the wait set is zero initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -144,6 +149,7 @@ rcl_wait_set_add_subscription( * \param[inout] wait_set struct to have its subscriptions cleared * \return RCL_RET_OK if cleared successfully, or * RCL_RET_NOT_INIT if the wait set is zero initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -170,6 +176,8 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); * \param[inout] wait_set struct to have its subscriptions cleared * \param[in] size a size for the new set * \return RCL_RET_OK if resized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -282,6 +290,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * 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 arugments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t diff --git a/rcl/package.xml b/rcl/package.xml index 034064c..700656b 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -11,9 +11,14 @@ ament_cmake rmw + + rmw_implementation rmw + rcl_interfaces + rosidl_generator_cpp + ament_lint_auto ament_lint_common diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c index 93eee17..a51ae70 100644 --- a/rcl/src/rcl/allocator.c +++ b/rcl/src/rcl/allocator.c @@ -40,5 +40,11 @@ __default_reallocate(void * pointer, size_t size, void * state) rcl_allocator_t rcl_get_default_allocator() { - return {__default_allocate, __default_deallocate, __default_reallocate, NULL}; + static rcl_allocator_t default_allocator = { + __default_allocate, + __default_deallocate, + __default_reallocate, + NULL + }; + return default_allocator; } diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index c62260b..827c33b 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -23,8 +23,8 @@ extern "C" #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_ARGUMENT_FOR_NULL(parameter, error_return_type) \ + RCL_CHECK_FOR_NULL_WITH_MSG(parameter, #parameter " argument is null", return error_return_type) #define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \ RCL_SET_ERROR_MSG(msg); \ diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c new file mode 100644 index 0000000..a57db09 --- /dev/null +++ b/rcl/src/rcl/guard_condition.c @@ -0,0 +1,150 @@ +// 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/guard_condition.h" + +#include "./common.h" +#include "rcl/rcl.h" +#include "rmw/rmw.h" + +typedef struct rcl_guard_condition_impl_t { + rmw_guard_condition_t * rmw_handle; + rcl_guard_condition_options_t options; +} rcl_guard_condition_impl_t; + +rcl_guard_condition_t +rcl_get_zero_initialized_guard_condition() +{ + static rcl_guard_condition_t null_guard_condition = {0}; + return null_guard_condition; +} + +rcl_ret_t +rcl_guard_condition_init( + rcl_guard_condition_t * guard_condition, + const rcl_node_t * node, + const rcl_guard_condition_options_t * options) +{ + // Perform argument validation. + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + 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); + // Ensure the node is valid. + if (!node->impl || rcl_node_get_rcl_instance_id(node) != rcl_get_instance_id()) { + RCL_SET_ERROR_MSG("node handle is invalid"); + return RCL_RET_NODE_INVALID; + } + // Ensure the guard_condition handle is zero initialized. + if (guard_condition->impl) { + RCL_SET_ERROR_MSG("guard_condition already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + // Allocate space for the guard condition impl. + guard_condition->impl = (rcl_guard_condition_impl_t *)allocator->allocate( + sizeof(rcl_guard_condition_impl_t), allocator->state); + if (!guard_condition->impl) { + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + // Create the rmw guard condition. + guard_condition->impl->rmw_handle = rmw_create_guard_condition(); + if (!guard_condition->impl->rmw_handle) { + // Deallocate impl and exit. + allocator->deallocate(guard_condition->impl, allocator->state); + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + // Copy options into impl. + guard_condition->impl->options = *options; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node) +{ + // Perform argument validation. + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + // Ensure the node is valid. + if (!node->impl || rcl_node_get_rcl_instance_id(node) != rcl_get_instance_id()) { + RCL_SET_ERROR_MSG("node handle is invalid"); + return RCL_RET_NODE_INVALID; + } + rcl_ret_t result = RCL_RET_OK; + if (guard_condition->impl) { + if (guard_condition->impl->rmw_handle) { + if (rmw_destroy_guard_condition(guard_condition->impl->rmw_handle) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + result = RCL_RET_ERROR; + } + } + rcl_allocator_t allocator = guard_condition->impl->options.allocator; + if (allocator.deallocate) { + allocator.deallocate(guard_condition->impl, allocator.state); + } else { + RCL_SET_ERROR_MSG("deallocate not set"); + result = RCL_RET_ERROR; + } + } + return result; +} + +rcl_guard_condition_options_t +rcl_guard_condition_get_default_options() +{ + static rcl_guard_condition_options_t default_options = {0}; + // Must set the allocator after because it is not a compile time constant. + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +rcl_ret_t +rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition) +{ + // Perform argument validation. + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + guard_condition->impl, + "guard condition implementation is invalid", + return RCL_RET_INVALID_ARGUMENT); + // Trigger the guard condition. + if (rmw_trigger_guard_condition(guard_condition->impl->rmw_handle) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rmw_guard_condition_t * +rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + guard_condition->impl, "guard condition implementation is invalid", return NULL); + return guard_condition->impl->rmw_handle; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 6d2d19c..50e5aa9 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -17,18 +17,18 @@ extern "C" { #endif +#include "rcl/node.h" + #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; @@ -37,7 +37,8 @@ typedef struct rcl_node_impl_t { rcl_node_t rcl_get_uninitialized_node() { - return {0}; + static rcl_node_t null_node = {0}; + return null_node; } rcl_ret_t @@ -46,19 +47,22 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o 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); + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); 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; + RCL_SET_ERROR_MSG("node already initialized, or struct 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. - 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); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); // Initialize node impl. // node name size_t name_len = strlen(name); @@ -66,9 +70,6 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o 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 @@ -79,7 +80,7 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o goto fail; } if (ros_domain_id) { - auto number = strtoul(ros_domain_id, NULL, 0); + unsigned long 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; @@ -92,20 +93,20 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o 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; + return fail_ret; } 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_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (!node->impl) { + // Repeat calls to fini or calling fini on a zero initialized node is ok. + return RCL_RET_OK; + } 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) { @@ -113,7 +114,8 @@ rcl_node_fini(rcl_node_t * node) result = RCL_RET_ERROR; } rcl_allocator_t allocator = node->impl->options.allocator; - allocator.deallocate(node->impl->name, allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); allocator.deallocate(node->impl, allocator.state); return result; } @@ -121,25 +123,31 @@ rcl_node_fini(rcl_node_t * node) rcl_node_options_t rcl_node_get_default_options() { - return {false, rcl_get_default_allocator()}; + static rcl_node_options_t default_options = { + .no_parameters = false, + .domain_id = RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID, + }; + // Must set the allocator after because it is not a compile time constant. + default_options.allocator = rcl_get_default_allocator(); + return default_options; } const char * rcl_node_get_name(const rcl_node_t * node) { - RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_ARGUMENT_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; + return node->impl->rmw_node_handle->name; } const rcl_node_options_t * rcl_node_get_options(const rcl_node_t * node) { - RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_ARGUMENT_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"); @@ -151,7 +159,7 @@ rcl_node_get_options(const rcl_node_t * node) rmw_node_t * rcl_node_get_rmw_node_handle(const rcl_node_t * node) { - RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_ARGUMENT_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"); @@ -163,8 +171,8 @@ rcl_node_get_rmw_node_handle(const rcl_node_t * node) 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); + RCL_CHECK_ARGUMENT_FOR_NULL(node, 0); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return 0); return node->impl->rcl_instance_id; } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c new file mode 100644 index 0000000..319b073 --- /dev/null +++ b/rcl/src/rcl/publisher.c @@ -0,0 +1,91 @@ +// 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/publisher.h" + +#include + +#include "./common.h" +#include "rmw/rmw.h" + +typedef struct rcl_publisher_impl_t { + rcl_publisher_options_t options; + rmw_publisher_t * rmw_handle; +} rcl_publisher_impl_t; + +rcl_publisher_t +rcl_get_zero_initialized_publisher() +{ + static rcl_publisher_t null_publisher = {0}; + return null_publisher; +} + +rcl_ret_t +rcl_publisher_init( + rcl_publisher_t * publisher, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_publisher_options_t * options) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + if (publisher->impl) { + RCL_SET_ERROR_MSG("publisher 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. + publisher->impl = (rcl_publisher_impl_t *)allocator->allocate( + sizeof(rcl_publisher_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + // Fill out implementation struct. + // rmw handle (create rmw publisher) + // TODO(wjwwood): pass along the allocator to rmw when it supports it + publisher->impl->rmw_handle = rmw_create_publisher( + rcl_node_get_rmw_node_handle(node), + type_support, + topic_name, + &rmw_qos_profile_default); + if (!publisher->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + goto fail; + } + // options + publisher->impl->options = *options; + return RCL_RET_OK; +fail: + if (publisher->impl) { + allocator->deallocate(publisher->impl, allocator->state); + } + return fail_ret; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index a8488ca..537b975 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -81,10 +81,10 @@ fail: } rcl_ret_t -rcl_fini() +rcl_shutdown() { if (!atomic_load(&__rcl_is_initialized)) { - RCL_SET_ERROR_MSG("rcl_fini called before rcl_init"); + RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init"); return RCL_RET_NOT_INIT; } __clean_up_init(); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c new file mode 100644 index 0000000..3136616 --- /dev/null +++ b/rcl/src/rcl/subscription.c @@ -0,0 +1,26 @@ +// 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/subscription.h" + + + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c new file mode 100644 index 0000000..32577a2 --- /dev/null +++ b/rcl/src/rcl/wait.c @@ -0,0 +1,24 @@ +// 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/wait.h" + +#if __cplusplus +} +#endif