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

more changes (I will squash this commit)

Browse source 
specifically I:
- used more descriptive error codes
- added some of the CMake logic
- More .c files
- Changed some of the documentation to be clearer
This commit is contained in:
William Woodall 2015-12-01 08:10:40 -08:00
parent 4b55f8e1cc
commit c24a214403
17 changed files with 470 additions and 67 deletions
Download patch file Download diff file Expand all files Collapse all files

29
rcl/CMakeLists.txt
View file

@ -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()

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

@ -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);

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

@ -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:

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

@ -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

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

@ -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();

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

@ -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(

3
rcl/include/rcl/types.h
View file

@ -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

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

@ -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

5
rcl/package.xml
View file

@ -11,9 +11,14 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rmw</build_depend>
<!-- This ensures the rmw impls are built first. -->
<build_depend>rmw_implementation</build_depend>
<build_export_depend>rmw</build_export_depend>
<depend>rcl_interfaces</depend>
<depend>rosidl_generator_cpp</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

8
rcl/src/rcl/allocator.c
View file

@ -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;
}

4
rcl/src/rcl/common.h
View file

@ -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); \

150
rcl/src/rcl/guard_condition.c Normal file
View file

@ -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

66
rcl/src/rcl/node.c
View file

@ -17,18 +17,18 @@ extern "C"
{
#endif
#include "rcl/node.h"
#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;
@ -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;
}

91
rcl/src/rcl/publisher.c Normal file
View file

@ -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 <string.h>
#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

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

@ -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();

26
rcl/src/rcl/subscription.c Normal file
View file

@ -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

24
rcl/src/rcl/wait.c Normal file
View file

@ -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