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

Merge pull request #5 from ros2/rcl_refactor

Browse source 
refactor rcl into a functional library
This commit is contained in:
William Woodall 2015-12-17 19:47:00 -08:00
commit 083f6ec533
43 changed files with 6645 additions and 164 deletions
Download patch file Download diff file Expand all files Collapse all files

54
rcl/CMakeLists.txt
View file

@ -3,14 +3,68 @@ 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(rmw_implementation REQUIRED)
find_package(rmw_implementation_cmake REQUIRED)
find_package(rosidl_generator_c REQUIRED)
include_directories(include)
if(NOT WIN32)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -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
src/rcl/time.c
src/rcl/timer.c
)
macro(target)
add_library(${PROJECT_NAME}${target_suffix} SHARED ${${PROJECT_NAME}_sources})
ament_target_dependencies(${PROJECT_NAME}${target_suffix}
"rcl_interfaces"
"rmw"
"rosidl_generator_c"
"${rmw_implementation}"
)
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
target_compile_definitions(${PROJECT_NAME}${target_suffix} PRIVATE "RCL_BUILDING_DLL")
install(
TARGETS ${PROJECT_NAME}${target_suffix}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
endmacro()
call_for_each_rmw_implementation(target GENERATE_DEFAULT)
ament_export_dependencies(rcl_interfaces)
ament_export_dependencies(rmw)
ament_export_dependencies(rmw_implementation)
ament_export_dependencies(rosidl_generator_c)
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
if(AMENT_ENABLE_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
add_subdirectory(test)
endif()
ament_package()

82
rcl/include/rcl/allocator.h Normal file
View file

@ -0,0 +1,82 @@
// 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__ALLOCATOR_H_
#define RCL__ALLOCATOR_H_
#if __cplusplus
extern "C"
{
#endif
#include "rcl/macros.h"
#include "rcl/types.h"
#include "rcl/visibility_control.h"
/// Encapsulation of an allocator.
/* To use malloc, free, and realloc use rcl_get_default_allocator().
*
* The allocator should be trivially copyable.
* Meaning that the struct should continue to work after being assignment
* copied into a new struct.
* Specifically the object pointed to by the state pointer should remain valid
* until all uses of the allocator have been made.
* Particular care should be taken when giving an allocator to rcl_init_* where
* it is stored within another object and used later.
*/
typedef struct rcl_allocator_t
{
/// Allocate memory, given a size and state structure.
/* An error should be indicated by returning NULL. */
void * (*allocate)(size_t size, void * state);
/// Deallocate previously allocated memory, mimicking free().
void (* deallocate)(void * pointer, void * state);
/// Reallocate if possible, otherwise it deallocates and allocates.
/* If unsupported then do deallocate and then allocate.
* This should behave as realloc is described, as opposed to reallocf, i.e.
* the memory given by pointer will not be free'd automatically if realloc
* fails.
* For reallocf behavior use rcl_reallocf().
* This function must be able to take an input pointer of NULL and succeed.
*/
void * (*reallocate)(void * pointer, size_t size, void * state);
/// Implementation defined state storage.
/* This is passed as the second parameter to other allocator functions. */
void * state;
} rcl_allocator_t;
/// Return a properly initialized rcl_allocator_t with default values.
/* This function does not allocate heap memory.
* This function is thread-safe.
* This function is lock-free.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_allocator_t
rcl_get_default_allocator();
/// Emulate the behavior of reallocf.
/* This function will return NULL if the allocator is NULL or has NULL for
* function pointer fields.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
void *
rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator);
#if __cplusplus
}
#endif
#endif // RCL__ALLOCATOR_H_

38
rcl/include/rcl/error_handling.h Normal file
View file

@ -0,0 +1,38 @@
// 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__ERROR_HANDLING_H_
#define RCL__ERROR_HANDLING_H_
#include "rmw/error_handling.h"
/* The error handling in RCL is just an alias to the error handling in RMW. */
typedef rmw_error_state_t rcl_error_state_t;
#define rcl_set_error_state rmw_set_error_state
#define RCL_SET_ERROR_MSG(msg) RMW_SET_ERROR_MSG(msg)
#define rcl_error_is_set rmw_error_is_set
#define rcl_get_error_state rmw_get_error_state
#define rcl_get_error_string rmw_get_error_string
#define rcl_get_error_string_safe rmw_get_error_string_safe
#define rcl_reset_error rmw_reset_error
#endif // RCL__ERROR_HANDLING_H_

174
rcl/include/rcl/guard_condition.h Normal file
View file

@ -0,0 +1,174 @@
// 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__GUARD_CONDITION_H_
#define RCL__GUARD_CONDITION_H_
#if __cplusplus
extern "C"
{
#endif
#include "rcl/macros.h"
#include "rcl/node.h"
#include "rcl/visibility_control.h"
/// Internal rcl guard condition implementation struct.
struct rcl_guard_condition_impl_t;
/// Handle for a rcl guard condition.
typedef struct rcl_guard_condition_t
{
struct rcl_guard_condition_impl_t * impl;
} rcl_guard_condition_t;
/// Options available for a rcl guard condition.
typedef struct rcl_guard_condition_options_t
{
/// Custom allocator for the guard condition, used for internal allocations.
rcl_allocator_t allocator;
} rcl_guard_condition_options_t;
/// Return a rcl_guard_condition_t struct with members set to NULL.
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_guard_condition_t
rcl_get_zero_initialized_guard_condition();
/// Initialize a rcl guard_condition.
/* After calling this function on a rcl_guard_condition_t, it can be passed to
* rcl_wait() and then concurrently it can be triggered to wake-up rcl_wait().
*
* The given rcl_node_t must be valid and the resulting rcl_guard_condition_t
* is only valid as long as the given rcl_node_t remains valid.
*
* Expected usage:
*
* #include <rcl/rcl.h>
*
* rcl_node_t node = rcl_get_zero_initialized_node();
* rcl_node_options_t node_ops = rcl_node_get_default_options();
* rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops);
* // ... error handling
* rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
* ret = rcl_guard_condition_init(
* &guard_condition, &node, rcl_guard_condition_get_default_options());
* // ... error handling, and on shutdown do deinitialization:
* ret = rcl_guard_condition_fini(&guard_condition, &node);
* // ... error handling for rcl_guard_condition_fini()
* ret = rcl_node_fini(&node);
* // ... error handling for rcl_deinitialize_node()
*
* This function does allocate heap memory.
* This function is not thread-safe.
* This function is lock-free.
*
* \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 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_PUBLIC
RCL_WARN_UNUSED
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);
/// Finalize a rcl_guard_condition_t.
/* After calling, calls to rcl_guard_condition_trigger() will fail when using
* this guard condition.
* However, the given node handle is still valid.
*
* This function does free heap memory and can allocate memory on errors.
* This function is not thread-safe with rcl_guard_condition_trigger().
* This function is lock-free.
*
* \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 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_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node);
/// Return the default options in a rcl_guard_condition_options_t struct.
/* This function does not allocate heap memory.
* This function is thread-safe.
* This function is lock-free.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_guard_condition_options_t
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 or called fini)
*
* A guard condition can be triggered from any thread.
*
* This function does not allocate heap memory, but can on errors.
* This function is thread-safe with itself, but cannot be called concurrently
* with rcl_guard_condition_fini() on the same guard condition.
* This function is lock-free, but the underlying system calls may not be.
*
* \param[in] guard_condition handle to the guard_condition to be triggered
* \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_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition);
/// Return the rmw guard condition handle.
/* The handle returned is a pointer to the internally held rmw handle.
* 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)
*
* The returned handle is made invalid if the guard condition is finalized or
* if rcl_shutdown() is called.
* The returned handle is not guaranteed to be valid for the life time of the
* guard condition as it may be finalized and recreated itself.
* Therefore it is recommended to get the handle from the guard condition using
* this function each time it is needed and avoid use of the handle
* concurrently with functions that might change it.
*
* \param[in] guard_condition pointer to the rcl guard_condition
* \return rmw guard_condition handle if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rmw_guard_condition_t *
rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition);
#if __cplusplus
}
#endif
#endif // RCL__GUARD_CONDITION_H_

33
rcl/include/rcl/macros.h Normal file
View file

@ -0,0 +1,33 @@
// 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__MACROS_H_
#define RCL__MACROS_H_
#if __cplusplus
extern "C"
{
#endif
#ifndef _WIN32
# define RCL_WARN_UNUSED __attribute__((warn_unused_result))
#else
# define RCL_WARN_UNUSED _Check_return_
#endif
#if __cplusplus
}
#endif
#endif // RCL__MACROS_H_

270
rcl/include/rcl/node.h Normal file
View file

@ -0,0 +1,270 @@
// 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__NODE_H_
#define RCL__NODE_H_
#if __cplusplus
extern "C"
{
#endif
#include <stdint.h>
#include "rcl/allocator.h"
#include "rcl/macros.h"
#include "rcl/types.h"
#include "rcl/visibility_control.h"
#define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID SIZE_MAX
struct rcl_node_impl_t;
/// Handle for a ROS node.
typedef struct rcl_node_t
{
/// Private implementation pointer.
struct rcl_node_impl_t * impl;
} rcl_node_t;
typedef struct rcl_node_options_t
{
// bool anonymous_name;
// 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 internal allocations.
rcl_allocator_t allocator;
} rcl_node_options_t;
/// Return a rcl_node_t struct with members initialized to NULL.
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_node_t
rcl_get_zero_initialized_node();
/// Initialize a ROS node.
/* Calling this on a rcl_node_t makes it a valid node handle until rcl_shutdown
* is called or until rcl_node_fini is called on it.
*
* After calling the ROS node object can be used to create other middleware
* primitives like publishers, services, parameters, and etc.
*
* The name of the node cannot coincide with another node of the same name.
* If a node of the same name is already in the domain, it will be shutdown.
*
* A node contains infrastructure for ROS parameters, which include advertising
* publishers and service servers.
* This function will create those external parameter interfaces even if
* parameters are not used later.
*
* 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:
*
* rcl_node_t node = rcl_get_zero_initialized_node();
* rcl_node_options_t * node_ops = rcl_node_get_default_options();
* rcl_ret_t ret = rcl_node_init(&node, "node_name", node_ops);
* // ... error handling and then use the node, but finally deinitialize it:
* ret = rcl_node_fini(&node);
* // ... error handling for rcl_node_fini()
*
* This function allocates heap memory.
* This function is not thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uint_least64_t.
*
* \pre the node handle must be allocated, zero initialized, and invalid
* \post the node handle is valid and can be used to in other rcl_* functions
*
* \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 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_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options);
/// Finalized a rcl_node_t.
/* Shuts down any automatically created infrastructure and deallocates memory.
* 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 manipulates heap memory.
* This function is not thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uint_least64_t.
*
* \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_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_node_fini(rcl_node_t * node);
/// Return the default node options in a rcl_node_options_t.
RCL_PUBLIC
rcl_node_options_t
rcl_node_get_default_options();
/// Get the name of the node.
/* This function returns the node's internal name string.
* This function can fail, and therefore return NULL, if:
* - node is NULL
* - node has not been initialized (the implementation is invalid)
*
* The returned string is only valid as long as the given rcl_node_t is valid.
* The value of the string may change if the value in the rcl_node_t changes,
* and therefore copying the string is recommended if this is a concern.
*
* This function does not manipulate heap memory.
* This function is thread-safe for different nodes.
* This function is lock-free.
*
* \param[in] node pointer to the node
* \return name string if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
const char *
rcl_node_get_name(const rcl_node_t * node);
/// Return the rcl node options.
/* This function returns the node's internal options struct.
* This function can fail, and therefore return NULL, if:
* - node is NULL
* - node has not been initialized (the implementation is invalid)
*
* The returned struct is only valid as long as the given rcl_node_t is valid.
* The values in the struct may change if the options of the rcl_node_t changes,
* and therefore copying the struct is recommended if this is a concern.
*
* This function does not manipulate heap memory.
* This function is thread-safe for different nodes.
* This function is lock-free.
*
* \param[in] node pointer to the node
* \return options struct if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
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.
*
* This function does not manipulate heap memory.
* This function is thread-safe for different nodes.
* This function is lock-free.
*
* \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_PUBLIC
RCL_WARN_UNUSED
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:
* - node is NULL
* - node has not been initialized (the implementation is invalid)
*
* The returned handle is made invalid if the node is finalized or if
* rcl_shutdown() is called.
* The returned handle is not guaranteed to be valid for the life time of the
* node as it may be finalized and recreated itself.
* Therefore it is recommended to get the handle from the node using
* this function each time it is needed and avoid use of the handle
* concurrently with functions that might change it.
*
* This function does not manipulate heap memory.
* This function is thread-safe for different nodes.
* This function is lock-free.
*
* \param[in] node pointer to the rcl node
* \return rmw node handle if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rmw_node_t *
rcl_node_get_rmw_handle(const rcl_node_t * node);
/// Return the associated rcl instance id.
/* This id is stored when rcl_node_init is called and can be compared with the
* value returned by rcl_get_instance_id() to check if this node was created in
* the current rcl context (since the latest call to rcl_init().
*
* This function can fail, and therefore return 0, if:
* - node is NULL
* - node has not been initialized (the implementation is invalid)
*
* This function will succeed, however, even if rcl_shutdown has been called
* since the node was created.
*
* This function does not manipulate heap memory.
* This function is thread-safe for different nodes.
* This function is lock-free.
*
* \param[in] node pointer to the rcl node
* \return rcl instance id captured at node creation or 0 if there was an error
*/
RCL_PUBLIC
RCL_WARN_UNUSED
uint64_t
rcl_node_get_rcl_instance_id(const rcl_node_t * node);
#if __cplusplus
}
#endif
#endif // RCL__NODE_H_

281
rcl/include/rcl/publisher.h Normal file
View file

@ -0,0 +1,281 @@
// 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__PUBLISHER_H_
#define RCL__PUBLISHER_H_
#if __cplusplus
extern "C"
{
#endif
#include "rosidl_generator_c/message_type_support.h"
#include "rcl/macros.h"
#include "rcl/node.h"
#include "rcl/visibility_control.h"
/// Internal rcl publisher implementation struct.
struct rcl_publisher_impl_t;
/// Handle for a rcl publisher.
typedef struct rcl_publisher_t
{
struct rcl_publisher_impl_t * impl;
} rcl_publisher_t;
/// Options available for a rcl publisher.
typedef struct rcl_publisher_options_t
{
/// Middleware quality of service settings for the publisher.
rmw_qos_profile_t qos;
/// Custom allocator for the publisher, used for incidental allocations.
/* For default behavior (malloc/free), use: rcl_get_default_allocator() */
rcl_allocator_t allocator;
} rcl_publisher_options_t;
/// Return a rcl_publisher_t struct with members set to NULL.
/* Should be called to get a null rcl_publisher_t before passing to
* rcl_initalize_publisher().
* It's also possible to use calloc() instead of this if the rcl_publisher is
* being allocated on the heap.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_publisher_t
rcl_get_zero_initialized_publisher();
/// Initialize a rcl publisher.
/* After calling this function on a rcl_publisher_t, it can be used to publish
* messages of the given type to the given topic using rcl_publish().
*
* The given rcl_node_t must be valid and the resulting rcl_publisher_t is only
* valid as long as the given rcl_node_t remains valid.
*
* The rosidl_message_type_support_t is obtained on a per .msg type basis.
* When the user defines a ROS message, code is generated which provides the
* required rosidl_message_type_support_t object.
* This object can be obtained using a language appropriate mechanism.
* \TODO(wjwwood) probably should talk about this once and link to it instead
* For C this macro can be used (using std_msgs/String as an example):
*
* #include <rosidl_generator_c/message_type_support.h>
* #include <std_msgs/msgs/string.h>
* rosidl_message_type_support_t * string_ts =
* ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String);
*
* For C++ a template function is used:
*
* #include <rosidl_generator_cpp/message_type_support.hpp>
* #include <std_msgs/msgs/string.hpp>
* rosidl_message_type_support_t * string_ts =
* rosidl_generator_cpp::get_message_type_support_handle<std_msgs::msg::String>();
*
* The rosidl_message_type_support_t object contains message type specific
* information used to publish messages.
*
* \TODO(wjwwood) update this once we've come up with an official scheme.
* The topic name must be a non-empty string which follows the topic naming
* format.
*
* The options struct allows the user to set the quality of service settings as
* well as a custom allocator which is used when initializing/finalizing the
* publisher to allocate space for incidentals, e.g. the topic name string.
*
* Expected usage (for C messages):
*
* #include <rcl/rcl.h>
* #include <rosidl_generator_c/message_type_support.h>
* #include <std_msgs/msgs/string.h>
*
* rcl_node_t node = rcl_get_zero_initialized_node();
* rcl_node_options_t node_ops = rcl_node_get_default_options();
* rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops);
* // ... error handling
* rosidl_message_type_support_t * ts = ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String);
* rcl_publisher_t publisher = rcl_get_zero_initialized_publisher();
* rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options();
* ret = rcl_publisher_init(&publisher, &node, ts, "chatter", &publisher_ops);
* // ... error handling, and on shutdown do finalization:
* ret = rcl_publisher_fini(&publisher, &node);
* // ... error handling for rcl_publisher_fini()
* ret = rcl_node_fini(&node);
* // ... error handling for rcl_deinitialize_node()
*
* This function is not thread-safe.
*
* \param[inout] publisher preallocated publisher structure
* \param[in] node valid rcl node handle
* \param[in] type_support type support object for the topic's type
* \param[in] topic_name the name of the topic to publish on
* \param[in] options publisher options, including quality of service settings
* \return RCL_RET_OK if the publisher was initialized successfully, or
* RCL_RET_NODE_INVALID if the node is invalid, or
* RCL_RET_ALREADY_INIT if the publisher is already initialized, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_BAD_ALLOC if allocating memory fails, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
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);
/// Finalize a rcl_publisher_t.
/* After calling, the node will no longer be advertising that it is publishing
* on this topic (assuming this is the only publisher on this topic).
*
* After calling, calls to rcl_publish will fail when using this publisher.
* However, the given node handle is still valid.
*
* This function is not thread-safe.
*
* \param[inout] publisher handle to the publisher to be finalized
* \param[in] node handle to the node used to create the publisher
* \return RCL_RET_OK if publisher was finalized successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node);
/// Return the default publisher options in a rcl_publisher_options_t.
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_publisher_options_t
rcl_publisher_get_default_options();
/// Publish a ROS message on a topic using a publisher.
/* It is the job of the caller to ensure that the type of the ros_message
* parameter and the type associate with the publisher (via the type support)
* match.
* Passing a different type to publish produces undefined behavior and cannot
* be checked by this function and therefore no deliberate error will occur.
*
* TODO(wjwwood): update after researching what the blocking behavior is
* or optionally link to a document that describes blocking
* behavior is more detail.
* Calling rcl_publish is a potentially blocking call.
* When called rcl_publish will immediately do any publishing related work,
* including, but not limited to, converting the message into a different type,
* serializing the message, collecting publish statistics, etc.
* The last thing it will do is call the underlying middleware's publish
* function which may or may not block based on the quality of service settings
* given via the publisher options in rcl_publisher_init().
* For example, if the reliability is set to reliable, then a publish may block
* until space in the publish queue is available, but if the reliability is set
* to best effort then it should not block.
*
* The ROS message given by the ros_message void pointer is always owned by the
* calling code, but should remain constant during publish.
*
* This function is thread safe so long as access to both the publisher and the
* ros_message is synchronized.
* That means that calling rcl_publish from multiple threads is allowed, but
* calling rcl_publish at the same time as non-thread safe publisher functions
* is not, e.g. calling rcl_publish and rcl_publisher_fini concurrently
* is not allowed.
* Before calling rcl_publish the message can change and after calling
* rcl_publish the message can change, but it cannot be changed during the
* publish call.
* The same ros_message, however, can be passed to multiple calls of
* rcl_publish simultaneously, even if the publishers differ.
* The ros_message is unmodified by rcl_publish.
*
* \param[in] publisher handle to the publisher which will do the publishing
* \param[in] ros_message type-erased pointer to the ROS message
* \return RCL_RET_OK if the message was published successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
/// Get the topic name for the publisher.
/* This function returns the publisher's internal topic name string.
* This function can fail, and therefore return NULL, if the:
* - publisher is NULL
* - publisher is invalid (never called init, called fini, or invalid node)
*
* The returned string is only valid as long as the rcl_publisher_t is valid.
* The value of the string may change if the topic name changes, and therefore
* copying the string is recommended if this is a concern.
*
* This function is not thread-safe, and copying the result is not thread-safe.
*
* \param[in] publisher pointer to the publisher
* \return name string if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
const char *
rcl_publisher_get_topic_name(const rcl_publisher_t * publisher);
/// Return the rcl publisher options.
/* This function returns the publisher's internal options struct.
* This function can fail, and therefore return NULL, if the:
* - publisher is NULL
* - publisher is invalid (never called init, called fini, or invalid node)
*
* The returned struct is only valid as long as the rcl_publisher_t is valid.
* The values in the struct may change if the options of the publisher change,
* and therefore copying the struct is recommended if this is a concern.
*
* This function is not thread-safe, and copying the result is not thread-safe.
*
* \param[in] publisher pointer to the publisher
* \return options struct if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
const rcl_publisher_options_t *
rcl_publisher_get_options(const rcl_publisher_t * publisher);
/// Return the rmw publisher handle.
/* The handle returned is a pointer to the internally held rmw handle.
* This function can fail, and therefore return NULL, if the:
* - publisher is NULL
* - publisher is invalid (never called init, called fini, or invalid node)
*
* The returned handle is made invalid if the publisher is finalized or if
* rcl_shutdown() is called.
* The returned handle is not guaranteed to be valid for the life time of the
* publisher as it may be finalized and recreated itself.
* Therefore it is recommended to get the handle from the publisher using
* this function each time it is needed and avoid use of the handle
* concurrently with functions that might change it.
*
* \param[in] publisher pointer to the rcl publisher
* \return rmw publisher handle if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rmw_publisher_t *
rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher);
#if __cplusplus
}
#endif
#endif // RCL__PUBLISHER_H_

227
rcl/include/rcl/rcl.h
View file

@ -15,140 +15,113 @@
#ifndef RCL__RCL_H_
#define RCL__RCL_H_
#include <stdbool.h> // For bool
#include <stddef.h> // For size_t
#if __cplusplus
extern "C"
{
#endif
// For rosidl_message_type_support_t
#include <rosidl_generator_c/message_type_support.h>
#include "rcl/macros.h"
#include "rcl/node.h"
#include "rcl/publisher.h"
#include "rcl/subscription.h"
#include "rcl/types.h"
#include "rcl/visibility_control.h"
#include "rcl/types.h" // For rcl_*_t types
/// Global initialization for rcl; should be called once per process.
/// Global initialization of rcl.
/* 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_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_shutdown is called, but it
* cannot be called from within a callback being executed by an rcl executor.
* 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.
*
* The argc and argv parameters can contain command line arguments for the
* program.
* rcl specific arguments will be parsed and removed, but other arguments will
* be ignored.
* If argc is 0 and argv is NULL no parameters will be parsed.
*
* This function allocates heap memory.
* This function is not thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uint_least64_t.
*
* \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_shutdown
* \return RCL_RET_OK if initialization is successful, or
* RCL_RET_ALREADY_INIT if rcl_init has already been called, or
* RCL_RET_BAD_ALLOC if allocating memory failed, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_init(int argc, char ** argv);
rcl_init(int argc, char ** argv, rcl_allocator_t allocator);
/// Creates a rcl_node_t; used to implement a ROS Node.
rcl_node_t *
rcl_create_node(const char * name);
/// Destroys a rcl_node_t.
/// Signal global shutdown of rcl.
/* This function does not have to be called on exit, but does have to be called
* making a repeat call to rcl_init.
*
* This function can only be called once after each call to rcl_init.
* Repeated calls will fail with RCL_RET_NOT_INIT.
* This function is not thread safe.
*
* When this function is called:
* - Any rcl objects created since the last call to rcl_init are invalidated.
* - Calls to rcl_ok will return false.
* - Any executors waiting for work (within a call to spin) are interrupted.
* - No new work (executing callbacks) will be done in executors.
* - Currently running work in executors will be finished.
*
* This function manipulates heap memory.
* This function is thread-safe, except with rcl_init().
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uint_least64_t.
*
* \return RCL_RET_OK if the shutdown was completed successfully, or
* RCL_RET_NOT_INIT if rcl is not currently initialized, or
* RCL_RET_ERROR if an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_destroy_node(rcl_node_t * node);
rcl_shutdown();
/// Creates a rcl_publisher_t; used to implement a ROS Publisher.
rcl_publisher_t *
rcl_create_publisher(
const rcl_node_t * node,
const rosidl_message_type_support_t * type_support,
const char * topic_name,
size_t queue_size);
/// Returns an uint64_t number that is unique for the latest rcl_init call.
/* If called before rcl_init or after rcl_shutdown then 0 will be returned.
*
* This function does not allocate memory.
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uint_least64_t.
*
* \return a unique id specific to this rcl instance, or 0 if not initialized.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
uint64_t
rcl_get_instance_id();
/// Destroys a rcl_publisher_t.
rcl_ret_t
rcl_destroy_publisher(rcl_publisher_t * publisher);
/// Return true if rcl is currently initialized, otherwise false.
/* This function does not allocate memory.
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uint_least64_t.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
bool
rcl_ok();
/// Publishes a ROS message to a ROS Topic using a rcl_publisher_t.
rcl_ret_t
rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
/// Creates a rcl_subscription_t; used to implement a ROS Subscription.
rcl_subscription_t *
rcl_create_subscription(
const rcl_node_t * node,
const rosidl_message_type_support_t * type_support,
const char * topic_name,
size_t queue_size,
bool ignore_local_publications);
/// Destroys a rcl_subscription_t.
rcl_ret_t
rcl_destroy_subscription(rcl_subscription_t * subscription);
/// Takes a ROS message from a given rcl_subscription_t if one is available.
rcl_ret_t
rcl_take(const rcl_subscription_t * subscriber, void * ros_message);
/// Creates a rcl_guard_condition_t; is used by the implementation to
/// asynchronously interrupt rmw_wait.
rcl_guard_condition_t *
rcl_create_guard_condition();
/// Destroys a rcl_guard_condition_t.
rcl_ret_t
rcl_destroy_guard_condition(rcl_guard_condition_t * guard_condition);
/// Triggers the condition, which will interrupt rmw_wait asynchronously.
rcl_ret_t
rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition);
/// Creates a rcl_callback_group_t, which provides hints about how to execute
/// groups of callbacks to an executor.
rcl_callback_group_t *
rcl_create_callback_group(rcl_node_t * node);
/// Destroys a rcl_callback_group_t.
rcl_ret_t
rcl_destroy_callback_group(rcl_callback_group_t * callback_group);
/// Creates a rcl_subscription_info_t, which associates rcl_subscription_t's
/// to a callback group.
rcl_subscription_info_t *
rcl_create_subscription_info(
rcl_subscription_t * const subscription,
rcl_callback_group_t * const callback_group);
/// Destroys a rcl_subscription_info_t.
rcl_ret_t
rcl_destroy_subscription_info(rcl_subscription_info_t * subscription_info);
/// Creates a rcl_timer_info_t, which associates a ROS Timer's guard condition
/// with a callback group.
rcl_timer_info_t *
rcl_create_timer_info(
rcl_guard_condition_t * const guard_condition,
rcl_callback_group_t * const callback_group);
/// Destroys a rcl_timer_info_t.
rcl_ret_t
rcl_destroy_timer_info(rcl_timer_info_t * timer_info);
/// Creates a rcl_executor_helper_t, which is a collection of callable items.
rcl_executor_helper_t *
rcl_create_executor_helper();
/// Destroys a rcl_executor_helper_t.
rcl_ret_t
rcl_destroy_executor_helper(rcl_executor_helper_t * executor_helper);
/// Add a rcl_subscription_info_t to the collection of callable items.
rcl_ret_t
rcl_add_subscription_info(
rcl_executor_helper_t * executor_helper,
rcl_subscription_info_t * subscription_info);
/// Removes a rcl_subscription_t from the collection of callable items.
rcl_ret_t
rcl_remove_subscription_info(
rcl_executor_helper_t * executor_helper,
rcl_subscription_info_t * subscription_info);
/// Add a rcl_timer_info_t to the collection of callable items.
rcl_ret_t
rcl_add_timer_info(
rcl_executor_helper_t * executor_helper,
rcl_timer_info_t * timer_info);
/// Removes a rcl_subscription_t from the collection of callable items.
rcl_ret_t
rcl_remove_timer_info(
rcl_executor_helper_t * executor_helper,
rcl_timer_info_t * timer_info);
/// Finds the next ready to be called item if one exists; optionally blocking.
rcl_ret_t
rcl_get_next_any_executable(
rcl_executor_helper_t * executor_helper,
rcl_any_executable_t * any_executable,
bool non_blocking);
#if __cplusplus
}
#endif
#endif // RCL__RCL_H_

288
rcl/include/rcl/subscription.h Normal file
View file

@ -0,0 +1,288 @@
// 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__SUBSCRIPTION_H_
#define RCL__SUBSCRIPTION_H_
#if __cplusplus
extern "C"
{
#endif
#include "rosidl_generator_c/message_type_support.h"
#include "rcl/macros.h"
#include "rcl/node.h"
#include "rcl/visibility_control.h"
/// Internal rcl implementation struct.
struct rcl_subscription_impl_t;
/// Handle for a rcl subscription.
typedef struct rcl_subscription_t
{
struct rcl_subscription_impl_t * impl;
} rcl_subscription_t;
/// Options available for a rcl subscription.
typedef struct rcl_subscription_options_t
{
/// Middleware quality of service settings for the subscription.
rmw_qos_profile_t qos;
/// If true, messages published from within the same node are ignored.
bool ignore_local_publications;
/// Custom allocator for the subscription, used for incidental allocations.
/* For default behavior (malloc/free), see: rcl_get_default_allocator() */
rcl_allocator_t allocator;
} rcl_subscription_options_t;
/// Return a rcl_subscription_t struct with members set to NULL.
/* Should be called to get a null rcl_subscription_t before passing to
* rcl_initalize_subscription().
* It's also possible to use calloc() instead of this if the rcl_subscription_t
* is being allocated on the heap.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_subscription_t
rcl_get_zero_initialized_subscription();
/// Initialize a ROS subscription.
/* After calling this function on a rcl_subscription_t, it can be used to take
* messages of the given type to the given topic using rcl_take().
*
* The given rcl_node_t must be valid and the resulting rcl_subscription_t is
* only valid as long as the given rcl_node_t remains valid.
*
* The rosidl_message_type_support_t is obtained on a per .msg type basis.
* When the user defines a ROS message, code is generated which provides the
* required rosidl_message_type_support_t object.
* This object can be obtained using a language appropriate mechanism.
* \TODO(wjwwood) probably should talk about this once and link to it instead
* For C this macro can be used (using std_msgs/String as an example):
*
* #include <rosidl_generator_c/message_type_support.h>
* #include <std_msgs/msgs/string.h>
* rosidl_message_type_support_t * string_ts =
* ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String);
*
* For C++ a template function is used:
*
* #include <rosidl_generator_cpp/message_type_support.hpp>
* #include <std_msgs/msgs/string.hpp>
* rosidl_message_type_support_t * string_ts =
* rosidl_generator_cpp::get_message_type_support_handle<std_msgs::msg::String>();
*
* The rosidl_message_type_support_t object contains message type specific
* information used to publish messages.
*
* \TODO(wjwwood) update this once we've come up with an official scheme.
* The topic name must be a non-empty string which follows the topic naming
* format.
*
* The options struct allows the user to set the quality of service settings as
* well as a custom allocator which is used when (de)initializing the
* subscription to allocate space for incidental things, e.g. the topic
* name string.
*
* Expected usage (for C messages):
*
* #include <rcl/rcl.h>
* #include <rosidl_generator_c/message_type_support.h>
* #include <std_msgs/msgs/string.h>
*
* rcl_node_t node = rcl_get_zero_initialized_node();
* rcl_node_options_t node_ops = rcl_node_get_default_options();
* rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops);
* // ... error handling
* rosidl_message_type_support_t * ts = ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String);
* rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
* rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options();
* ret = rcl_subscription_init(&subscription, &node, ts, "chatter", &subscription_ops);
* // ... error handling, and when finished deinitialization
* ret = rcl_subscription_fini(&subscription, &node);
* // ... error handling for rcl_subscription_fini()
* ret = rcl_node_fini(&node);
* // ... error handling for rcl_deinitialize_node()
*
* This function is not thread-safe.
*
* \param[out] subscription preallocated subscription structure
* \param[in] node valid rcl node handle
* \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 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_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_subscription_init(
rcl_subscription_t * subscription,
const rcl_node_t * node,
const rosidl_message_type_support_t * type_support,
const char * topic_name,
const rcl_subscription_options_t * options);
/// Deinitialize a rcl_subscription_t.
/* After calling, the node will no longer be subscribed on this topic
* (assuming this is the only subscription on this topic in this node).
*
* After calling, calls to rcl_wait and rcl_take will fail when using this
* subscription.
* Additioanlly rcl_wait will be interrupted if currently blocking.
* However, the given node handle is still valid.
*
* This function is not thread-safe.
*
* \param[inout] subscription handle to the subscription to be deinitialized
* \param[in] node handle to the node used to create the subscription
* \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_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node);
/// Return the default subscription options in a rcl_subscription_options_t.
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_subscription_options_t
rcl_subscription_get_default_options();
/// Take a ROS message from a topic using a rcl subscription.
/* It is the job of the caller to ensure that the type of the ros_message
* argument and the type associate with the subscription, via the type
* support, match.
* Passing a different type to rcl_take produces undefined behavior and cannot
* be checked by this function and therefore no deliberate error will occur.
*
* TODO(wjwwood) blocking of take?
* TODO(wjwwood) pre-, during-, and post-conditions for message ownership?
* TODO(wjwwood) is rcl_take thread-safe?
* TODO(wjwwood) Should there be an rcl_message_info_t?
*
* The ros_message pointer should point to an already allocated ROS message
* struct of the correct type, into which the taken ROS message will be copied
* if one is available.
* If taken is false after calling, then the ROS message will be unmodified.
*
* If allocation is required when taking the message, e.g. if space needs to
* be allocated for a dynamically sized array in the target message, then the
* allocator given in the subscription options is used.
*
* The taken pointer should point to an already allocated boolean in which the
* result can be stored.
*
* The rmw message_info struct contains meta information about this particular
* message instance, like what the GUID of the publisher which published it
* originally or whether or not the message received from within the same
* process.
* The message_info argument should be an already allocated rmw_message_info_t
* structure.
* Passing NULL for message_info will result in the argument being ignored.
*
* \param[in] subscription the handle to the subscription from which to take
* \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 RCL_RET_OK if the message was published, or
* RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
* RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or
* RCL_RET_BAD_ALLOC if allocating memory failed, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_take(
const rcl_subscription_t * subscription,
void * ros_message,
bool * taken,
rmw_message_info_t * message_info);
/// Get the topic name for the subscription.
/* This function returns the subscription's internal topic name string.
* This function can fail, and therefore return NULL, if the:
* - subscription is NULL
* - subscription is invalid (never called init, called fini, or invalid)
*
* The returned string is only valid as long as the subscription is valid.
* The value of the string may change if the topic name changes, and therefore
* copying the string is recommended if this is a concern.
*
* This function is not thread-safe, and copying the result is not thread-safe.
*
* \param[in] subscription the pointer to the subscription
* \return name string if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
const char *
rcl_subscription_get_topic_name(const rcl_subscription_t * subscription);
/// Return the rcl subscription options.
/* This function returns the subscription's internal options struct.
* This function can fail, and therefore return NULL, if the:
* - subscription is NULL
* - subscription is invalid (never called init, called fini, or invalid)
*
* The returned struct is only valid as long as the subscription is valid.
* The values in the struct may change if the subscription's options change,
* and therefore copying the struct is recommended if this is a concern.
*
* This function is not thread-safe, and copying the result is not thread-safe.
*
* \param[in] subscription pointer to the subscription
* \return options struct if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
const rcl_subscription_options_t *
rcl_subscription_get_options(const rcl_subscription_t * subscription);
/// Return the rmw subscription handle.
/* The handle returned is a pointer to the internally held rmw handle.
* This function can fail, and therefore return NULL, if the:
* - subscription is NULL
* - subscription is invalid (never called init, called fini, or invalid)
*
* The returned handle is made invalid if the subscription is finalized or if
* rcl_shutdown() is called.
* The returned handle is not guaranteed to be valid for the life time of the
* subscription as it may be finalized and recreated itself.
* Therefore it is recommended to get the handle from the subscription using
* this function each time it is needed and avoid use of the handle
* concurrently with functions that might change it.
*
* This function is not thread-safe.
*
* \param[in] subscription pointer to the rcl subscription
* \return rmw subscription handle if successful, otherwise NULL
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rmw_subscription_t *
rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription);
#if __cplusplus
}
#endif
#endif // RCL__SUBSCRIPTION_H_

130
rcl/include/rcl/time.h Normal file
View file

@ -0,0 +1,130 @@
// 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__TIME_H_
#define RCL__TIME_H_
#if __cplusplus
extern "C"
{
#endif
#include "rcl/macros.h"
#include "rcl/types.h"
#include "rcl/visibility_control.h"
#define RCL_S_TO_NS(seconds) (seconds * 1000 * 1000 * 1000)
#define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000 * 1000)
#define RCL_US_TO_NS(microseconds) (microseconds * 1000)
#define RCL_NS_TO_S(nanoseconds) (nanoseconds / 1000 * 1000 * 1000)
#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000 * 1000)
#define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000)
/// Struct which encapsulates a point in time according to a system clock.
/* The system clock's point of reference is the Unix Epoch (January 1st, 1970).
* See: http://stackoverflow.com/a/32189845/671658
* Therefore all times in this struct are positive and count the nanoseconds
* since the Unix epoch and this struct cannot be used on systems which report
* a current time before the Unix Epoch.
* Leap seconds are not considered.
*
* The struct represents time as nanoseconds in an unsigned 64-bit integer.
* The struct is capable of representing any time until the year 2554 with
* nanosecond precisions.
*/
typedef struct rcl_system_time_point_t
{
uint64_t nanoseconds;
} rcl_system_time_point_t;
/// Struct which encapsulates a point in time according to a steady clock.
/* The steady clock's point of reference is system defined, but often the time
* that the process started plus a signed random offset.
* See: http://stackoverflow.com/a/32189845/671658
* However, the clock is guaranteed to be monotonically increasing.
* Therefore all times in this struct are positive and count nanoseconds
* since an unspecified, but constant point in time.
*
* The struct represents time as nanoseconds in an unsigned 64-bit integer.
*/
typedef struct rcl_steady_time_point_t
{
uint64_t nanoseconds;
} rcl_steady_time_point_t;
/// Struct which encapsulates a duration of time between two points in time.
/* The struct can represent any time within the range [~292 years, ~-292 years]
* with nanosecond precision.
*/
typedef struct rcl_duration_t
{
int64_t nanoseconds;
} rcl_duration_t;
/// Retrieve the current time as a rcl_system_time_point_t struct.
/* This function returns the time from a system clock.
* The closest equivalent would be to std::chrono::system_clock::now();
*
* The resolution (e.g. nanoseconds vs microseconds) is not guaranteed.
*
* The now argument must point to an allocated rcl_system_time_point_t struct,
* as the result is copied into this variable.
*
* This function may allocate heap memory when an error occurs.
* This function is thread-safe.
* This function is lock-free, with an exception on Windows.
* On Windows this is lock-free if the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t.
*
* \param[out] now a struct in which the current time is stored
* \return RCL_RET_OK if the current time was successfully obtained, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_system_time_point_now(rcl_system_time_point_t * now);
/// Retrieve the current time as a rcl_steady_time_point_t struct.
/* This function returns the time from a monotonically increasing clock.
* The closest equivalent would be to std::chrono::steady_clock::now();
*
* The resolution (e.g. nanoseconds vs microseconds) is not guaranteed.
*
* The now argument must point to an allocated rcl_steady_time_point_t struct,
* as the result is copied into this variable.
*
* This function may allocate heap memory when an error occurs.
* This function is thread-safe.
* This function is lock-free, with an exception on Windows.
* On Windows this is lock-free if the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t.
*
* \param[out] now a struct in which the current time is stored
* \return RCL_RET_OK if the current time was successfully obtained, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_steady_time_point_now(rcl_steady_time_point_t * now);
#if __cplusplus
}
#endif
#endif // RCL__TIME_H_

412
rcl/include/rcl/timer.h Normal file
View file

@ -0,0 +1,412 @@
// 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__TIMER_H_
#define RCL__TIMER_H_
#if __cplusplus
extern "C"
{
#endif
#include <stdbool.h>
#include "rcl/allocator.h"
#include "rcl/macros.h"
#include "rcl/time.h"
#include "rcl/types.h"
#include "rmw/rmw.h"
struct rcl_timer_impl_t;
/// Handle for a ROS timer.
typedef struct rcl_timer_t
{
/// Private implementation pointer.
struct rcl_timer_impl_t * impl;
} rcl_timer_t;
/// User callback signature for timers.
/* The first argument the callback gets is a pointer to the timer.
* This can be used to cancel the timer, query the time until the next
* timer callback, exchange the callback with a different one, etc.
*
* The only caveat is that the function rcl_timer_get_time_since_last_call will
* return the time since just before this callback was called, not the last.
* Therefore the second argument given is the time since the previous callback
* was called, because that information is no longer accessible via the timer.
* The time since the last callback call is given in nanoseconds.
*/
typedef void (* rcl_timer_callback_t)(rcl_timer_t *, uint64_t);
/// Return a zero initialized timer.
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_timer_t
rcl_get_zero_initialized_timer();
/// Initialize a timer.
/* A timer consists of a callback function and a period.
* A timer can be added to a wait set and waited on, such that the wait set
* will wake up when a timer is ready to be executed.
*
* A timer simply holds state and does not automatically call callbacks.
* It does not create any threads, register interrupts, or consume signals.
* For blocking behavior it can be used in conjunction with a wait set and
* rcl_wait().
* When rcl_timer_is_ready() returns true, the timer must still be called
* explicitly using rcl_timer_call().
*
* The timer handle must be a pointer to an allocated and zero initialized
* rcl_timer_t struct.
* Calling this function on an already initialized timer will fail.
* Calling this function on a timer struct which has been allocated but not
* zero initialized is undefined behavior.
*
* The period is a duration (rather an absolute time in the future).
* If the period is 0 then it will always be ready.
*
* The callback must be a function which returns void and takes two arguments,
* the first being a pointer to the associated timer, and the second a uint64_t
* which is the time since the previous call, or since the timer was created
* if it is the first call to the callback.
*
* Expected usage:
*
* #include <rcl/rcl.h>
*
* void my_timer_callback(rcl_timer_t * timer, uint64_t last_call_time)
* {
* // Do timer work...
* // Optionally reconfigure, cancel, or reset the timer...
* }
*
* rcl_timer_t timer = rcl_get_zero_initialized_timer();
* rcl_ret_t ret =
* rcl_timer_init(&timer, RCL_MS_TO_NS(100), my_timer_callback, rcl_get_default_allocator());
* // ... error handling, use the timer with a wait set, or poll it manually, then cleanup
* ret = rcl_timer_fini(&timer);
* // ... error handling
*
* This function does allocate heap memory.
* This function is not thread-safe.
* This function is not lock-free.
*
* \param[inout] timer the timer handle to be initialized
* \param[in] period the duration between calls to the callback in nanoseconds
* \param[in] callback the user defined function to be called every period
* \param[in] allocator the allocator to use for allocations
* \return RCL_RET_OK if the timer was initialized successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ALREADY_INIT if the timer was already initialized, or
* RCL_RET_BAD_ALLOC if allocating memory failed, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_init(
rcl_timer_t * timer,
uint64_t period,
const rcl_timer_callback_t callback,
rcl_allocator_t allocator);
/// Finalize a timer.
/* This function will deallocate any memory and make the timer invalid.
*
* A timer that is already invalid (zero initialized) or NULL will not fail.
*
* This function is not thread-safe with any rcl_timer_* functions used on the
* same timer object.
*
* This function may allocate heap memory when an error occurs.
* This function is not thread-safe.
* This function is not lock-free.
*
* \param[inout] timer the handle to the timer to be finalized.
* \return RCL_RET_OK if the timer was finalized successfully, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_fini(rcl_timer_t * timer);
/// Call the timer's callback and set the last call time.
/* This function will call the callback and change the last call time even if
* the timer's period has not yet elapsed.
* It is up to the calling code to make sure the period has elapsed by first
* calling rcl_timer_is_ready().
* The order of operations in this command are as follows:
*
* - Ensure the timer has not been canceled.
* - Get the current time into a temporary rcl_steady_time_point_t.
* - Exchange the current time with the last call time of the timer.
* - Call the callback, passing this timer and the time since the last call.
* - Return after the callback has completed.
*
* During the callback the timer can be canceled or have its period and/or
* callback modified.
*
* This function may allocate heap memory when an error occurs.
* This function is thread-safe, but the user's callback may not be.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t, but the user's
* callback may not be lock-free.
*
* \param[inout] timer the handle to the timer to call
* \return RCL_RET_OK if the timer was called successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_TIMER_CANCELED if the timer has been canceled, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_call(rcl_timer_t * timer);
/// Calculates whether or not the timer should be called.
/* The result is true if the time until next call is less than, or equal to, 0
* and the timer has not been canceled.
* Otherwise the result is false, indicating the timer should not be called.
*
* The is_ready argument must point to an allocated bool object, as the result
* is copied into it.
*
* This function may allocate heap memory when an error occurs.
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t.
*
* \param[in] timer the handle to the timer which is being checked
* \param[out] is_ready the bool used to store the result of the calculation
* \return RCL_RET_OK if the last call time was retrieved successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready);
/// Calculate and retrieve the time until the next call in nanoseconds.
/* This function calculates the time until the next call by adding the timer's
* period to the last call time and subtracting that sum from the current time.
* The calculated time until the next call can be positive, indicating that it
* is not ready to be called as the period has not elapsed since the last call.
* The calculated time until the next call can also be 0 or negative,
* indicating that the period has elapsed since the last call and the timer
* should be called.
* A negative value indicates the timer call is overdue by that amount.
*
* The time_until_next_call argument must point to an allocated int64_t, as the
* the time until is coped into that instance.
*
* This function may allocate heap memory when an error occurs.
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t.
*
* \param[in] timer the handle to the timer that is being queried
* \param[out] time_until_next_call the output variable for the result
* \return RCL_RET_OK if the timer until next call was successfully calculated, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call);
/// Retrieve the time since the previous call to rcl_timer_call() occurred.
/* This function calculates the time since the last call and copies it into
* the given uint64_t variable.
*
* Calling this function within a callback will not return the time since the
* previous call but instead the time since the current callback was called.
*
* The time_since_last_call argument must be a pointer to an already allocated
* uint64_t.
*
* This function may allocate heap memory when an error occurs.
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t.
*
* \param[in] timer the handle to the timer which is being queried
* \param[out] time_since_last_call the struct in which the time is stored
* \return RCL_RET_OK if the last call time was retrieved successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call);
/// Retrieve the period of the timer.
/* This function retrieves the period and copies it into the give variable.
*
* The period argument must be a pointer to an already allocated uint64_t.
*
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t.
*
* \param[in] timer the handle to the timer which is being queried
* \param[out] period the uint64_t in which the period is stored
* \return RCL_RET_OK if the period was retrieved successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period);
/// Exchange the period of the timer and return the previous period.
/* This function exchanges the period in the timer and copies the old one into
* the give variable.
*
* Exchanging (changing) the period will not affect already waiting wait sets.
*
* The old_period argument must be a pointer to an already allocated uint64_t.
*
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t.
*
* \param[in] timer the handle to the timer which is being modified
* \param[out] new_period the uint64_t to exchange into the timer
* \param[out] old_period the uint64_t in which the previous period is stored
* \return RCL_RET_OK if the period was retrieved successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period);
/// Return the current timer callback.
/* This function can fail, and therefore return NULL, if:
* - timer is NULL
* - timer has not been initialized (the implementation is invalid)
*
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uintptr_t.
*
* \param[in] timer handle to the timer from the callback should be returned
* \return function pointer to the callback, or NULL if an error occurred
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_timer_callback_t
rcl_timer_get_callback(const rcl_timer_t * timer);
/// Exchange the current timer callback and return the current callback.
/* This function can fail, and therefore return NULL, if:
* - timer is NULL
* - timer has not been initialized (the implementation is invalid)
* - the new_callback argument is NULL
*
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_uintptr_t.
*
* \param[inout] timer handle to the timer from the callback should be exchanged
* \param[in] new_callback the callback to be exchanged into the timer
* \return function pointer to the old callback, or NULL if an error occurred
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_timer_callback_t
rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback);
/// Cancel a timer.
/* When a timer is canceled, rcl_timer_is_ready() will return false for that
* timer, and rcl_timer_call() will fail with RCL_RET_TIMER_CANCELED.
*
* A canceled timer can be reset with rcl_timer_reset(), and then used again.
* Calling this function on an already canceled timer will succeed.
*
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_bool.
*
* \param[inout] timer the timer to be canceled
* \return RCL_RET_OK if the last call time was retrieved successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_cancel(rcl_timer_t * timer);
/// Retrieve the canceled state of a timer.
/* If the timer is canceled true will be stored in the is_canceled argument.
* Otherwise false will be stored in the is_canceled argument.
*
* The is_canceled argument must point to an allocated bool, as the result is
* copied into this variable.
*
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_bool.
*
* \param[in] timer the timer to be queried
* \return RCL_RET_OK if the last call time was retrieved successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled);
/// Reset a timer.
/* This function can be called on a timer, canceled or not.
* For all timers it will reset the last call time to now.
* For canceled timers it will additionally make the timer not canceled.
*
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function
* atomic_is_lock_free() returns true for atomic_int_least64_t.
*
* \param[inout] timer the timer to be reset
* \return RCL_RET_OK if the last call time was retrieved successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_TIMER_INVALID if the timer is invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_timer_reset(rcl_timer_t * timer);
#if __cplusplus
}
#endif
#endif // RCL__TIMER_H_

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

@ -19,40 +19,28 @@
typedef rmw_ret_t rcl_ret_t;
#define RCL_RET_OK RMW_RET_OK
typedef rmw_node_t rcl_node_t;
typedef rmw_publisher_t rcl_publisher_t;
typedef rmw_subscription_t rcl_subscription_t;
typedef rmw_guard_condition_t rcl_guard_condition_t;
typedef rmw_subscriptions_t rcl_subscriptions_t;
typedef rmw_guard_conditions_t rcl_guard_conditions_t;
typedef struct rcl_callback_group_t
{
rcl_node_t ** node;
} rcl_callback_group_t;
typedef struct rcl_subscription_info_t
{
rcl_subscription_t ** subscription;
rcl_callback_group_t ** callback_group;
} rcl_subscription_info_t;
typedef struct rcl_timer_info_t
{
rcl_guard_condition_t ** guard_condition;
rcl_callback_group_t ** callback_group;
} rcl_timer_info_t;
typedef struct rcl_executor_helper_t
{
// TODO(wjwwood): fill with something
} rcl_executor_helper_t;
typedef struct rcl_any_executable_t
{
rcl_subscription_info_t ** subscription_info;
rcl_timer_info_t ** timer_info;
} rcl_any_executable_t;
#define RCL_RET_ERROR RMW_RET_ERROR
#define RCL_RET_TIMEOUT RMW_RET_TIMEOUT
// 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
#define RCL_RET_PUBLISHER_INVALID 300
// rcl subscription specific ret codes in 4XX
#define RCL_RET_SUBSCRIPTION_INVALID 400
// rcl service client specific ret codes in 5XX
// rcl service server specific ret codes in 6XX
// rcl guard condition specific ret codes in 7XX
// rcl timer specific ret codes in 8XX
#define RCL_RET_TIMER_INVALID 800
#define RCL_RET_TIMER_CANCELED 801
// rcl wait and wait set specific ret codes in 9XX
#define RCL_RET_WAIT_SET_INVALID 900
#define RCL_RET_WAIT_SET_EMPTY 901
#define RCL_RET_WAIT_SET_FULL 902
#endif // RCL__TYPES_H_

58
rcl/include/rcl/visibility_control.h Normal file
View file

@ -0,0 +1,58 @@
// 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__VISIBILITY_CONTROL_H_
#define RCL__VISIBILITY_CONTROL_H_
#if __cplusplus
extern "C"
{
#endif
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define RCL_EXPORT __attribute__ ((dllexport))
#define RCL_IMPORT __attribute__ ((dllimport))
#else
#define RCL_EXPORT __declspec(dllexport)
#define RCL_IMPORT __declspec(dllimport)
#endif
#ifdef RCL_BUILDING_DLL
#define RCL_PUBLIC RCL_EXPORT
#else
#define RCL_PUBLIC RCL_IMPORT
#endif
#define RCL_PUBLIC_TYPE RCL_PUBLIC
#define RCL_LOCAL
#else
#define RCL_EXPORT __attribute__ ((visibility("default")))
#define RCL_IMPORT
#if __GNUC__ >= 4
#define RCL_PUBLIC __attribute__ ((visibility("default")))
#define RCL_LOCAL __attribute__ ((visibility("hidden")))
#else
#define RCL_PUBLIC
#define RCL_LOCAL
#endif
#define RCL_PUBLIC_TYPE
#endif
#if __cplusplus
}
#endif
#endif // RCL__VISIBILITY_CONTROL_H_

377
rcl/include/rcl/wait.h Normal file
View file

@ -0,0 +1,377 @@
// 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__WAIT_H_
#define RCL__WAIT_H_
#if __cplusplus
extern "C"
{
#endif
#include <stdbool.h>
#include <stddef.h>
#include "rcl/guard_condition.h"
#include "rcl/macros.h"
#include "rcl/subscription.h"
#include "rcl/timer.h"
#include "rcl/types.h"
#include "rcl/visibility_control.h"
struct rcl_wait_set_impl_t;
/// Container for subscription's, guard condition's, etc to be waited on.
typedef struct rcl_wait_set_t
{
/// Storage for subscription pointers.
const rcl_subscription_t ** subscriptions;
size_t size_of_subscriptions;
/// Storage for guard condition pointers.
const rcl_guard_condition_t ** guard_conditions;
size_t size_of_guard_conditions;
/// Storage for timer pointers.
const rcl_timer_t ** timers;
size_t size_of_timers;
/// Implementation specific storage.
struct rcl_wait_set_impl_t * impl;
} rcl_wait_set_t;
/// Return a rcl_wait_set_t struct with members set to NULL.
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_wait_set_t
rcl_get_zero_initialized_wait_set();
/// Initialize a rcl wait set with space for items to be waited on.
/* This function allocates space for the subscriptions and other wait-able
* entities that can be stored in the wait set.
* It also sets the allocator to the given allocator and initializes the pruned
* member to be false.
*
* The wait_set struct should be allocated and initialized to NULL.
* If the wait_set is allocated but the memory is uninitialized the behavior is
* undefined.
* Calling this function on a wait set that has already been initialized will
* result in an error.
* A wait set can be reinitialized if rcl_wait_set_fini() was called on it.
*
* To use the default allocator use rcl_get_default_allocator().
*
* Expected usage:
*
* #include <rcl/wait.h>
*
* rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
* rcl_ret_t ret = rcl_wait_set_init(&wait_set, 42, 42, rcl_get_default_allocator());
* // ... error handling, then use it, then call the matching fini:
* ret = rcl_wait_set_fini(&wait_set);
* // ... error handling
*
* This function is thread-safe for different wait_set objects.
* Thread-safety of this function requires a thread-safe allocator if the
* allocator is shared with other parts of the system.
*
* \param[inout] wait_set the wait set struct to be initialized
* \param[in] number_of_subscriptions non-zero size of the subscriptions set
* \param[in] number_of_guard_conditions non-zero size of the guard conditions set
* \param[in] number_of_timers non-zero size of the timers 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 arguments are invalid, or
* RCL_RET_BAD_ALLOC if allocating memory failed, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_init(
rcl_wait_set_t * wait_set,
size_t number_of_subscriptions,
size_t number_of_guard_conditions,
size_t number_of_timers,
rcl_allocator_t allocator);
/// Finalize a rcl wait set.
/* Deallocates any memory in the wait set that was allocated in
* rcl_wait_set_init() using the allocator given in the initialization.
*
* Calling this function on a zero initialized wait set will do nothing and
* return RCL_RET_OK.
* Calling this function on uninitialized memory results in undefined behavior.
* After calling this function the wait set will once again be zero initialized
* and so calling this function or rcl_wait_set_init() immediately after will
* succeed.
*
* This function is not thread-safe.
* This function is lock-free.
*
* \param[inout] wait_set the wait set struct to be finalized.
* \return RCL_RET_OK if the finalization was successful, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_fini(rcl_wait_set_t * wait_set);
/// Retrieve the wait set's allocator.
/* The allocator must be an allocated rcl_allocator_t struct, as the result is
* copied into this variable.
*
* This function is not thread-safe.
* This function is lock-free.
*
* \param[in] wait_set the handle to the wait set
* \param[out] allocator the rcl_allocator_t struct to which the result is copied
* \return RCL_RET_OK if the allocator was successfully retrieved, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator);
/// Store a pointer to the given subscription in the next empty spot in the set.
/* This function does not guarantee that the subscription is not already in the
* wait set.
*
* This function is not thread-safe.
* This function is lock-free.
*
* \param[inout] wait_set struct in which the subscription is to be stored
* \param[in] subscription the subscription to be added to the wait set
* \return RCL_RET_OK if added successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or
* RCL_RET_WAIT_SET_FULL if the subscription set is full, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_add_subscription(
rcl_wait_set_t * wait_set,
const rcl_subscription_t * subscription);
/// Remove (sets to NULL) the subscriptions in the wait set.
/* This function should be used after passing using rcl_wait, but before
* adding new subscriptions to the set.
*
* Calling this on an uninitialized (zero initialized) wait set will fail.
*
* This function is not thread-safe.
* This function is lock-free.
*
* \param[inout] wait_set struct to have its subscriptions cleared
* \return RCL_RET_OK if cleared successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set);
/// Reallocate space for the subscriptions in the wait set.
/* This function will deallocate and reallocate the memory for the
* subscriptions set.
*
* A size of 0 will just deallocate the memory and assign NULL to the array.
*
* Allocation and deallocation is done with the allocator given during the
* wait set's initialization.
*
* After calling this function all values in the set will be set to NULL,
* effectively the same as calling rcl_wait_set_clear_subscriptions().
*
* If the requested size matches the current size, no allocation will be done.
*
* This can be called on an uninitialized (zero initialized) wait set.
*
* This function is not thread-safe.
*
* \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 arguments are invalid, or
* RCL_RET_BAD_ALLOC if allocating memory failed, or
* RCL_RET_ERROR if an unspecified error occurs.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size);
/// Store a pointer to the guard condition in the next empty spot in the set.
/* This function behaves exactly the same as for subscriptions.
* \see rcl_wait_set_add_subscription
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_add_guard_condition(
rcl_wait_set_t * wait_set,
const rcl_guard_condition_t * guard_condition);
/// Remove (sets to NULL) the guard conditions in the wait set.
/* This function behaves exactly the same as for subscriptions.
* \see rcl_wait_set_clear_subscriptions
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set);
/// Reallocate space for the guard conditions in the wait set.
/* This function behaves exactly the same as for subscriptions.
* \see rcl_wait_set_resize_subscriptions
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
/// Store a pointer to the timer in the next empty spot in the set.
/* This function behaves exactly the same as for subscriptions.
* \see rcl_wait_set_add_subscription
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_add_timer(
rcl_wait_set_t * wait_set,
const rcl_timer_t * timer);
/// Remove (sets to NULL) the timers in the wait set.
/* This function behaves exactly the same as for subscriptions.
* \see rcl_wait_set_clear_subscriptions
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set);
/// Reallocate space for the timers in the wait set.
/* This function behaves exactly the same as for subscriptions.
* \see rcl_wait_set_resize_subscriptions
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size);
/// Block until the wait set is ready or until the timeout has been exceeded.
/* This function will collect the items in the rcl_wait_set_t and pass them
* to the underlying rmw_wait function.
*
* The items in the wait set will be either left untouched or set to NULL after
* this function returns.
* Items that are not NULL are ready, where ready means different things based
* on the type of the item.
* For subscriptions this means there are messages that can be taken.
* For guard conditions this means the guard condition was triggered.
*
* Expected usage:
*
* #include <rcl/rcl.h>
*
* // rcl_init() called successfully before here...
* rcl_node_t node; // initialize this, see rcl_node_init()
* rcl_subscription_t sub1; // initialize this, see rcl_subscription_init()
* rcl_subscription_t sub2; // initialize this, see rcl_subscription_init()
* rcl_guard_condition_t gc1; // initialize this, see rcl_guard_condition_init()
* rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
* rcl_ret_t ret = rcl_wait_set_init(&wait_set, 2, 1, rcl_get_default_allocator());
* // ... error handling
* do {
* ret = rcl_wait_set_clear_subscriptions(&wait_set);
* // ... error handling
* ret = rcl_wait_set_clear_guard_conditions(&wait_set);
* // ... error handling
* ret = rcl_wait_set_add_subscription(&wait_set, &sub1);
* // ... error handling
* ret = rcl_wait_set_add_subscription(&wait_set, &sub2);
* // ... error handling
* ret = rcl_wait_set_add_guard_condition(&wait_set, &gc1);
* // ... error handling
* ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1000)); // 1000ms == 1s, passed as ns
* if (ret == RCL_RET_TIMEOUT) {
* continue;
* }
* for (int i = 0; i < wait_set.size_of_subscriptions; ++i) {
* if (wait_set.subscriptions[i]) {
* // The subscription is ready...
* }
* }
* for (int i = 0; i < wait_set.size_of_guard_conditions; ++i) {
* if (wait_set.guard_conditions[i]) {
* // The subscription is ready...
* }
* }
* } while(check_some_condition());
* // ... fini node, and subscriptions and guard conditions...
* ret = rcl_wait_set_fini(&wait_set);
* // ... error handling
*
* The wait set struct must be allocated, initialized, and should have been
* cleared and then filled with items, e.g. subscriptions and guard conditions.
* Passing a wait set with no wait-able items in it will fail.
* NULL items in the sets are ignored, e.g. it is valid to have as input:
* - subscriptions[0] = valid pointer
* - subscriptions[1] = NULL
* - subscriptions[2] = valid pointer
* - size_of_subscriptions = 3
* Passing an uninitialized (zero initialized) wait set struct will fail.
* Passing a wait set struct with uninitialized memory is undefined behavior.
*
* The unit of timeout is nanoseconds.
* If the timeout is negative then this function will block indefinitely until
* something in the wait set is valid or it is interrupted.
* If the timeout is 0 then this function will be non-blocking; checking what's
* ready now, but not waiting if nothing is ready yet.
* If the timeout is greater than 0 then this function will return after
* that period of time has elapsed or the wait set becomes ready, which ever
* comes first.
* Passing a timeout struct with uninitialized memory is undefined behavior.
*
* \TODO(wjwwood) this function should probably be thread-safe with itself but
* it's not clear to me what happens if the wait sets being
* waited on can be overlapping or not or if we can even check.
* This function is not thread-safe and cannot be called concurrently, even if
* the given wait sets are not the same and non-overlapping in contents.
*
* \param[inout] wait_set the set of things to be waited on and to be pruned if not ready
* \param[in] timeout the duration to wait for the wait set to be ready, in nanoseconds
* \return RCL_RET_OK something in the wait set became ready, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or
* RCL_RET_WAIT_SET_EMPTY if the wait set contains no items, or
* RCL_RET_TIMEOUT if the timeout expired before something was ready, or
* RCL_RET_ERROR an unspecified error occur.
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout);
#if __cplusplus
}
#endif
#endif // RCL__WAIT_H_

14
rcl/package.xml
View file

@ -10,12 +10,22 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rmw</build_depend>
<build_export_depend>rmw</build_export_depend>
<build_depend>rcl_interfaces</build_depend>
<build_depend>rmw_implementation_cmake</build_depend>
<build_depend>rosidl_generator_c</build_depend>
<build_export_depend>rcl_interfaces</build_export_depend>
<build_export_depend>rosidl_generator_c</build_export_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<depend>rmw_implementation</depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>rmw</test_depend>
<export>
<build_type>ament_cmake</build_type>

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

@ -0,0 +1,65 @@
// 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"
#include "rcl/error_handling.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
free(pointer);
}
static void *
__default_reallocate(void * pointer, size_t size, void * state)
{
(void)state; // unused
return realloc(pointer, size);
}
rcl_allocator_t
rcl_get_default_allocator()
{
static rcl_allocator_t default_allocator = {
__default_allocate,
__default_deallocate,
__default_reallocate,
NULL
};
return default_allocator;
}
void *
rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator)
{
if (!allocator || !allocator->reallocate || !allocator->deallocate) {
RCL_SET_ERROR_MSG("invalid allocator or allocator function pointers");
return NULL;
}
void * new_pointer = allocator->reallocate(pointer, size, allocator->state);
if (!new_pointer) {
allocator->deallocate(pointer, allocator->state);
}
return new_pointer;
}

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

@ -0,0 +1,55 @@
// 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)
# define WINDOWS_ENV_BUFFER_SIZE 2048
static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE];
#endif // defined(WIN32)
rcl_ret_t
rcl_impl_getenv(const char * env_name, const char ** env_value)
{
RCL_CHECK_ARGUMENT_FOR_NULL(env_name, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(env_value, RCL_RET_INVALID_ARGUMENT);
*env_value = NULL;
#if !defined(WIN32)
*env_value = getenv(env_name);
if (*env_value == NULL) {
*env_value = "";
}
#else // !defined(WIN32)
size_t required_size;
errno_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_buffer[WINDOWS_ENV_BUFFER_SIZE - 1] = '\0';
*env_value = __env_buffer;
#endif // !defined(WIN32)
return RCL_RET_OK;
}
#if __cplusplus
}
#endif

60
rcl/src/rcl/common.h Normal file
View file

@ -0,0 +1,60 @@
// 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_ARGUMENT_FOR_NULL(argument, error_return_type) \
RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " 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); \
error_statement; \
}
/// Retrieve the value of the given environment variable if it exists, or "".
/* The returned cstring is only valid until the next time this function is
* called, because it is a direct pointer to the static storage.
* The returned value char * variable should never have free() called on it.
* If the environment variable is not set, an empty string will be returned.
*
* Environment variables will be truncated at 2048 characters on Windows.
*
* This function does not allocate heap memory, but the system calls might.
* This function is not thread-safe.
* This function is not lock-free.
*
* \param[in] env_name the name of the environment variable
* \param[out] env_value pointer to the value cstring, or "" if unset
* \return RCL_RET_OK if the value is retrieved successfully, or
* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
* RCL_RET_ERROR an unspecified error occur.
*/
rcl_ret_t
rcl_impl_getenv(const char * env_name, const char ** env_value);
#if __cplusplus
}
#endif
#endif // RCL__COMMON_H_

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

@ -0,0 +1,149 @@
// 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);
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;
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_handle(const 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

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

@ -0,0 +1,207 @@
// 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/node.h"
#include <limits.h>
#include <stdlib.h>
#include <string.h>
#include "rcl/rcl.h"
#include "rmw/rmw.h"
#include "./common.h"
typedef struct rcl_node_impl_t
{
rcl_node_options_t options;
rmw_node_t * rmw_node_handle;
uint64_t rcl_instance_id;
size_t actual_domain_id;
} rcl_node_impl_t;
rcl_node_t
rcl_get_zero_initialized_node()
{
static rcl_node_t null_node = {0};
return null_node;
}
rcl_ret_t
rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options)
{
size_t domain_id = 0;
const char * ros_domain_id;
rcl_ret_t ret;
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("node already initialized, or struct memory was unintialized");
return RCL_RET_ALREADY_INIT;
}
// Make sure rcl has been initialized.
if (!rcl_ok()) {
RCL_SET_ERROR_MSG("rcl_init() has not been called");
return RCL_RET_NOT_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.
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_BAD_ALLOC);
// 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 options (assume it is trivially copyable)
node->impl->options = *options;
// node rmw_node_handle
if (node->impl->options.domain_id == RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID) {
// Find the domain ID set by the environment.
ret = rcl_impl_getenv("ROS_DOMAIN_ID", &ros_domain_id);
if (ret != RCL_RET_OK) {
goto fail;
}
if (ros_domain_id) {
unsigned long number = strtoul(ros_domain_id, NULL, 0); // NOLINT(runtime/int)
if (number == ULONG_MAX) {
RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number");
goto fail;
}
domain_id = (size_t)number;
}
} else {
domain_id = node->impl->options.domain_id;
}
node->impl->actual_domain_id = domain_id;
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) {
allocator->deallocate(node->impl, allocator->state);
}
return fail_ret;
}
rcl_ret_t
rcl_node_fini(rcl_node_t * node)
{
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 ret = rmw_destroy_node(node->impl->rmw_node_handle);
if (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;
RCL_CHECK_FOR_NULL_WITH_MSG(
allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
allocator.deallocate(node->impl, allocator.state);
node->impl = NULL;
return result;
}
rcl_node_options_t
rcl_node_get_default_options()
{
static rcl_node_options_t default_options = {
.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_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->rmw_node_handle->name;
}
const rcl_node_options_t *
rcl_node_get_options(const rcl_node_t * node)
{
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->options;
}
rcl_ret_t
rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id)
{
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl, "node implementation is invalid", return RCL_RET_NODE_INVALID);
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 RCL_RET_NODE_INVALID;
}
*domain_id = node->impl->actual_domain_id;
return RCL_RET_OK;
}
rmw_node_t *
rcl_node_get_rmw_handle(const rcl_node_t * node)
{
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->rmw_node_handle;
}
uint64_t
rcl_node_get_rcl_instance_id(const rcl_node_t * node)
{
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;
}
#if __cplusplus
}
#endif

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

@ -0,0 +1,164 @@
// 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_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;
}
rcl_ret_t
rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
{
rcl_ret_t result = RCL_RET_OK;
RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
if (publisher->impl) {
rmw_ret_t ret =
rmw_destroy_publisher(rcl_node_get_rmw_handle(node), publisher->impl->rmw_handle);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
result = RCL_RET_ERROR;
}
rcl_allocator_t allocator = publisher->impl->options.allocator;
allocator.deallocate(publisher->impl, allocator.state);
}
return result;
}
rcl_publisher_options_t
rcl_publisher_get_default_options()
{
static rcl_publisher_options_t default_options;
// Must set the allocator and qos after because they are not a compile time constant.
default_options.qos = rmw_qos_profile_default;
default_options.allocator = rcl_get_default_allocator();
return default_options;
}
rcl_ret_t
rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
{
RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(
publisher->impl, "publisher is invalid", return RCL_RET_PUBLISHER_INVALID);
if (rmw_publish(publisher->impl->rmw_handle, ros_message) != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
return RCL_RET_ERROR;
}
return RCL_RET_OK;
}
const char *
rcl_publisher_get_topic_name(const rcl_publisher_t * publisher)
{
RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
publisher->impl, "publisher is invalid", return NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
publisher->impl->rmw_handle, "publisher is invalid", return NULL);
return publisher->impl->rmw_handle->topic_name;
}
const rcl_publisher_options_t *
rcl_publisher_get_options(const rcl_publisher_t * publisher)
{
RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
publisher->impl, "publisher is invalid", return NULL);
return &publisher->impl->options;
}
rmw_publisher_t *
rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher)
{
RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
publisher->impl, "publisher is invalid", return NULL);
return publisher->impl->rmw_handle;
}
#if __cplusplus
}
#endif

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

@ -0,0 +1,131 @@
// 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 <string.h>
#include "./common.h"
#include "./stdatomic_helper.h"
#include "rcl/error_handling.h"
static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false);
static rcl_allocator_t __rcl_allocator;
static int __rcl_argc = 0;
static char ** __rcl_argv = NULL;
static atomic_uint_least64_t __rcl_instance_id = ATOMIC_VAR_INIT(0);
static uint64_t __rcl_next_unique_id = 0;
static void
__clean_up_init()
{
if (__rcl_argv) {
int i;
for (i = 0; i < __rcl_argc; ++i) {
if (__rcl_argv[i]) {
// Use the old allocator.
__rcl_allocator.deallocate(__rcl_argv[i], __rcl_allocator.state);
}
}
// Use the old allocator.
__rcl_allocator.deallocate(__rcl_argv, __rcl_allocator.state);
}
__rcl_argc = 0;
__rcl_argv = NULL;
rcl_atomic_store(&__rcl_instance_id, 0);
rcl_atomic_store(&__rcl_is_initialized, false);
}
rcl_ret_t
rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
if (argc > 0) {
RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT);
}
RCL_CHECK_FOR_NULL_WITH_MSG(
allocator.allocate,
"invalid allocator, allocate not set", return RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(
allocator.deallocate,
"invalid allocator, deallocate not set", return RCL_RET_INVALID_ARGUMENT);
if (rcl_atomic_exchange_bool(&__rcl_is_initialized, true)) {
RCL_SET_ERROR_MSG("rcl_init called while already initialized");
return RCL_RET_ALREADY_INIT;
}
// There is a race condition between the time __rcl_is_initialized is set true,
// and when the allocator is set, in which rcl_shutdown() could get rcl_ok() as
// true and try to use the allocator, but it isn't set yet...
// A very unlikely race condition, but it is possile I think.
// I've documented that rcl_init() and rcl_shutdown() are not thread-safe with each other.
__rcl_allocator = allocator; // Set the new allocator.
// 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");
fail_ret = RCL_RET_BAD_ALLOC;
goto fail;
}
memset(__rcl_argv, 0, sizeof(char **) * argc);
int i;
for (i = 0; i < argc; ++i) {
__rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state);
memcpy(__rcl_argv[i], argv[i], strlen(argv[i]));
}
rcl_atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id);
if (rcl_atomic_load_uint64_t(&__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 fail_ret;
}
rcl_ret_t
rcl_shutdown()
{
if (!rcl_ok()) {
RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init");
return RCL_RET_NOT_INIT;
}
__clean_up_init();
return RCL_RET_OK;
}
uint64_t
rcl_get_instance_id()
{
return rcl_atomic_load_uint64_t(&__rcl_instance_id);
}
bool
rcl_ok()
{
return rcl_atomic_load_bool(&__rcl_is_initialized);
}
#if __cplusplus
}
#endif

111
rcl/src/rcl/stdatomic_helper.h Normal file
View file

@ -0,0 +1,111 @@
// 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__STDATOMIC_HELPER_H_
#define RCL__STDATOMIC_HELPER_H_
#if !defined(WIN32)
#if defined(__GNUC__) && __GNUC__ <= 4 && __GNUC_MINOR__ <= 9
// If GCC and below GCC-4.9, use the compatability header.
#include "stdatomic_helper/gcc/stdatomic.h"
#elif defined(__clang__) && defined(__has_feature) && !__has_feature(c_atomic)
// If Clang and no c_atomics (true for some older versions), use the compatability header.
#include "stdatomic_helper/gcc/stdatomic.h"
#else
#include <stdatomic.h>
#endif
#define rcl_atomic_load(object, out) (out) = atomic_load(object)
#define rcl_atomic_compare_exchange_strong(object, out, expected, desired) \
(out) = atomic_compare_exchange_strong(object, expected, desired)
#define rcl_atomic_exchange(object, out, desired) (out) = atomic_exchange(object, desired)
#define rcl_atomic_store(object, desired) atomic_store(object, desired)
#else // !defined(WIN32)
#include "./stdatomic_helper/win32/stdatomic.h"
#define rcl_atomic_load(object, out) rcl_win32_atomic_load(object, out)
#define rcl_atomic_compare_exchange_strong(object, out, expected, desired) \
rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired)
#define rcl_atomic_exchange(object, out, desired) rcl_win32_atomic_exchange(object, out, desired)
#define rcl_atomic_store(object, desired) rcl_win32_atomic_store(object, desired)
#endif // !defined(WIN32)
static inline bool
rcl_atomic_load_bool(atomic_bool * a_bool)
{
bool result = false;
rcl_atomic_load(a_bool, result);
return result;
}
static inline uint64_t
rcl_atomic_load_uint64_t(atomic_uint_least64_t * a_uint64_t)
{
uint64_t result = 0;
rcl_atomic_load(a_uint64_t, result);
return result;
}
static inline uintptr_t
rcl_atomic_load_uintptr_t(atomic_uintptr_t * a_uintptr_t)
{
uintptr_t result = 0;
rcl_atomic_load(a_uintptr_t, result);
return result;
}
static inline bool
rcl_atomic_compare_exchange_strong_uint_least64_t(
atomic_uint_least64_t * a_uint_least64_t, uint64_t * expected, uint64_t desired)
{
bool result;
rcl_atomic_compare_exchange_strong(a_uint_least64_t, result, expected, desired);
return result;
}
static inline bool
rcl_atomic_exchange_bool(atomic_bool * a_bool, bool desired)
{
bool result;
rcl_atomic_exchange(a_bool, result, desired);
return result;
}
static inline uint64_t
rcl_atomic_exchange_uint64_t(atomic_uint_least64_t * a_uint64_t, uint64_t desired)
{
uint64_t result;
rcl_atomic_exchange(a_uint64_t, result, desired);
return result;
}
static inline uint64_t
rcl_atomic_exchange_uintptr_t(atomic_uintptr_t * a_uintptr_t, uintptr_t desired)
{
uintptr_t result;
rcl_atomic_exchange(a_uintptr_t, result, desired);
return result;
}
#endif // RCL__STDATOMIC_HELPER_H_

400
rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h Normal file
View file

@ -0,0 +1,400 @@
// 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.
/* This file was taken from:
* https://gist.github.com/nhatminhle/5181506
* It is a modification of the FreeBSD stdatomic header with some modifications
* so that it will work with gcc 4.7+ and Clang 3.1+.
* The original FreeBSD header can be found here:
* https://github.com/freebsd/freebsd/blob/release/10.2.0/sys/sys/stdatomic.h
*/
// *INDENT-OFF* (disable uncrustify)
/**** Start included file. ****/
/*
* An implementation of C11 stdatomic.h directly borrowed from FreeBSD
* (original copyright follows), with minor modifications for
* portability to other systems. Works for recent Clang (that
* implement the feature c_atomic) and GCC 4.7+; includes
* compatibility for GCC below 4.7 but I wouldn't recommend it.
*
* Caveats and limitations:
* - Only the ``_Atomic parentheses'' notation is implemented, while
* the ``_Atomic space'' one is not.
* - _Atomic types must be typedef'ed, or programs using them will
* not type check correctly (incompatible anonymous structure
* types).
* - Non-scalar _Atomic types would require runtime support for
* runtime locking, which, as far as I know, is not currently
* available on any system.
*/
/*-
* Copyright (c) 2011 Ed Schouten <ed@FreeBSD.org>
* David Chisnall <theraven@FreeBSD.org>
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* $FreeBSD: src/include/stdatomic.h,v 1.10.2.2 2012/05/30 19:21:54 theraven Exp $
*/
#ifndef _STDATOMIC_H_ // NOLINT
#define _STDATOMIC_H_
#if __cplusplus
// This will suppress warnings about _Bool not being defined.
typedef bool _Bool;
#endif
#include <stddef.h>
#include <stdint.h>
#if !defined(__has_feature)
#define __has_feature(x) 0
#endif
#if !defined(__has_builtin)
#define __has_builtin(x) 0
#endif
#if !defined(__GNUC_PREREQ__)
#if defined(__GNUC__) && defined(__GNUC_MINOR__)
#define __GNUC_PREREQ__(maj, min) \
((__GNUC__ << 16) + __GNUC_MINOR__ >= ((maj) << 16) + (min))
#else
#define __GNUC_PREREQ__(maj, min) 0
#endif
#endif
#if !defined(__CLANG_ATOMICS) && !defined(__GNUC_ATOMICS)
#if __has_feature(c_atomic)
#define __CLANG_ATOMICS
#elif __GNUC_PREREQ__(4, 7)
#define __GNUC_ATOMICS
#elif !defined(__GNUC__)
#error "stdatomic.h does not support your compiler"
#endif
#endif
#if !defined(__CLANG_ATOMICS)
#define _Atomic(T) struct { volatile __typeof__(T) __val; }
#endif
/*
* 7.17.2 Initialization.
*/
#if defined(__CLANG_ATOMICS)
#define ATOMIC_VAR_INIT(value) (value)
#define atomic_init(obj, value) __c11_atomic_init(obj, value)
#else
#define ATOMIC_VAR_INIT(value) { .__val = (value) }
#define atomic_init(obj, value) do { \
(obj)->__val = (value); \
} while (0)
#endif
/*
* Clang and recent GCC both provide predefined macros for the memory
* orderings. If we are using a compiler that doesn't define them, use the
* clang values - these will be ignored in the fallback path.
*/
#ifndef __ATOMIC_RELAXED
#define __ATOMIC_RELAXED 0
#endif
#ifndef __ATOMIC_CONSUME
#define __ATOMIC_CONSUME 1
#endif
#ifndef __ATOMIC_ACQUIRE
#define __ATOMIC_ACQUIRE 2
#endif
#ifndef __ATOMIC_RELEASE
#define __ATOMIC_RELEASE 3
#endif
#ifndef __ATOMIC_ACQ_REL
#define __ATOMIC_ACQ_REL 4
#endif
#ifndef __ATOMIC_SEQ_CST
#define __ATOMIC_SEQ_CST 5
#endif
/*
* 7.17.3 Order and consistency.
*
* The memory_order_* constants that denote the barrier behaviour of the
* atomic operations.
*/
enum memory_order {
memory_order_relaxed = __ATOMIC_RELAXED,
memory_order_consume = __ATOMIC_CONSUME,
memory_order_acquire = __ATOMIC_ACQUIRE,
memory_order_release = __ATOMIC_RELEASE,
memory_order_acq_rel = __ATOMIC_ACQ_REL,
memory_order_seq_cst = __ATOMIC_SEQ_CST
};
typedef enum memory_order memory_order;
/*
* 7.17.4 Fences.
*/
#ifdef __CLANG_ATOMICS
#define atomic_thread_fence(order) __c11_atomic_thread_fence(order)
#define atomic_signal_fence(order) __c11_atomic_signal_fence(order)
#elif defined(__GNUC_ATOMICS)
#define atomic_thread_fence(order) __atomic_thread_fence(order)
#define atomic_signal_fence(order) __atomic_signal_fence(order)
#else
#define atomic_thread_fence(order) __sync_synchronize()
#define atomic_signal_fence(order) __asm volatile ("" : : : "memory")
#endif
/*
* 7.17.5 Lock-free property.
*/
#if defined(__CLANG_ATOMICS)
#define atomic_is_lock_free(obj) \
__c11_atomic_is_lock_free(sizeof(obj))
#elif defined(__GNUC_ATOMICS)
#define atomic_is_lock_free(obj) \
__atomic_is_lock_free(sizeof((obj)->__val))
#else
#define atomic_is_lock_free(obj) \
(sizeof((obj)->__val) <= sizeof(void *))
#endif
/*
* 7.17.6 Atomic integer types.
*/
typedef _Atomic(_Bool) atomic_bool;
typedef _Atomic(char) atomic_char;
typedef _Atomic(signed char) atomic_schar;
typedef _Atomic(unsigned char) atomic_uchar;
typedef _Atomic(short) atomic_short; // NOLINT
typedef _Atomic(unsigned short) atomic_ushort; // NOLINT
typedef _Atomic(int) atomic_int;
typedef _Atomic(unsigned int) atomic_uint;
typedef _Atomic(long) atomic_long; // NOLINT
typedef _Atomic(unsigned long) atomic_ulong; // NOLINT
typedef _Atomic(long long) atomic_llong; // NOLINT
typedef _Atomic(unsigned long long) atomic_ullong; // NOLINT
#if 0
typedef _Atomic(char16_t) atomic_char16_t;
typedef _Atomic(char32_t) atomic_char32_t;
#endif
typedef _Atomic(wchar_t) atomic_wchar_t;
typedef _Atomic(int_least8_t) atomic_int_least8_t;
typedef _Atomic(uint_least8_t) atomic_uint_least8_t;
typedef _Atomic(int_least16_t) atomic_int_least16_t;
typedef _Atomic(uint_least16_t) atomic_uint_least16_t;
typedef _Atomic(int_least32_t) atomic_int_least32_t;
typedef _Atomic(uint_least32_t) atomic_uint_least32_t;
typedef _Atomic(int_least64_t) atomic_int_least64_t;
typedef _Atomic(uint_least64_t) atomic_uint_least64_t;
typedef _Atomic(int_fast8_t) atomic_int_fast8_t;
typedef _Atomic(uint_fast8_t) atomic_uint_fast8_t;
typedef _Atomic(int_fast16_t) atomic_int_fast16_t;
typedef _Atomic(uint_fast16_t) atomic_uint_fast16_t;
typedef _Atomic(int_fast32_t) atomic_int_fast32_t;
typedef _Atomic(uint_fast32_t) atomic_uint_fast32_t;
typedef _Atomic(int_fast64_t) atomic_int_fast64_t;
typedef _Atomic(uint_fast64_t) atomic_uint_fast64_t;
typedef _Atomic(intptr_t) atomic_intptr_t;
typedef _Atomic(uintptr_t) atomic_uintptr_t;
typedef _Atomic(size_t) atomic_size_t;
typedef _Atomic(ptrdiff_t) atomic_ptrdiff_t;
typedef _Atomic(intmax_t) atomic_intmax_t;
typedef _Atomic(uintmax_t) atomic_uintmax_t;
/*
* 7.17.7 Operations on atomic types.
*/
/*
* Compiler-specific operations.
*/
#if defined(__CLANG_ATOMICS)
#define atomic_compare_exchange_strong_explicit(object, expected, \
desired, success, failure) \
__c11_atomic_compare_exchange_strong(object, expected, desired, \
success, failure)
#define atomic_compare_exchange_weak_explicit(object, expected, \
desired, success, failure) \
__c11_atomic_compare_exchange_weak(object, expected, desired, \
success, failure)
#define atomic_exchange_explicit(object, desired, order) \
__c11_atomic_exchange(object, desired, order)
#define atomic_fetch_add_explicit(object, operand, order) \
__c11_atomic_fetch_add(object, operand, order)
#define atomic_fetch_and_explicit(object, operand, order) \
__c11_atomic_fetch_and(object, operand, order)
#define atomic_fetch_or_explicit(object, operand, order) \
__c11_atomic_fetch_or(object, operand, order)
#define atomic_fetch_sub_explicit(object, operand, order) \
__c11_atomic_fetch_sub(object, operand, order)
#define atomic_fetch_xor_explicit(object, operand, order) \
__c11_atomic_fetch_xor(object, operand, order)
#define atomic_load_explicit(object, order) \
__c11_atomic_load(object, order)
#define atomic_store_explicit(object, desired, order) \
__c11_atomic_store(object, desired, order)
#elif defined(__GNUC_ATOMICS)
#define atomic_compare_exchange_strong_explicit(object, expected, \
desired, success, failure) \
__atomic_compare_exchange_n(&(object)->__val, expected, \
desired, 0, success, failure)
#define atomic_compare_exchange_weak_explicit(object, expected, \
desired, success, failure) \
__atomic_compare_exchange_n(&(object)->__val, expected, \
desired, 1, success, failure)
#define atomic_exchange_explicit(object, desired, order) \
__atomic_exchange_n(&(object)->__val, desired, order)
#define atomic_fetch_add_explicit(object, operand, order) \
__atomic_fetch_add(&(object)->__val, operand, order)
#define atomic_fetch_and_explicit(object, operand, order) \
__atomic_fetch_and(&(object)->__val, operand, order)
#define atomic_fetch_or_explicit(object, operand, order) \
__atomic_fetch_or(&(object)->__val, operand, order)
#define atomic_fetch_sub_explicit(object, operand, order) \
__atomic_fetch_sub(&(object)->__val, operand, order)
#define atomic_fetch_xor_explicit(object, operand, order) \
__atomic_fetch_xor(&(object)->__val, operand, order)
#define atomic_load_explicit(object, order) \
__atomic_load_n(&(object)->__val, order)
#define atomic_store_explicit(object, desired, order) \
__atomic_store_n(&(object)->__val, desired, order)
#else
#define atomic_compare_exchange_strong_explicit(object, expected, \
desired, success, failure) ({ \
__typeof__((object)->__val) __v; \
_Bool __r; \
__v = __sync_val_compare_and_swap(&(object)->__val, \
*(expected), desired); \
__r = *(expected) == __v; \
*(expected) = __v; \
__r; \
})
#define atomic_compare_exchange_weak_explicit(object, expected, \
desired, success, failure) \
atomic_compare_exchange_strong_explicit(object, expected, \
desired, success, failure)
#if __has_builtin(__sync_swap)
/* Clang provides a full-barrier atomic exchange - use it if available. */
#define atomic_exchange_explicit(object, desired, order) \
__sync_swap(&(object)->__val, desired)
#else
/*
* __sync_lock_test_and_set() is only an acquire barrier in theory (although in
* practice it is usually a full barrier) so we need an explicit barrier after
* it.
*/
#define atomic_exchange_explicit(object, desired, order) ({ \
__typeof__((object)->__val) __v; \
__v = __sync_lock_test_and_set(&(object)->__val, desired); \
__sync_synchronize(); \
__v; \
})
#endif
#define atomic_fetch_add_explicit(object, operand, order) \
__sync_fetch_and_add(&(object)->__val, operand)
#define atomic_fetch_and_explicit(object, operand, order) \
__sync_fetch_and_and(&(object)->__val, operand)
#define atomic_fetch_or_explicit(object, operand, order) \
__sync_fetch_and_or(&(object)->__val, operand)
#define atomic_fetch_sub_explicit(object, operand, order) \
__sync_fetch_and_sub(&(object)->__val, operand)
#define atomic_fetch_xor_explicit(object, operand, order) \
__sync_fetch_and_xor(&(object)->__val, operand)
#define atomic_load_explicit(object, order) \
__sync_fetch_and_add(&(object)->__val, 0)
#define atomic_store_explicit(object, desired, order) do { \
__sync_synchronize(); \
(object)->__val = (desired); \
__sync_synchronize(); \
} while (0)
#endif
/*
* Convenience functions.
*/
#define atomic_compare_exchange_strong(object, expected, desired) \
atomic_compare_exchange_strong_explicit(object, expected, \
desired, memory_order_seq_cst, memory_order_seq_cst)
#define atomic_compare_exchange_weak(object, expected, desired) \
atomic_compare_exchange_weak_explicit(object, expected, \
desired, memory_order_seq_cst, memory_order_seq_cst)
#define atomic_exchange(object, desired) \
atomic_exchange_explicit(object, desired, memory_order_seq_cst)
#define atomic_fetch_add(object, operand) \
atomic_fetch_add_explicit(object, operand, memory_order_seq_cst)
#define atomic_fetch_and(object, operand) \
atomic_fetch_and_explicit(object, operand, memory_order_seq_cst)
#define atomic_fetch_or(object, operand) \
atomic_fetch_or_explicit(object, operand, memory_order_seq_cst)
#define atomic_fetch_sub(object, operand) \
atomic_fetch_sub_explicit(object, operand, memory_order_seq_cst)
#define atomic_fetch_xor(object, operand) \
atomic_fetch_xor_explicit(object, operand, memory_order_seq_cst)
#define atomic_load(object) \
atomic_load_explicit(object, memory_order_seq_cst)
#define atomic_store(object, desired) \
atomic_store_explicit(object, desired, memory_order_seq_cst)
/*
* 7.17.8 Atomic flag type and operations.
*/
typedef atomic_bool atomic_flag;
#define ATOMIC_FLAG_INIT ATOMIC_VAR_INIT(0)
#define atomic_flag_clear_explicit(object, order) \
atomic_store_explicit(object, 0, order)
#define atomic_flag_test_and_set_explicit(object, order) \
atomic_compare_exchange_strong_explicit(object, 0, 1, order, order)
#define atomic_flag_clear(object) \
atomic_flag_clear_explicit(object, memory_order_seq_cst)
#define atomic_flag_test_and_set(object) \
atomic_flag_test_and_set_explicit(object, memory_order_seq_cst)
#endif /* !_STDATOMIC_H_ */ // NOLINT

361
rcl/src/rcl/stdatomic_helper/win32/stdatomic.h Normal file
View file

@ -0,0 +1,361 @@
// 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.
/*
* An implementation of C11 stdatomic.h for Win32, part borrowed from FreeBSD
* (original copyright follows), with major modifications for
* portability to Win32 systems.
*
* Caveats and limitations:
* - Only the ``_Atomic parentheses'' notation is implemented, while
* the ``_Atomic space'' one is not.
* - _Atomic types must be typedef'ed, or programs using them will
* not type check correctly (incompatible anonymous structure
* types).
* - Support is limited to 16, 32, and 64 bit types only.
* - Stripped down to only the functions used locally in rcl.
*/
/*-
* Copyright (c) 2011 Ed Schouten <ed@FreeBSD.org>
* David Chisnall <theraven@FreeBSD.org>
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* $FreeBSD: src/include/stdatomic.h,v 1.10.2.2 2012/05/30 19:21:54 theraven Exp $
*/
#if !defined(WIN32)
#error "this stdatomic.h does not support your compiler"
#endif
#ifndef RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_
#define RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_
#include <Windows.h>
#include <stddef.h>
#include <stdint.h>
// In MSVC, correct alignment of each type is already ensured.
#define _Atomic(T) struct { T __val; }
/*
* 7.17.2 Initialization.
*/
#define ATOMIC_VAR_INIT(value) {.__val = (value)}
#define atomic_init(obj, value) do { \
(obj)->__val = (value); \
} while (0)
/*
* Clang and recent GCC both provide predefined macros for the memory
* orderings. If we are using a compiler that doesn't define them, use the
* clang values - these will be ignored in the fallback path.
*/
#ifndef __ATOMIC_RELAXED
#define __ATOMIC_RELAXED 0
#endif
#ifndef __ATOMIC_CONSUME
#define __ATOMIC_CONSUME 1
#endif
#ifndef __ATOMIC_ACQUIRE
#define __ATOMIC_ACQUIRE 2
#endif
#ifndef __ATOMIC_RELEASE
#define __ATOMIC_RELEASE 3
#endif
#ifndef __ATOMIC_ACQ_REL
#define __ATOMIC_ACQ_REL 4
#endif
#ifndef __ATOMIC_SEQ_CST
#define __ATOMIC_SEQ_CST 5
#endif
/*
* 7.17.3 Order and consistency.
*
* The memory_order_* constants that denote the barrier behaviour of the
* atomic operations.
*/
enum memory_order
{
memory_order_relaxed = __ATOMIC_RELAXED,
memory_order_consume = __ATOMIC_CONSUME,
memory_order_acquire = __ATOMIC_ACQUIRE,
memory_order_release = __ATOMIC_RELEASE,
memory_order_acq_rel = __ATOMIC_ACQ_REL,
memory_order_seq_cst = __ATOMIC_SEQ_CST
};
typedef enum memory_order memory_order;
/*
* 7.17.4 Fences.
*/
#define atomic_thread_fence(order) MemoryBarrier()
#define atomic_signal_fence(order) _ReadWriteBarrier()
/*
* 7.17.5 Lock-free property.
*/
#define atomic_is_lock_free(obj) (sizeof((obj)->__val) <= sizeof(void *))
/*
* 7.17.6 Atomic integer types.
*/
typedef _Atomic (_Bool) atomic_bool;
typedef _Atomic (char) atomic_char;
typedef _Atomic (signed char) atomic_schar;
typedef _Atomic (unsigned char) atomic_uchar;
typedef _Atomic (short) atomic_short; // NOLINT
typedef _Atomic (unsigned short) atomic_ushort; // NOLINT
typedef _Atomic (int) atomic_int;
typedef _Atomic (unsigned int) atomic_uint;
typedef _Atomic (long) atomic_long; // NOLINT
typedef _Atomic (unsigned long) atomic_ulong; // NOLINT
typedef _Atomic (long long) atomic_llong; // NOLINT
typedef _Atomic (unsigned long long) atomic_ullong; // NOLINT
#if 0
typedef _Atomic (char16_t) atomic_char16_t;
typedef _Atomic (char32_t) atomic_char32_t;
typedef _Atomic (wchar_t) atomic_wchar_t;
typedef _Atomic (int_least8_t) atomic_int_least8_t;
typedef _Atomic (uint_least8_t) atomic_uint_least8_t;
#endif
typedef _Atomic (int_least16_t) atomic_int_least16_t;
typedef _Atomic (uint_least16_t) atomic_uint_least16_t;
typedef _Atomic (int_least32_t) atomic_int_least32_t;
typedef _Atomic (uint_least32_t) atomic_uint_least32_t;
typedef _Atomic (int_least64_t) atomic_int_least64_t;
typedef _Atomic (uint_least64_t) atomic_uint_least64_t;
#if 0
typedef _Atomic (int_fast8_t) atomic_int_fast8_t;
typedef _Atomic (uint_fast8_t) atomic_uint_fast8_t;
#endif
typedef _Atomic (int_fast16_t) atomic_int_fast16_t;
typedef _Atomic (uint_fast16_t) atomic_uint_fast16_t;
typedef _Atomic (int_fast32_t) atomic_int_fast32_t;
typedef _Atomic (uint_fast32_t) atomic_uint_fast32_t;
typedef _Atomic (int_fast64_t) atomic_int_fast64_t;
typedef _Atomic (uint_fast64_t) atomic_uint_fast64_t;
typedef _Atomic (intptr_t) atomic_intptr_t;
typedef _Atomic (uintptr_t) atomic_uintptr_t;
typedef _Atomic (size_t) atomic_size_t;
typedef _Atomic (ptrdiff_t) atomic_ptrdiff_t;
typedef _Atomic (intmax_t) atomic_intmax_t;
typedef _Atomic (uintmax_t) atomic_uintmax_t;
/*
* 7.17.7 Operations on atomic types. (pruned modified for Windows' crappy C compiler)
*/
#define rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) \
__pragma(warning(push)) \
__pragma(warning(disable: 4244)) \
do { \
switch (sizeof(object)) { \
case sizeof(uint64_t): \
out = _InterlockedCompareExchange64((LONGLONG *) object, desired, *expected); \
break; \
case sizeof(uint32_t): \
out = _InterlockedCompareExchange((LONG *) object, desired, *expected); \
break; \
case sizeof(uint16_t): \
out = _InterlockedCompareExchange16((SHORT *) object, desired, *expected); \
break; \
default: \
break; \
} \
} while (0); \
__pragma(warning(pop))
#define rcl_win32_atomic_compare_exchange_weak(object, out, expected, desired) \
rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired)
#define rcl_win32_atomic_exchange(object, out, desired) \
__pragma(warning(push)) \
__pragma(warning(disable: 4244)) \
do { \
switch (sizeof(object)) { \
case sizeof(uint64_t): \
out = _InterlockedExchange64((LONGLONG *) object, desired); \
break; \
case sizeof(uint32_t): \
out = _InterlockedExchange((LONG *) object, desired); \
break; \
case sizeof(uint16_t): \
out = _InterlockedExchange16((SHORT *) object, desired); \
break; \
default: \
break; \
} \
} while (0); \
__pragma(warning(pop))
#define rcl_win32_atomic_fetch_add(object, out, operand) \
__pragma(warning(push)) \
__pragma(warning(disable: 4244)) \
do { \
switch (sizeof(object)) { \
case sizeof(uint64_t): \
out = _InterlockedExchangeAdd64((LONGLONG *) object, operand); \
break; \
case sizeof(uint32_t): \
out = _InterlockedExchangeAdd((LONG *) object, operand); \
break; \
case sizeof(uint16_t): \
out = _InterlockedExchangeAdd16((SHORT *) object, operand); \
break; \
default: \
break; \
} \
} while (0); \
__pragma(warning(pop))
#define rcl_win32_atomic_fetch_and(object, out, operand) \
__pragma(warning(push)) \
__pragma(warning(disable: 4244)) \
do { \
switch (sizeof(object)) { \
case sizeof(uint64_t): \
out = _InterlockedAnd64((LONGLONG *) object, operand); \
break; \
case sizeof(uint32_t): \
out = _InterlockedAnd((LONG *) object, operand); \
break; \
case sizeof(uint16_t): \
out = _InterlockedAnd16((SHORT *) object, operand); \
break; \
default: \
break; \
} \
} while (0); \
__pragma(warning(pop))
#define rcl_win32_atomic_fetch_or(object, out, operand) \
__pragma(warning(push)) \
__pragma(warning(disable: 4244)) \
do { \
switch (sizeof(object)) { \
case sizeof(uint64_t): \
out = _InterlockedOr64((LONGLONG *) object, operand); \
break; \
case sizeof(uint32_t): \
out = _InterlockedOr((LONG *) object, operand); \
break; \
case sizeof(uint16_t): \
out = _InterlockedOr16((SHORT *) object, operand); \
break; \
default: \
break; \
} \
} while (0); \
__pragma(warning(pop))
#define rcl_win32_atomic_fetch_sub(object, out, operand) \
rcl_win32_atomic_fetch_add(object, out, -(operand))
#define rcl_win32_atomic_fetch_xor(object, out, operand) \
__pragma(warning(push)) \
__pragma(warning(disable: 4244)) \
do { \
switch (sizeof(object)) { \
case sizeof(uint64_t): \
out = _InterlockedXor64((LONGLONG *) object, operand); \
break; \
case sizeof(uint32_t): \
out = _InterlockedXor((LONG *) object, operand); \
break; \
case sizeof(uint16_t): \
out = _InterlockedXor16((SHORT *) object, operand); \
break; \
default: \
break; \
} \
} while (0); \
__pragma(warning(pop))
#define rcl_win32_atomic_load(object, out) \
__pragma(warning(push)) \
__pragma(warning(disable: 4244)) \
do { \
switch (sizeof(object)) { \
case sizeof(uint64_t): \
out = _InterlockedExchangeAdd64((LONGLONG *) object, 0); \
break; \
case sizeof(uint32_t): \
out = _InterlockedExchangeAdd((LONG *) object, 0); \
break; \
case sizeof(uint16_t): \
out = _InterlockedExchangeAdd16((SHORT *) object, 0); \
break; \
default: \
break; \
} \
} while (0); \
__pragma(warning(pop))
#define rcl_win32_atomic_store(object, desired) \
do { \
MemoryBarrier(); \
(object)->__val = (desired); \
MemoryBarrier(); \
} while (0)
/*
* 7.17.8 Atomic flag type and operations. (disabled for now)
*/
// typedef atomic_bool atomic_flag;
// #define ATOMIC_FLAG_INIT ATOMIC_VAR_INIT(0)
// #define atomic_flag_clear_explicit(object, order) \
// atomic_store_explicit(object, 0, order)
// #define atomic_flag_test_and_set_explicit(object, order) \
// atomic_compare_exchange_strong_explicit(object, 0, 1, order, order)
// #define atomic_flag_clear(object) \
// atomic_flag_clear_explicit(object, memory_order_seq_cst)
// #define atomic_flag_test_and_set(object) \
// atomic_flag_test_and_set_explicit(object, memory_order_seq_cst)
#endif // RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_

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

@ -0,0 +1,180 @@
// 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"
#include "rmw/rmw.h"
#include "./common.h"
typedef struct rcl_subscription_impl_t
{
rcl_subscription_options_t options;
rmw_subscription_t * rmw_handle;
} rcl_subscription_impl_t;
rcl_subscription_t
rcl_get_zero_initialized_subscription()
{
static rcl_subscription_t null_subscription = {0};
return null_subscription;
}
rcl_ret_t
rcl_subscription_init(
rcl_subscription_t * subscription,
const rcl_node_t * node,
const rosidl_message_type_support_t * type_support,
const char * topic_name,
const rcl_subscription_options_t * options)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, 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 (subscription->impl) {
RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized");
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 memory for the implementation struct.
subscription->impl = (rcl_subscription_impl_t *)allocator->allocate(
sizeof(rcl_subscription_impl_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
subscription->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
// Fill out the implemenation struct.
// rmw_handle
// TODO(wjwwood): pass allocator once supported in rmw api.
subscription->impl->rmw_handle = rmw_create_subscription(
rcl_node_get_rmw_handle(node),
type_support,
topic_name,
&rmw_qos_profile_default,
options->ignore_local_publications);
if (!subscription->impl->rmw_handle) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
goto fail;
}
// options
subscription->impl->options = *options;
return RCL_RET_OK;
fail:
if (subscription->impl) {
allocator->deallocate(subscription->impl, allocator->state);
}
return fail_ret;
}
rcl_ret_t
rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node)
{
rcl_ret_t result = RCL_RET_OK;
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
if (subscription->impl) {
rmw_ret_t ret =
rmw_destroy_subscription(rcl_node_get_rmw_handle(node), subscription->impl->rmw_handle);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
result = RCL_RET_ERROR;
}
rcl_allocator_t allocator = subscription->impl->options.allocator;
allocator.deallocate(subscription->impl, allocator.state);
}
return result;
}
rcl_subscription_options_t
rcl_subscription_get_default_options()
{
static rcl_subscription_options_t default_options = {
.ignore_local_publications = false,
};
// Must set the allocator and qos after because they are not a compile time constant.
default_options.qos = rmw_qos_profile_default;
default_options.allocator = rcl_get_default_allocator();
return default_options;
}
rcl_ret_t
rcl_take(
const rcl_subscription_t * subscription,
void * ros_message,
bool * taken,
rmw_message_info_t * message_info)
{
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(taken, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(
subscription->impl, "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID);
RCL_CHECK_FOR_NULL_WITH_MSG(
subscription->impl->rmw_handle,
"subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID);
// If message_info is NULL, use a place holder which can be discarded.
rmw_message_info_t dummy_message_info;
rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
// Call rmw_take_with_info.
rmw_ret_t ret =
rmw_take_with_info(subscription->impl->rmw_handle, ros_message, taken, message_info_local);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
return RCL_RET_ERROR;
}
return RCL_RET_OK;
}
const char *
rcl_subscription_get_topic_name(const rcl_subscription_t * subscription)
{
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
subscription->impl, "subscription is invalid", return NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
subscription->impl->rmw_handle,
"subscription is invalid", return NULL);
return subscription->impl->rmw_handle->topic_name;
}
const rcl_subscription_options_t *
rcl_subscription_get_options(const rcl_subscription_t * subscription)
{
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
subscription->impl, "subscription is invalid", return NULL);
return &subscription->impl->options;
}
rmw_subscription_t *
rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription)
{
RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(
subscription->impl, "subscription is invalid", return NULL);
return subscription->impl->rmw_handle;
}
#if __cplusplus
}
#endif

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

@ -0,0 +1,19 @@
// 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 defined(WIN32)
#include "./time_win32.c"
#else // defined(WIN32)
#include "./time_unix.c"
#endif // defined(WIN32)

106
rcl/src/rcl/time_unix.c Normal file
View file

@ -0,0 +1,106 @@
// 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 defined(WIN32)
# error time_unix.c is not intended to be used with win32 based systems
#endif // defined(WIN32)
#if __cplusplus
extern "C"
{
#endif
#include "rcl/time.h"
#if defined(__MACH__)
#include <mach/clock.h>
#include <mach/mach.h>
#endif // defined(__MACH__)
#include <math.h>
#include <time.h>
#include <unistd.h>
#include "./common.h"
#include "rcl/error_handling.h"
#if !defined(__MACH__) // Assume clock_get_time is available on OS X.
// This id an appropriate check for clock_gettime() according to:
// http://man7.org/linux/man-pages/man2/clock_gettime.2.html
# if !defined(_POSIX_TIMERS) || !_POSIX_TIMERS
# error no monotonic clock function available
# endif // !defined(_POSIX_TIMERS) || !_POSIX_TIMERS
#endif // !defined(__MACH__)
#define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0))
rcl_ret_t
rcl_system_time_point_now(rcl_system_time_point_t * now)
{
RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
struct timespec timespec_now;
#if defined(__MACH__)
// On OS X use clock_get_time.
clock_serv_t cclock;
mach_timespec_t mts;
host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
clock_get_time(cclock, &mts);
mach_port_deallocate(mach_task_self(), cclock);
timespec_now.tv_sec = mts.tv_sec;
timespec_now.tv_nsec = mts.tv_nsec;
#else // defined(__MACH__)
// Otherwise use clock_gettime.
clock_gettime(CLOCK_REALTIME, &timespec_now);
#endif // defined(__MACH__)
if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) {
RCL_SET_ERROR_MSG("unexpected negative time");
return RCL_RET_ERROR;
}
now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec;
return RCL_RET_OK;
}
rcl_ret_t
rcl_steady_time_point_now(rcl_steady_time_point_t * now)
{
RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
// If clock_gettime is available or on OS X, use a timespec.
struct timespec timespec_now;
#if defined(__MACH__)
// On OS X use clock_get_time.
clock_serv_t cclock;
mach_timespec_t mts;
host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
clock_get_time(cclock, &mts);
mach_port_deallocate(mach_task_self(), cclock);
timespec_now.tv_sec = mts.tv_sec;
timespec_now.tv_nsec = mts.tv_nsec;
#else // defined(__MACH__)
// Otherwise use clock_gettime.
#if defined(CLOCK_MONOTONIC_RAW)
clock_gettime(CLOCK_MONOTONIC_RAW, &timespec_now);
#else // defined(CLOCK_MONOTONIC_RAW)
clock_gettime(CLOCK_MONOTONIC, &timespec_now);
#endif // defined(CLOCK_MONOTONIC_RAW)
#endif // defined(__MACH__)
if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) {
RCL_SET_ERROR_MSG("unexpected negative time");
return RCL_RET_ERROR;
}
now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec;
return RCL_RET_OK;
}
#if __cplusplus
}
#endif

69
rcl/src/rcl/time_win32.c Normal file
View file

@ -0,0 +1,69 @@
// 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 WIN32
# error time_win32.c is only intended to be used with win32 based systems
#endif
#if __cplusplus
extern "C"
{
#endif
#include "rcl/time.h"
#include <windows.h>
#include "./common.h"
#include "./stdatomic_helper.h"
#include "rcl/error_handling.h"
rcl_ret_t
rcl_system_time_point_now(rcl_system_time_point_t * now)
{
RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
FILETIME ft;
GetSystemTimeAsFileTime(&ft);
ULARGE_INTEGER uli;
uli.LowPart = ft.dwLowDateTime;
uli.HighPart = ft.dwHighDateTime;
// Adjust for January 1st, 1970, see:
// https://support.microsoft.com/en-us/kb/167296
uli.QuadPart -= 116444736000000000;
// Convert to nanoseconds from 100's of nanoseconds.
now->nanoseconds = uli.QuadPart * 100;
return RCL_RET_OK;
}
rcl_ret_t
rcl_steady_time_point_now(rcl_steady_time_point_t * now)
{
RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
LARGE_INTEGER cpu_frequency, performance_count;
// These should not ever fail since XP is already end of life:
// From https://msdn.microsoft.com/en-us/library/windows/desktop/ms644905(v=vs.85).aspx and
// https://msdn.microsoft.com/en-us/library/windows/desktop/ms644904(v=vs.85).aspx:
// "On systems that run Windows XP or later, the function will always succeed and will
// thus never return zero."
QueryPerformanceFrequency(&cpu_frequency);
QueryPerformanceCounter(&performance_count);
// Convert to nanoseconds before converting from ticks to avoid precision loss.
now->nanoseconds = RCL_S_TO_NS(performance_count.QuadPart);
now->nanoseconds /= cpu_frequency.QuadPart;
return RCL_RET_OK;
}
#if __cplusplus
}
#endif

239
rcl/src/rcl/timer.c Normal file
View file

@ -0,0 +1,239 @@
// 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/timer.h"
#include "./common.h"
#include "./stdatomic_helper.h"
typedef struct rcl_timer_impl_t
{
// The user supplied callback.
atomic_uintptr_t callback;
// This is a duration in nanoseconds.
atomic_uint_least64_t period;
// This is a time in nanoseconds since an unspecified time.
atomic_uint_least64_t last_call_time;
// A flag which indicates if the timer is canceled.
atomic_bool canceled;
// The user supplied allocator.
rcl_allocator_t allocator;
} rcl_timer_impl_t;
rcl_timer_t
rcl_get_zero_initialized_timer()
{
static rcl_timer_t null_timer = {0};
return null_timer;
}
rcl_ret_t
rcl_timer_init(
rcl_timer_t * timer,
uint64_t period,
const rcl_timer_callback_t callback,
rcl_allocator_t allocator)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT);
if (timer->impl) {
RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized");
return RCL_RET_ALREADY_INIT;
}
rcl_steady_time_point_t now_steady;
rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady);
if (now_ret != RCL_RET_OK) {
return now_ret; // rcl error state should already be set.
}
rcl_timer_impl_t impl = {
.callback = ATOMIC_VAR_INIT((uintptr_t)callback),
.period = ATOMIC_VAR_INIT(period),
.last_call_time = ATOMIC_VAR_INIT(now_steady.nanoseconds),
.canceled = ATOMIC_VAR_INIT(false),
.allocator = 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);
timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
*timer->impl = impl;
return RCL_RET_OK;
}
rcl_ret_t
rcl_timer_fini(rcl_timer_t * timer)
{
if (!timer || !timer->impl) {
return RCL_RET_OK;
}
// Will return either RCL_RET_OK or RCL_RET_ERROR since the timer is valid.
rcl_ret_t result = rcl_timer_cancel(timer);
rcl_allocator_t allocator = timer->impl->allocator;
allocator.deallocate(timer->impl, allocator.state);
return result;
}
rcl_ret_t
rcl_timer_call(rcl_timer_t * timer)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
if (rcl_atomic_load_bool(&timer->impl->canceled)) {
RCL_SET_ERROR_MSG("timer is canceled");
return RCL_RET_TIMER_CANCELED;
}
rcl_steady_time_point_t now_steady;
rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady);
if (now_ret != RCL_RET_OK) {
return now_ret; // rcl error state should already be set.
}
uint64_t previous_ns =
rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady.nanoseconds);
uint64_t since_last_call = now_steady.nanoseconds - previous_ns;
rcl_timer_callback_t typed_callback =
(rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
typed_callback(timer, since_last_call);
return RCL_RET_OK;
}
rcl_ret_t
rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(is_ready, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
int64_t time_until_next_call;
rcl_ret_t ret = rcl_timer_get_time_until_next_call(timer, &time_until_next_call);
if (ret != RCL_RET_OK) {
return ret; // rcl error state should already be set.
}
*is_ready = (time_until_next_call <= 0) && !rcl_atomic_load_bool(&timer->impl->canceled);
return RCL_RET_OK;
}
rcl_ret_t
rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
rcl_steady_time_point_t now;
rcl_ret_t ret = rcl_steady_time_point_now(&now);
if (ret != RCL_RET_OK) {
return ret; // rcl error state should already be set.
}
uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
*time_until_next_call =
(rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now.nanoseconds;
return RCL_RET_OK;
}
rcl_ret_t
rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
rcl_steady_time_point_t now;
rcl_ret_t ret = rcl_steady_time_point_now(&now);
if (ret != RCL_RET_OK) {
return ret; // rcl error state should already be set.
}
*time_since_last_call =
now.nanoseconds - rcl_atomic_load_uint64_t(&timer->impl->last_call_time);
return RCL_RET_OK;
}
rcl_ret_t
rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
*period = rcl_atomic_load_uint64_t(&timer->impl->period);
return RCL_RET_OK;
}
rcl_ret_t
rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
*old_period = rcl_atomic_exchange_uint64_t(&timer->impl->period, new_period);
return RCL_RET_OK;
}
rcl_timer_callback_t
rcl_timer_get_callback(const rcl_timer_t * timer)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
return (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
}
rcl_timer_callback_t
rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
RCL_CHECK_ARGUMENT_FOR_NULL(new_callback, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
return (rcl_timer_callback_t)rcl_atomic_exchange_uintptr_t(
&timer->impl->callback, (uintptr_t)new_callback);
}
rcl_ret_t
rcl_timer_cancel(rcl_timer_t * timer)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
rcl_atomic_store(&timer->impl->canceled, true);
return RCL_RET_OK;
}
rcl_ret_t
rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
*is_canceled = rcl_atomic_load_bool(&timer->impl->canceled);
return RCL_RET_OK;
}
rcl_ret_t
rcl_timer_reset(rcl_timer_t * timer)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
rcl_steady_time_point_t now;
rcl_ret_t now_ret = rcl_steady_time_point_now(&now);
if (now_ret != RCL_RET_OK) {
return now_ret; // rcl error state should already be set.
}
rcl_atomic_store(&timer->impl->last_call_time, now.nanoseconds);
rcl_atomic_store(&timer->impl->canceled, false);
return RCL_RET_OK;
}
#if __cplusplus
}
#endif

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

@ -0,0 +1,457 @@
// 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"
#include <assert.h>
#include <stdbool.h>
#include <string.h>
#include "./common.h"
#include "./stdatomic_helper.h"
#include "rcl/error_handling.h"
#include "rcl/time.h"
#include "rmw/rmw.h"
typedef struct rcl_wait_set_impl_t
{
size_t subscription_index;
rmw_subscriptions_t rmw_subscriptions;
size_t guard_condition_index;
rmw_guard_conditions_t rmw_guard_conditions;
size_t timer_index;
rcl_allocator_t allocator;
} rcl_wait_set_impl_t;
rcl_wait_set_t
rcl_get_zero_initialized_wait_set()
{
static rcl_wait_set_t null_wait_set = {
.subscriptions = NULL,
.size_of_subscriptions = 0,
.guard_conditions = NULL,
.size_of_guard_conditions = 0,
.timers = NULL,
.size_of_timers = 0,
.impl = NULL,
};
return null_wait_set;
}
static bool
__wait_set_is_valid(const rcl_wait_set_t * wait_set)
{
return wait_set && wait_set->impl;
}
static void
__wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator)
{
rcl_ret_t ret;
if (wait_set->subscriptions) {
ret = rcl_wait_set_resize_subscriptions(wait_set, 0);
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->guard_conditions) {
ret = rcl_wait_set_resize_guard_conditions(wait_set, 0);
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->timers) {
ret = rcl_wait_set_resize_timers(wait_set, 0);
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
}
if (wait_set->impl) {
allocator.deallocate(wait_set->impl, allocator.state);
wait_set->impl = NULL;
}
}
rcl_ret_t
rcl_wait_set_init(
rcl_wait_set_t * wait_set,
size_t number_of_subscriptions,
size_t number_of_guard_conditions,
size_t number_of_timers,
rcl_allocator_t allocator)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
if (__wait_set_is_valid(wait_set)) {
RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized.");
return RCL_RET_ALREADY_INIT;
}
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);
RCL_CHECK_FOR_NULL_WITH_MSG(
allocator.reallocate, "reallocate not set", return RCL_RET_INVALID_ARGUMENT);
// Allocate space for the implementation struct.
wait_set->impl = (rcl_wait_set_impl_t *)allocator.allocate(
sizeof(rcl_wait_set_impl_t), allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG(
wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
wait_set->impl->rmw_subscriptions.subscribers = NULL;
wait_set->impl->rmw_subscriptions.subscriber_count = 0;
wait_set->impl->rmw_guard_conditions.guard_conditions = NULL;
wait_set->impl->rmw_guard_conditions.guard_condition_count = 0;
// Initialize subscription space.
rcl_ret_t ret;
if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_subscriptions(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Initialize guard condition space.
ret = rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions);
if (ret != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_guard_conditions(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Initialize timer space.
ret = rcl_wait_set_resize_timers(wait_set, number_of_timers);
if (ret != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
if ((ret = rcl_wait_set_clear_timers(wait_set)) != RCL_RET_OK) {
fail_ret = ret;
goto fail;
}
// Set allocator.
wait_set->impl->allocator = allocator;
return RCL_RET_OK;
fail:
__wait_set_clean_up(wait_set, allocator);
return fail_ret;
}
rcl_ret_t
rcl_wait_set_fini(rcl_wait_set_t * wait_set)
{
rcl_ret_t result = RCL_RET_OK;
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
if (__wait_set_is_valid(wait_set)) {
__wait_set_clean_up(wait_set, wait_set->impl->allocator);
}
return result;
}
rcl_ret_t
rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator)
{
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT);
if (!__wait_set_is_valid(wait_set)) {
RCL_SET_ERROR_MSG("wait set is invalid");
return RCL_RET_WAIT_SET_INVALID;
}
*allocator = wait_set->impl->allocator;
return RCL_RET_OK;
}
#define SET_ADD(Type) \
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
RCL_CHECK_ARGUMENT_FOR_NULL(Type, RCL_RET_INVALID_ARGUMENT); \
if (!__wait_set_is_valid(wait_set)) { \
RCL_SET_ERROR_MSG("wait set is invalid"); \
return RCL_RET_WAIT_SET_INVALID; \
} \
if (!(wait_set->impl->Type ## _index < wait_set->size_of_ ## Type ## s)) { \
RCL_SET_ERROR_MSG(#Type "s set is full"); \
return RCL_RET_WAIT_SET_FULL; \
} \
size_t current_index = wait_set->impl->Type ## _index++; \
wait_set->Type ## s[current_index] = Type;
#define SET_ADD_RMW(Type, RMWStorage) \
/* Also place into rmw storage. */ \
rmw_ ## Type ## _t * rmw_handle = rcl_ ## Type ## _get_rmw_handle(Type); \
RCL_CHECK_FOR_NULL_WITH_MSG( \
rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR); \
wait_set->impl->RMWStorage[current_index] = rmw_handle->data;
#define SET_CLEAR(Type) \
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
if (!__wait_set_is_valid(wait_set)) { \
RCL_SET_ERROR_MSG("wait set is invalid"); \
return RCL_RET_WAIT_SET_INVALID; \
} \
memset( \
(void *)wait_set->Type ## s, \
0, \
sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \
wait_set->impl->Type ## _index = 0; \
#define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \
/* Also clear the rmw storage. */ \
memset( \
wait_set->impl->RMWStorage, \
0, \
sizeof(rmw_subscription_t *) * wait_set->impl->RMWCount);
#define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
RCL_CHECK_FOR_NULL_WITH_MSG( \
wait_set->impl, "wait set is invalid", return RCL_RET_WAIT_SET_INVALID); \
if (size == wait_set->size_of_ ## Type ## s) { \
return RCL_RET_OK; \
} \
rcl_allocator_t allocator = wait_set->impl->allocator; \
if (size == 0) { \
if (wait_set->Type ## s) { \
allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \
wait_set->Type ## s = NULL; \
} \
ExtraDealloc \
} else { \
wait_set->size_of_ ## Type ## s = 0; \
wait_set->Type ## s = (const rcl_ ## Type ## _t * *)allocator.reallocate( \
(void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \
RCL_CHECK_FOR_NULL_WITH_MSG( \
wait_set->Type ## s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \
wait_set->size_of_ ## Type ## s = size; \
ExtraRealloc \
} \
return RCL_RET_OK;
#define SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount) \
/* Also deallocate the rmw storage. */ \
if (wait_set->impl->RMWStorage) { \
allocator.deallocate((void *)wait_set->impl->RMWStorage, allocator.state); \
wait_set->impl->RMWStorage = NULL; \
}
#define SET_RESIZE_RMW_REALLOC(Type, RMWStorage, RMWCount) \
/* Also resize the rmw storage. */ \
wait_set->impl->RMWCount = 0; \
wait_set->impl->RMWStorage = (void **)allocator.reallocate( \
wait_set->impl->RMWStorage, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \
if (!wait_set->impl->RMWStorage) { \
allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \
wait_set->size_of_ ## Type ## s = 0; \
RCL_SET_ERROR_MSG("allocating memory failed"); \
return RCL_RET_BAD_ALLOC; \
} \
wait_set->impl->RMWCount = size;
rcl_ret_t
rcl_wait_set_add_subscription(
rcl_wait_set_t * wait_set,
const rcl_subscription_t * subscription)
{
SET_ADD(subscription)
SET_ADD_RMW(subscription, rmw_subscriptions.subscribers)
return RCL_RET_OK;
}
rcl_ret_t
rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set)
{
SET_CLEAR(subscription)
SET_CLEAR_RMW(
subscription,
rmw_subscriptions.subscribers,
rmw_subscriptions.subscriber_count)
return RCL_RET_OK;
}
rcl_ret_t
rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size)
{
SET_RESIZE(
subscription,
SET_RESIZE_RMW_DEALLOC(
rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count),
SET_RESIZE_RMW_REALLOC(
subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count)
)
}
rcl_ret_t
rcl_wait_set_add_guard_condition(
rcl_wait_set_t * wait_set,
const rcl_guard_condition_t * guard_condition)
{
SET_ADD(guard_condition)
SET_ADD_RMW(guard_condition, rmw_guard_conditions.guard_conditions)
return RCL_RET_OK;
}
rcl_ret_t
rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set)
{
SET_CLEAR(guard_condition)
SET_CLEAR_RMW(
guard_condition,
rmw_guard_conditions.guard_conditions,
rmw_guard_conditions.guard_condition_count)
return RCL_RET_OK;
}
rcl_ret_t
rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size)
{
SET_RESIZE(
guard_condition,
SET_RESIZE_RMW_DEALLOC(
rmw_guard_conditions.guard_conditions, rmw_guard_conditions.guard_condition_count),
SET_RESIZE_RMW_REALLOC(
guard_condition,
rmw_guard_conditions.guard_conditions,
rmw_guard_conditions.guard_condition_count)
)
}
rcl_ret_t
rcl_wait_set_add_timer(
rcl_wait_set_t * wait_set,
const rcl_timer_t * timer)
{
SET_ADD(timer)
return RCL_RET_OK;
}
rcl_ret_t
rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set)
{
SET_CLEAR(timer)
return RCL_RET_OK;
}
rcl_ret_t
rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size)
{
SET_RESIZE(timer,;,;) // NOLINT
}
rcl_ret_t
rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
{
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(timeout, RCL_RET_INVALID_ARGUMENT);
if (!__wait_set_is_valid(wait_set)) {
RCL_SET_ERROR_MSG("wait set is invalid");
return RCL_RET_WAIT_SET_INVALID;
}
if (wait_set->size_of_subscriptions == 0 && wait_set->size_of_guard_conditions == 0) {
RCL_SET_ERROR_MSG("wait set is empty");
return RCL_RET_WAIT_SET_EMPTY;
}
// Create dummy sets for currently unsupported wait-ables.
static rmw_services_t dummy_services = {0, NULL};
static rmw_clients_t dummy_clients = {0, NULL};
// Calculate the timeout argument.
rmw_time_t * timeout_argument;
rmw_time_t temporary_timeout_storage;
if (timeout < 0) {
// Pass NULL to wait to indicate block indefinitely.
timeout_argument = NULL;
}
if (timeout > 0) {
// Determine the nearest timeout (given or a timer).
uint64_t min_timeout = timeout;
if (min_timeout > 0) { // Do not consider timer timeouts if non-blocking.
size_t i;
for (i = 0; i < wait_set->size_of_timers; ++i) {
if (!wait_set->timers[i]) {
continue; // Skip NULL timers.
}
int64_t timer_timeout;
rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout);
if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set.
}
if ((uint64_t)timer_timeout < min_timeout) {
min_timeout = timer_timeout;
}
}
}
// Set that in the temporary storage and point to that for the argument.
temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout);
temporary_timeout_storage.nsec = min_timeout % 1000000000;
timeout_argument = &temporary_timeout_storage;
}
if (timeout == 0) {
// Then it is non-blocking, so set the temporary storage to 0, 0 and pass it.
temporary_timeout_storage.sec = 0;
temporary_timeout_storage.nsec = 0;
timeout_argument = &temporary_timeout_storage;
}
// Wait.
rmw_ret_t ret = rmw_wait(
&wait_set->impl->rmw_subscriptions,
&wait_set->impl->rmw_guard_conditions,
&dummy_services,
&dummy_clients,
timeout_argument);
// Check for error.
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
return RCL_RET_ERROR;
}
// Check for ready timers next, and set not ready timers to NULL.
size_t i;
for (i = 0; i < wait_set->size_of_timers; ++i) {
bool is_ready = false;
rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready);
if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set.
}
if (!is_ready) {
wait_set->timers[i] = NULL;
}
}
// Check for timeout.
if (ret == RMW_RET_TIMEOUT) {
// Assume none were set (because timeout was reached first), and clear all.
rcl_ret_t rcl_ret;
rcl_ret = rcl_wait_set_clear_subscriptions(wait_set);
assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set.
rcl_ret = rcl_wait_set_clear_guard_conditions(wait_set);
assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set.
return RCL_RET_TIMEOUT;
}
// Set corresponding rcl subscription handles NULL.
for (i = 0; i < wait_set->size_of_subscriptions; ++i) {
assert(i < wait_set->impl->rmw_subscriptions.subscriber_count); // Defensive.
if (!wait_set->impl->rmw_subscriptions.subscribers[i]) {
wait_set->subscriptions[i] = NULL;
}
}
// Set corresponding rcl guard_condition handles NULL.
for (i = 0; i < wait_set->size_of_guard_conditions; ++i) {
assert(i < wait_set->impl->rmw_guard_conditions.guard_condition_count); // Defensive.
if (!wait_set->impl->rmw_guard_conditions.guard_conditions[i]) {
wait_set->guard_conditions[i] = NULL;
}
}
return RCL_RET_OK;
}
#if __cplusplus
}
#endif

106
rcl/test/CMakeLists.txt Normal file
View file

@ -0,0 +1,106 @@
find_package(ament_cmake_gtest REQUIRED)
set(extra_test_libraries)
set(extra_memory_tools_env PHONY=) # Use a phony env var so there is always at least one.
ament_find_gtest() # For GTEST_LIBRARIES
if(APPLE)
add_library(${PROJECT_NAME}_memory_tools_interpose SHARED memory_tools_osx_interpose.cpp)
target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES})
set_target_properties(${PROJECT_NAME}_memory_tools_interpose
PROPERTIES COMPILE_FLAGS "-std=c++11")
list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose)
list(APPEND extra_memory_tools_env
DYLD_INSERT_LIBRARIES=$<TARGET_FILE:${PROJECT_NAME}_memory_tools_interpose>)
endif()
add_library(${PROJECT_NAME}_memory_tools SHARED memory_tools.cpp)
if(NOT WIN32)
set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
if(UNIX AND NOT APPLE)
list(APPEND extra_test_libraries dl)
list(APPEND extra_memory_tools_env DL_PRELOAD=$<TARGET_FILE:${PROJECT_NAME}_memory_tools>)
endif()
target_link_libraries(${PROJECT_NAME}_memory_tools ${extra_test_libraries})
target_compile_definitions(${PROJECT_NAME}_memory_tools
PRIVATE "RCL_MEMORY_TOOLS_BUILDING_DLL")
list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools)
if(NOT WIN32)
ament_add_gtest(test_memory_tools test_memory_tools.cpp ENV ${extra_memory_tools_env})
if(TARGET test_memory_tools)
target_include_directories(test_memory_tools PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
)
if(NOT WIN32)
set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries})
endif()
endif()
ament_add_gtest(test_allocator rcl/test_allocator.cpp ENV ${extra_memory_tools_env})
if(TARGET test_allocator)
target_include_directories(test_allocator PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
)
if(NOT WIN32)
set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries})
endif()
ament_add_gtest(test_time rcl/test_time.cpp ENV ${extra_memory_tools_env})
if(TARGET test_time)
target_include_directories(test_time PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
)
if(NOT WIN32)
set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries})
endif()
ament_add_gtest(test_common
rcl/test_common.cpp
ENV
${extra_memory_tools_env}
EMPTY_TEST=
NORMAL_TEST=foo
)
if(TARGET test_common)
target_include_directories(test_common PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
)
if(NOT WIN32)
set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries})
endif()
ament_add_gtest(test_rcl rcl/test_rcl.cpp ENV ${extra_memory_tools_env})
if(TARGET test_rcl)
target_include_directories(test_rcl PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
)
if(NOT WIN32)
set_target_properties(test_rcl PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
target_link_libraries(test_rcl ${PROJECT_NAME} ${extra_test_libraries})
endif()
ament_add_gtest(test_node rcl/test_node.cpp ENV ${extra_memory_tools_env})
if(TARGET test_node)
target_include_directories(test_node PUBLIC
${rcl_interfaces_INCLUDE_DIRS}
${rmw_INCLUDE_DIRS}
)
if(NOT WIN32)
set_target_properties(test_node PROPERTIES COMPILE_FLAGS "-std=c++11")
endif()
target_link_libraries(test_node ${PROJECT_NAME} ${extra_test_libraries})
endif()

141
rcl/test/memory_tools.cpp Normal file
View file

@ -0,0 +1,141 @@
// 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 defined(__linux__)
/******************************************************************************
* Begin Linux
*****************************************************************************/
#include <dlfcn.h>
#include <stdio.h>
#include <malloc.h>
#include "./memory_tools_common.cpp"
void *
malloc(size_t size)
{
void * (* libc_malloc)(size_t) = (void *(*)(size_t))dlsym(RTLD_NEXT, "malloc");
if (enabled.load()) {
return custom_malloc(size);
}
return libc_malloc(size);
}
void *
realloc(void * pointer, size_t size)
{
void * (* libc_realloc)(void *, size_t) = (void *(*)(void *, size_t))dlsym(RTLD_NEXT, "realloc");
if (enabled.load()) {
return custom_realloc(pointer, size);
}
return libc_realloc(pointer, size);
}
void
free(void * pointer)
{
void (* libc_free)(void *) = (void (*)(void *))dlsym(RTLD_NEXT, "free");
if (enabled.load()) {
return custom_free(pointer);
}
return libc_free(pointer);
}
void start_memory_checking()
{
printf("starting memory checking...\n");
enabled.store(true);
}
void stop_memory_checking()
{
printf("stopping memory checking...\n");
enabled.store(false);
}
/******************************************************************************
* End Linux
*****************************************************************************/
#elif defined(__APPLE__)
/******************************************************************************
* Begin Apple
*****************************************************************************/
// The apple implementation is in a separate shared library, loaded with DYLD_INSERT_LIBRARIES.
// Therefore we do not include the common cpp file here.
void osx_start_memory_checking();
void osx_stop_memory_checking();
void start_memory_checking()
{
return osx_start_memory_checking();
}
void stop_memory_checking()
{
return osx_stop_memory_checking();
}
/******************************************************************************
* End Apple
*****************************************************************************/
// #elif defined(WIN32)
/******************************************************************************
* Begin Windows
*****************************************************************************/
// TODO(wjwwood): install custom malloc (and others) for Windows.
/******************************************************************************
* End Windows
*****************************************************************************/
#else
// Default case: do nothing.
#include "./memory_tools.hpp"
#include <stdio.h>
void start_memory_checking()
{
printf("starting memory checking... not available\n");
}
void stop_memory_checking()
{
printf("stopping memory checking... not available\n");
}
void assert_no_malloc_begin() {}
void assert_no_malloc_end() {}
void set_on_unexpected_malloc_callback(UnexpectedCallbackType callback) {}
void assert_no_realloc_begin() {}
void assert_no_realloc_end() {}
void set_on_unexpected_realloc_callback(UnexpectedCallbackType callback) {}
void assert_no_free_begin() {}
void assert_no_free_end() {}
void set_on_unexpected_free_callback(UnexpectedCallbackType callback) {}
void memory_checking_thread_init() {}
#endif // if defined(__linux__) elif defined(__APPLE__) elif defined(WIN32) else ...

107
rcl/test/memory_tools.hpp Normal file
View file

@ -0,0 +1,107 @@
// 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.
// Code to do replacing of malloc/free/etc... inspired by:
// https://dxr.mozilla.org/mozilla-central/rev/
// cc9c6cd756cb744596ba039dcc5ad3065a7cc3ea/memory/build/replace_malloc.c
#ifndef MEMORY_TOOLS_HPP_
#define MEMORY_TOOLS_HPP_
#include <stddef.h>
#include <functional>
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define RCL_MEMORY_TOOLS_EXPORT __attribute__ ((dllexport))
#define RCL_MEMORY_TOOLS_IMPORT __attribute__ ((dllimport))
#else
#define RCL_MEMORY_TOOLS_EXPORT __declspec(dllexport)
#define RCL_MEMORY_TOOLS_IMPORT __declspec(dllimport)
#endif
#ifdef RCL_MEMORY_TOOLS_BUILDING_DLL
#define RCL_MEMORY_TOOLS_PUBLIC RCL_MEMORY_TOOLS_EXPORT
#else
#define RCL_MEMORY_TOOLS_PUBLIC RCL_MEMORY_TOOLS_IMPORT
#endif
#define RCL_MEMORY_TOOLS_PUBLIC_TYPE RCL_MEMORY_TOOLS_PUBLIC
#define RCL_MEMORY_TOOLS_LOCAL
#else
#define RCL_MEMORY_TOOLS_EXPORT __attribute__ ((visibility("default")))
#define RCL_MEMORY_TOOLS_IMPORT
#if __GNUC__ >= 4
#define RCL_MEMORY_TOOLS_PUBLIC __attribute__ ((visibility("default")))
#define RCL_MEMORY_TOOLS_LOCAL __attribute__ ((visibility("hidden")))
#else
#define RCL_MEMORY_TOOLS_PUBLIC
#define RCL_MEMORY_TOOLS_LOCAL
#endif
#define RCL_MEMORY_TOOLS_PUBLIC_TYPE
#endif
typedef std::function<void ()> UnexpectedCallbackType;
RCL_MEMORY_TOOLS_PUBLIC
void
start_memory_checking();
#define ASSERT_NO_MALLOC(statements) \
assert_no_malloc_begin(); statements; assert_no_malloc_end();
RCL_MEMORY_TOOLS_PUBLIC
void
assert_no_malloc_begin();
RCL_MEMORY_TOOLS_PUBLIC
void
assert_no_malloc_end();
RCL_MEMORY_TOOLS_PUBLIC
void
set_on_unexpected_malloc_callback(UnexpectedCallbackType callback);
#define ASSERT_NO_REALLOC(statements) \
assert_no_realloc_begin(); statements; assert_no_realloc_end();
RCL_MEMORY_TOOLS_PUBLIC
void
assert_no_realloc_begin();
RCL_MEMORY_TOOLS_PUBLIC
void
assert_no_realloc_end();
RCL_MEMORY_TOOLS_PUBLIC
void
set_on_unexpected_realloc_callback(UnexpectedCallbackType callback);
#define ASSERT_NO_FREE(statements) \
assert_no_free_begin(); statements; assert_no_free_end();
RCL_MEMORY_TOOLS_PUBLIC
void
assert_no_free_begin();
RCL_MEMORY_TOOLS_PUBLIC
void
assert_no_free_end();
RCL_MEMORY_TOOLS_PUBLIC
void
set_on_unexpected_free_callback(UnexpectedCallbackType callback);
RCL_MEMORY_TOOLS_PUBLIC
void
stop_memory_checking();
RCL_MEMORY_TOOLS_PUBLIC
void
memory_checking_thread_init();
#endif // MEMORY_TOOLS_HPP_

160
rcl/test/memory_tools_common.cpp Normal file
View file

@ -0,0 +1,160 @@
// 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 <inttypes.h>
#include <atomic>
#include <cstdlib>
#if defined(__APPLE__)
#include <malloc/malloc.h>
#define MALLOC_PRINTF malloc_printf
#else // defined(__APPLE__)
#define MALLOC_PRINTF printf
#endif // defined(__APPLE__)
#include "./memory_tools.hpp"
#include "./scope_exit.hpp"
static std::atomic<bool> enabled(false);
static __thread bool malloc_expected = true;
static __thread UnexpectedCallbackType * unexpected_malloc_callback = nullptr;
void set_on_unexpected_malloc_callback(UnexpectedCallbackType callback)
{
if (unexpected_malloc_callback) {
unexpected_malloc_callback->~UnexpectedCallbackType();
free(unexpected_malloc_callback);
unexpected_malloc_callback = nullptr;
}
if (!callback) {
return;
}
if (!unexpected_malloc_callback) {
unexpected_malloc_callback =
reinterpret_cast<UnexpectedCallbackType *>(malloc(sizeof(UnexpectedCallbackType)));
if (!unexpected_malloc_callback) {
throw std::bad_alloc();
}
new (unexpected_malloc_callback) UnexpectedCallbackType();
}
*unexpected_malloc_callback = callback;
}
void *
custom_malloc(size_t size)
{
if (!enabled.load()) {return malloc(size);}
auto foo = SCOPE_EXIT(enabled.store(true););
enabled.store(false);
if (!malloc_expected) {
if (unexpected_malloc_callback) {
(*unexpected_malloc_callback)();
}
}
void * memory = malloc(size);
uint64_t fw_size = size;
MALLOC_PRINTF(
" malloc (%s) %p %" PRIu64 "\n",
malloc_expected ? " expected" : "not expected", memory, fw_size);
return memory;
}
static __thread bool realloc_expected = true;
static __thread UnexpectedCallbackType * unexpected_realloc_callback = nullptr;
void set_on_unexpected_realloc_callback(UnexpectedCallbackType callback)
{
if (unexpected_realloc_callback) {
unexpected_realloc_callback->~UnexpectedCallbackType();
free(unexpected_realloc_callback);
unexpected_realloc_callback = nullptr;
}
if (!callback) {
return;
}
if (!unexpected_realloc_callback) {
unexpected_realloc_callback =
reinterpret_cast<UnexpectedCallbackType *>(malloc(sizeof(UnexpectedCallbackType)));
if (!unexpected_realloc_callback) {
throw std::bad_alloc();
}
new (unexpected_realloc_callback) UnexpectedCallbackType();
}
*unexpected_realloc_callback = callback;
}
void *
custom_realloc(void * memory_in, size_t size)
{
if (!enabled.load()) {return realloc(memory_in, size);}
auto foo = SCOPE_EXIT(enabled.store(true););
enabled.store(false);
if (!realloc_expected) {
if (unexpected_realloc_callback) {
(*unexpected_realloc_callback)();
}
}
void * memory = realloc(memory_in, size);
uint64_t fw_size = size;
MALLOC_PRINTF(
"realloc (%s) %p %p %" PRIu64 "\n",
malloc_expected ? " expected" : "not expected", memory_in, memory, fw_size);
return memory;
}
static __thread bool free_expected = true;
static __thread UnexpectedCallbackType * unexpected_free_callback = nullptr;
void set_on_unexpected_free_callback(UnexpectedCallbackType callback)
{
if (unexpected_free_callback) {
unexpected_free_callback->~UnexpectedCallbackType();
free(unexpected_free_callback);
unexpected_free_callback = nullptr;
}
if (!callback) {
return;
}
if (!unexpected_free_callback) {
unexpected_free_callback =
reinterpret_cast<UnexpectedCallbackType *>(malloc(sizeof(UnexpectedCallbackType)));
if (!unexpected_free_callback) {
throw std::bad_alloc();
}
new (unexpected_free_callback) UnexpectedCallbackType();
}
*unexpected_free_callback = callback;
}
void
custom_free(void * memory)
{
if (!enabled.load()) {return free(memory);}
auto foo = SCOPE_EXIT(enabled.store(true););
enabled.store(false);
if (!free_expected) {
if (unexpected_free_callback) {
(*unexpected_free_callback)();
}
}
MALLOC_PRINTF(
" free (%s) %p\n", malloc_expected ? " expected" : "not expected", memory);
free(memory);
}
void assert_no_malloc_begin() {malloc_expected = false;}
void assert_no_malloc_end() {malloc_expected = true;}
void assert_no_realloc_begin() {realloc_expected = false;}
void assert_no_realloc_end() {realloc_expected = true;}
void assert_no_free_begin() {free_expected = false;}
void assert_no_free_end() {free_expected = true;}

55
rcl/test/memory_tools_osx_interpose.cpp Normal file
View file

@ -0,0 +1,55 @@
// 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.
// Pulled from:
// https://github.com/emeryberger/Heap-Layers/blob/
// 076e9e7ef53b66380b159e40473b930f25cc353b/wrappers/macinterpose.h
// The interposition data structure (just pairs of function pointers),
// used an interposition table like the following:
//
typedef struct interpose_s
{
void * new_func;
void * orig_func;
} interpose_t;
#define OSX_INTERPOSE(newf, oldf) \
__attribute__((used)) static const interpose_t \
macinterpose ## newf ## oldf __attribute__ ((section("__DATA, __interpose"))) = { \
reinterpret_cast<void *>(newf), \
reinterpret_cast<void *>(oldf), \
}
// End Interpose.
#include "./memory_tools_common.cpp"
void osx_start_memory_checking()
{
// No loading required, it is handled by DYLD_INSERT_LIBRARIES and dynamic library interposing.
MALLOC_PRINTF("starting memory checking...\n");
enabled.store(true);
}
void osx_stop_memory_checking()
{
MALLOC_PRINTF("stopping memory checking...\n");
enabled.store(false);
}
OSX_INTERPOSE(custom_malloc, malloc);
OSX_INTERPOSE(custom_realloc, realloc);
OSX_INTERPOSE(custom_free, free);

84
rcl/test/rcl/test_allocator.cpp Normal file
View file

@ -0,0 +1,84 @@
// 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 <gtest/gtest.h>
#include "rcl/allocator.h"
#include "../memory_tools.hpp"
class TestAllocatorFixture : public::testing::Test
{
public:
TestAllocatorFixture()
{
start_memory_checking();
stop_memory_checking();
}
void SetUp()
{
set_on_unexpected_malloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
void TearDown()
{
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
set_on_unexpected_malloc_callback(nullptr);
set_on_unexpected_realloc_callback(nullptr);
set_on_unexpected_free_callback(nullptr);
}
};
/* Tests the default allocator.
*/
TEST_F(TestAllocatorFixture, test_default_allocator_normal) {
#if defined(WIN32)
printf("Allocator tests disabled on Windows.\n");
return;
#endif // defined(WIN32)
ASSERT_NO_MALLOC(
rcl_allocator_t allocator = rcl_get_default_allocator();
)
size_t mallocs = 0;
size_t reallocs = 0;
size_t frees = 0;
set_on_unexpected_malloc_callback([&mallocs]() {
mallocs++;
});
set_on_unexpected_realloc_callback([&reallocs]() {
reallocs++;
});
set_on_unexpected_free_callback([&frees]() {
frees++;
});
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
void * allocated_memory = allocator.allocate(1024, allocator.state);
EXPECT_EQ(mallocs, 1);
EXPECT_NE(allocated_memory, nullptr);
allocated_memory = allocator.reallocate(allocated_memory, 2048, allocator.state);
EXPECT_EQ(reallocs, 1);
EXPECT_NE(allocated_memory, nullptr);
allocator.deallocate(allocated_memory, allocator.state);
EXPECT_EQ(mallocs, 1);
EXPECT_EQ(reallocs, 1);
EXPECT_EQ(frees, 1);
}

52
rcl/test/rcl/test_common.cpp Normal file
View file

@ -0,0 +1,52 @@
// 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 <gtest/gtest.h>
#include <string>
#include "../../src/rcl/common.h"
#include "../../src/rcl/common.c"
/* Tests the default allocator.
*
* Expected environment variables must be set by the calling code:
*
* - EMPTY_TEST=
* - NORMAL_TEST=foo
*
* These are set in the call to `ament_add_gtest()` in the `CMakeLists.txt`.
*/
TEST(TestCommon, test_getenv) {
const char * env;
rcl_ret_t ret;
ret = rcl_impl_getenv("NORMAL_TEST", NULL);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ret = rcl_impl_getenv(NULL, &env);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ret = rcl_impl_getenv("SHOULD_NOT_EXIST_TEST", &env);
EXPECT_EQ(RCL_RET_OK, ret);
EXPECT_EQ("", std::string(env)) << std::string(env);
rcl_reset_error();
ret = rcl_impl_getenv("NORMAL_TEST", &env);
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_NE(nullptr, env);
EXPECT_EQ("foo", std::string(env));
ret = rcl_impl_getenv("EMPTY_TEST", &env);
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_NE(nullptr, env);
EXPECT_EQ("", std::string(env));
}

309
rcl/test/rcl/test_node.cpp Normal file
View file

@ -0,0 +1,309 @@
// 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 <gtest/gtest.h>
#include <string>
#include "rcl/rcl.h"
#include "rcl/node.h"
#include "../memory_tools.hpp"
#include "../scope_exit.hpp"
#include "rcl/error_handling.h"
class TestNodeFixture : public::testing::Test
{
public:
void SetUp()
{
set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
void TearDown()
{
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
set_on_unexpected_malloc_callback(nullptr);
set_on_unexpected_realloc_callback(nullptr);
set_on_unexpected_free_callback(nullptr);
}
};
void *
failing_malloc(size_t size, void * state)
{
(void)(size);
(void)(state);
return nullptr;
}
void *
failing_realloc(void * pointer, size_t size, void * state)
{
(void)(pointer);
(void)(size);
(void)(state);
return nullptr;
}
void
failing_free(void * pointer, void * state)
{
(void)pointer;
(void)state;
}
/* Tests the node accessors, i.e. rcl_node_get_* functions.
*/
TEST_F(TestNodeFixture, test_rcl_node_accessors) {
stop_memory_checking();
rcl_ret_t ret;
// Initialize rcl with rcl_init().
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret); // Shutdown later after invalid node.
// Create an invalid node (rcl_shutdown).
rcl_node_t invalid_node = rcl_get_zero_initialized_node();
const char * name = "node_name";
rcl_node_options_t default_options = rcl_node_get_default_options();
default_options.domain_id = 42; // Set the domain id to something explicit.
ret = rcl_node_init(&invalid_node, name, &default_options);
ASSERT_EQ(RCL_RET_OK, ret);
auto rcl_invalid_node_exit = make_scope_exit([&invalid_node]() {
stop_memory_checking();
rcl_ret_t ret = rcl_node_fini(&invalid_node);
EXPECT_EQ(RCL_RET_OK, ret);
});
ret = rcl_shutdown(); // Shutdown to invalidate the node.
ASSERT_EQ(RCL_RET_OK, ret);
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret);
auto rcl_shutdown_exit = make_scope_exit([]() {
stop_memory_checking();
rcl_ret_t ret = rcl_shutdown();
ASSERT_EQ(RCL_RET_OK, ret);
});
// Create a zero init node.
rcl_node_t zero_node = rcl_get_zero_initialized_node();
// Create a normal node.
rcl_node_t node = rcl_get_zero_initialized_node();
ret = rcl_node_init(&node, name, &default_options);
ASSERT_EQ(RCL_RET_OK, ret);
auto rcl_node_exit = make_scope_exit([&node]() {
stop_memory_checking();
rcl_ret_t ret = rcl_node_fini(&node);
EXPECT_EQ(RCL_RET_OK, ret);
});
// Test rcl_node_get_name().
const char * actual_node_name;
actual_node_name = rcl_node_get_name(nullptr);
EXPECT_EQ(nullptr, actual_node_name);
rcl_reset_error();
actual_node_name = rcl_node_get_name(&zero_node);
EXPECT_EQ(nullptr, actual_node_name);
rcl_reset_error();
actual_node_name = rcl_node_get_name(&invalid_node);
EXPECT_EQ(nullptr, actual_node_name);
rcl_reset_error();
start_memory_checking();
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
actual_node_name = rcl_node_get_name(&node);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_TRUE(actual_node_name ? true : false);
if (actual_node_name) {
EXPECT_EQ(std::string(name), std::string(actual_node_name));
}
// Test rcl_node_get_options().
const rcl_node_options_t * actual_options;
actual_options = rcl_node_get_options(nullptr);
EXPECT_EQ(nullptr, actual_options);
rcl_reset_error();
actual_options = rcl_node_get_options(&zero_node);
EXPECT_EQ(nullptr, actual_options);
rcl_reset_error();
actual_options = rcl_node_get_options(&invalid_node);
EXPECT_EQ(nullptr, actual_options);
rcl_reset_error();
start_memory_checking();
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
actual_options = rcl_node_get_options(&node);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_NE(nullptr, actual_options);
if (actual_options) {
EXPECT_EQ(default_options.allocator.allocate, actual_options->allocator.allocate);
EXPECT_EQ(default_options.domain_id, actual_options->domain_id);
}
// Test rcl_node_get_domain_id().
size_t actual_domain_id;
ret = rcl_node_get_domain_id(nullptr, &actual_domain_id);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ret = rcl_node_get_domain_id(&zero_node, &actual_domain_id);
EXPECT_EQ(RCL_RET_NODE_INVALID, ret);
rcl_reset_error();
ret = rcl_node_get_domain_id(&invalid_node, &actual_domain_id);
EXPECT_EQ(RCL_RET_NODE_INVALID, ret);
rcl_reset_error();
start_memory_checking();
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
ret = rcl_node_get_domain_id(&node, &actual_domain_id);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_EQ(RCL_RET_OK, ret);
if (RCL_RET_OK == ret) {
EXPECT_EQ(42, actual_domain_id);
}
// Test rcl_node_get_rmw_handle().
rmw_node_t * node_handle;
node_handle = rcl_node_get_rmw_handle(nullptr);
EXPECT_EQ(nullptr, node_handle);
rcl_reset_error();
node_handle = rcl_node_get_rmw_handle(&zero_node);
EXPECT_EQ(nullptr, node_handle);
rcl_reset_error();
node_handle = rcl_node_get_rmw_handle(&invalid_node);
EXPECT_EQ(nullptr, node_handle);
rcl_reset_error();
start_memory_checking();
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
node_handle = rcl_node_get_rmw_handle(&node);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_NE(nullptr, node_handle);
// Test rcl_node_get_rcl_instance_id().
uint64_t instance_id;
instance_id = rcl_node_get_rcl_instance_id(nullptr);
EXPECT_EQ(0, instance_id);
rcl_reset_error();
instance_id = rcl_node_get_rcl_instance_id(&zero_node);
EXPECT_EQ(0, instance_id);
rcl_reset_error();
instance_id = rcl_node_get_rcl_instance_id(&invalid_node);
EXPECT_NE(0, instance_id);
EXPECT_NE(42, instance_id);
rcl_reset_error();
start_memory_checking();
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
instance_id = rcl_node_get_rcl_instance_id(&node);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_NE(0, instance_id);
}
/* Tests the node life cycle, including rcl_node_init() and rcl_node_fini().
*/
TEST_F(TestNodeFixture, test_rcl_node_life_cycle) {
stop_memory_checking();
rcl_ret_t ret;
rcl_node_t node = rcl_get_zero_initialized_node();
const char * name = "node_name";
rcl_node_options_t default_options = rcl_node_get_default_options();
// Trying to init before rcl_init() should fail.
ret = rcl_node_init(&node, name, &default_options);
ASSERT_EQ(RCL_RET_NOT_INIT, ret) << "Expected RCL_RET_NOT_INIT";
rcl_reset_error();
// Initialize rcl with rcl_init().
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret);
auto rcl_shutdown_exit = make_scope_exit([]() {
rcl_ret_t ret = rcl_shutdown();
ASSERT_EQ(RCL_RET_OK, ret);
});
// Try invalid arguments.
ret = rcl_node_init(nullptr, name, &default_options);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ret = rcl_node_init(&node, nullptr, &default_options);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ret = rcl_node_init(&node, name, nullptr);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
// Try with invalid allocator.
rcl_node_options_t options_with_invalid_allocator = rcl_node_get_default_options();
options_with_invalid_allocator.allocator.allocate = nullptr;
options_with_invalid_allocator.allocator.deallocate = nullptr;
options_with_invalid_allocator.allocator.reallocate = nullptr;
ret = rcl_node_init(&node, name, &options_with_invalid_allocator);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";
rcl_reset_error();
node = rcl_get_zero_initialized_node();
// Try with failing allocator.
rcl_node_options_t options_with_failing_allocator = rcl_node_get_default_options();
options_with_failing_allocator.allocator.allocate = failing_malloc;
options_with_failing_allocator.allocator.deallocate = failing_free;
options_with_failing_allocator.allocator.reallocate = failing_realloc;
ret = rcl_node_init(&node, name, &options_with_failing_allocator);
EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC";
rcl_reset_error();
node = rcl_get_zero_initialized_node();
// Try fini with invalid arguments.
ret = rcl_node_fini(nullptr);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";
rcl_reset_error();
// Try fini with an uninitialized node.
ret = rcl_node_fini(&node);
EXPECT_EQ(RCL_RET_OK, ret);
// Try a normal init and fini.
ret = rcl_node_init(&node, name, &default_options);
EXPECT_EQ(RCL_RET_OK, ret);
ret = rcl_node_fini(&node);
EXPECT_EQ(RCL_RET_OK, ret);
node = rcl_get_zero_initialized_node();
// Try repeated init and fini calls.
ret = rcl_node_init(&node, name, &default_options);
EXPECT_EQ(RCL_RET_OK, ret);
ret = rcl_node_init(&node, name, &default_options);
EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << "Expected RCL_RET_ALREADY_INIT";
ret = rcl_node_fini(&node);
EXPECT_EQ(RCL_RET_OK, ret);
ret = rcl_node_fini(&node);
EXPECT_EQ(RCL_RET_OK, ret);
node = rcl_get_zero_initialized_node();
// Try with a specific domain id.
rcl_node_options_t options_with_custom_domain_id = rcl_node_get_default_options();
options_with_custom_domain_id.domain_id = 42;
ret = rcl_node_init(&node, name, &options_with_custom_domain_id);
EXPECT_EQ(RCL_RET_OK, ret);
ret = rcl_node_fini(&node);
EXPECT_EQ(RCL_RET_OK, ret);
node = rcl_get_zero_initialized_node();
}

233
rcl/test/rcl/test_rcl.cpp Normal file
View file

@ -0,0 +1,233 @@
// 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 <gtest/gtest.h>
#include "rcl/rcl.h"
#include "../memory_tools.hpp"
#include "rcl/error_handling.h"
class TestRCLFixture : public::testing::Test
{
public:
void SetUp()
{
set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
void TearDown()
{
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
set_on_unexpected_malloc_callback(nullptr);
set_on_unexpected_realloc_callback(nullptr);
set_on_unexpected_free_callback(nullptr);
}
};
void *
failing_malloc(size_t size, void * state)
{
(void)(size);
(void)(state);
return nullptr;
}
void *
failing_realloc(void * pointer, size_t size, void * state)
{
(void)(pointer);
(void)(size);
(void)(state);
return nullptr;
}
void
failing_free(void * pointer, void * state)
{
(void)pointer;
(void)state;
}
struct FakeTestArgv
{
FakeTestArgv()
: argc(2)
{
this->argv = reinterpret_cast<char **>(malloc(2 * sizeof(char *)));
if (!this->argv) {
throw std::bad_alloc();
}
static const size_t size = 10;
this->argv[0] = reinterpret_cast<char *>(malloc(size * sizeof(char)));
snprintf(this->argv[0], size, "foo");
this->argv[1] = reinterpret_cast<char *>(malloc(size * sizeof(char)));
snprintf(this->argv[1], size, "bar");
}
~FakeTestArgv()
{
if (this->argv) {
if (this->argc > 0) {
size_t unsigned_argc = this->argc;
for (size_t i = 0; i < unsigned_argc; --i) {
free(this->argv[i]);
}
}
}
free(this->argv);
}
int argc;
char ** argv;
};
/* Tests the rcl_init(), rcl_ok(), and rcl_shutdown() functions.
*/
TEST_F(TestRCLFixture, test_rcl_init_and_ok_and_shutdown) {
rcl_ret_t ret;
// A shutdown before any init has been called should fail.
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_NOT_INIT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
// If argc is not 0, but argv is, it should be an invalid argument.
ret = rcl_init(42, nullptr, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
// If either the allocate or deallocate function pointers are not set, it should be invalid arg.
rcl_allocator_t invalid_allocator = rcl_get_default_allocator();
invalid_allocator.allocate = nullptr;
ret = rcl_init(0, nullptr, invalid_allocator);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
invalid_allocator.allocate = rcl_get_default_allocator().allocate;
invalid_allocator.deallocate = nullptr;
ret = rcl_init(0, nullptr, invalid_allocator);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
// If the malloc call fails (with some valid arguments to copy), it should be a bad alloc.
{
FakeTestArgv test_args;
rcl_allocator_t failing_allocator = rcl_get_default_allocator();
failing_allocator.allocate = &failing_malloc;
failing_allocator.deallocate = failing_free;
failing_allocator.reallocate = failing_realloc;
ret = rcl_init(test_args.argc, test_args.argv, failing_allocator);
EXPECT_EQ(RCL_RET_BAD_ALLOC, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
}
// If argc is 0 and argv is nullptr and the allocator is valid, it should succeed.
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
// Then shutdown should work.
ret = rcl_shutdown();
EXPECT_EQ(ret, RCL_RET_OK);
ASSERT_FALSE(rcl_ok());
// Valid argc/argv values and a valid allocator should succeed.
{
FakeTestArgv test_args;
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
}
// Then shutdown should work.
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_FALSE(rcl_ok());
// A repeat call to shutdown should not work.
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_NOT_INIT, ret);
rcl_reset_error();
ASSERT_FALSE(rcl_ok());
// Repeat, but valid, calls to rcl_init() should fail.
{
FakeTestArgv test_args;
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_ALREADY_INIT, ret);
rcl_reset_error();
ASSERT_TRUE(rcl_ok());
}
// But shutdown should still work.
ret = rcl_shutdown();
EXPECT_EQ(ret, RCL_RET_OK);
ASSERT_FALSE(rcl_ok());
}
/* Tests the rcl_get_instance_id() and rcl_ok() functions.
*/
TEST_F(TestRCLFixture, test_rcl_get_instance_id_and_ok) {
rcl_ret_t ret;
// Instance id should be 0 before rcl_init().
EXPECT_EQ(0, rcl_get_instance_id());
ASSERT_FALSE(rcl_ok());
// It should still return 0 after an invalid init.
ret = rcl_init(1, nullptr, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
EXPECT_EQ(0, rcl_get_instance_id());
ASSERT_FALSE(rcl_ok());
// A non-zero instance id should be returned after a valid init.
{
FakeTestArgv test_args;
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
}
// And it should be allocation free.
uint64_t first_instance_id;
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
first_instance_id = rcl_get_instance_id();
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
EXPECT_NE(0, first_instance_id);
EXPECT_EQ(first_instance_id, rcl_get_instance_id()); // Repeat calls should return the same.
EXPECT_EQ(true, rcl_ok());
// Calling after a shutdown should return 0.
ret = rcl_shutdown();
EXPECT_EQ(ret, RCL_RET_OK);
EXPECT_EQ(0, rcl_get_instance_id());
ASSERT_FALSE(rcl_ok());
// It should return a different value after another valid init.
{
FakeTestArgv test_args;
ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret);
ASSERT_TRUE(rcl_ok());
}
EXPECT_NE(0, rcl_get_instance_id());
EXPECT_NE(first_instance_id, rcl_get_instance_id());
ASSERT_TRUE(rcl_ok());
// Shutting down a second time should result in 0 again.
ret = rcl_shutdown();
EXPECT_EQ(ret, RCL_RET_OK);
EXPECT_EQ(0, rcl_get_instance_id());
ASSERT_FALSE(rcl_ok());
}

120
rcl/test/rcl/test_time.cpp Normal file
View file

@ -0,0 +1,120 @@
// 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 <gtest/gtest.h>
#include <inttypes.h>
#include <chrono>
#include <thread>
#include "rcl/error_handling.h"
#include "rcl/time.h"
#include "../memory_tools.hpp"
class TestTimeFixture : public::testing::Test
{
public:
void SetUp()
{
set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
void TearDown()
{
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
set_on_unexpected_malloc_callback(nullptr);
set_on_unexpected_realloc_callback(nullptr);
set_on_unexpected_free_callback(nullptr);
}
};
/* Tests the rcl_system_time_point_now() function.
*/
TEST_F(TestTimeFixture, test_rcl_system_time_point_now) {
assert_no_realloc_begin();
rcl_ret_t ret;
// Check for invalid argument error condition (allowed to alloc).
ret = rcl_system_time_point_now(nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
assert_no_malloc_begin();
assert_no_free_begin();
// Check for normal operation (not allowed to alloc).
rcl_system_time_point_t now = {0};
ret = rcl_system_time_point_now(&now);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_NE(now.nanoseconds, 0);
// Compare to std::chrono::system_clock time (within a second).
now = {0};
ret = rcl_system_time_point_now(&now);
{
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
int64_t now_ns_int = now_ns.count();
int64_t now_diff = now.nanoseconds - now_ns_int;
const int k_tolerance_ms = 1000;
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs";
}
}
/* Tests the rcl_steady_time_point_now() function.
*/
TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) {
assert_no_realloc_begin();
rcl_ret_t ret;
// Check for invalid argument error condition (allowed to alloc).
ret = rcl_steady_time_point_now(nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
rcl_reset_error();
assert_no_malloc_begin();
assert_no_free_begin();
// Check for normal operation (not allowed to alloc).
rcl_steady_time_point_t now = {0};
ret = rcl_steady_time_point_now(&now);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
EXPECT_NE(now.nanoseconds, 0);
// Compare to std::chrono::steady_clock difference of two times (within a second).
now = {0};
ret = rcl_steady_time_point_now(&now);
std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now();
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
// Wait for a little while.
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// Then take a new timestamp with each and compare.
rcl_steady_time_point_t later;
ret = rcl_steady_time_point_now(&later);
std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now();
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
int64_t steady_diff = later.nanoseconds - now.nanoseconds;
int64_t sc_diff =
std::chrono::duration_cast<std::chrono::nanoseconds>(later_sc - now_sc).count();
const int k_tolerance_ms = 1;
EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "steady_clock differs";
}

40
rcl/test/scope_exit.hpp Normal file
View file

@ -0,0 +1,40 @@
// 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 SCOPE_EXIT_HPP_
#define SCOPE_EXIT_HPP_
#include <functional>
template<typename Callable>
struct ScopeExit
{
explicit ScopeExit(Callable callable)
: callable_(callable) {}
~ScopeExit() {callable_(); }
private:
Callable callable_;
};
template<typename Callable>
ScopeExit<Callable>
make_scope_exit(Callable callable)
{
return ScopeExit<Callable>(callable);
}
#define SCOPE_EXIT(code) make_scope_exit([&]() {code; })
#endif // SCOPE_EXIT_HPP_

133
rcl/test/test_memory_tools.cpp Normal file
View file

@ -0,0 +1,133 @@
// 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 <gtest/gtest.h>
#include "./memory_tools.hpp"
/* Tests the allocatation checking tools.
*/
TEST(TestMemoryTools, test_allocation_checking_tools) {
size_t unexpected_mallocs = 0;
auto on_unexpected_malloc = ([&unexpected_mallocs]() {
unexpected_mallocs++;
});
set_on_unexpected_malloc_callback(on_unexpected_malloc);
size_t unexpected_reallocs = 0;
auto on_unexpected_realloc = ([&unexpected_reallocs]() {
unexpected_reallocs++;
});
set_on_unexpected_realloc_callback(on_unexpected_realloc);
size_t unexpected_frees = 0;
auto on_unexpected_free = ([&unexpected_frees]() {
unexpected_frees++;
});
set_on_unexpected_free_callback(on_unexpected_free);
void * mem = nullptr;
void * remem = nullptr;
// First try before enabling, should have no effect.
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 0);
EXPECT_EQ(unexpected_reallocs, 0);
EXPECT_EQ(unexpected_frees, 0);
// Enable checking, but no assert, should have no effect.
start_memory_checking();
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 0);
EXPECT_EQ(unexpected_reallocs, 0);
EXPECT_EQ(unexpected_frees, 0);
// Enable no_* asserts, should increment all once.
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
mem = malloc(1024);
assert_no_malloc_end();
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
assert_no_realloc_end();
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
assert_no_free_end();
EXPECT_EQ(unexpected_mallocs, 1);
EXPECT_EQ(unexpected_reallocs, 1);
EXPECT_EQ(unexpected_frees, 1);
// Enable on malloc assert, only malloc should increment.
assert_no_malloc_begin();
mem = malloc(1024);
assert_no_malloc_end();
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 2);
EXPECT_EQ(unexpected_reallocs, 1);
EXPECT_EQ(unexpected_frees, 1);
// Enable on realloc assert, only realloc should increment.
assert_no_realloc_begin();
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
assert_no_realloc_end();
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 2);
EXPECT_EQ(unexpected_reallocs, 2);
EXPECT_EQ(unexpected_frees, 1);
// Enable on free assert, only free should increment.
assert_no_free_begin();
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
assert_no_free_end();
EXPECT_EQ(unexpected_mallocs, 2);
EXPECT_EQ(unexpected_reallocs, 2);
EXPECT_EQ(unexpected_frees, 2);
// Go again, after disabling asserts, should have no effect.
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 2);
EXPECT_EQ(unexpected_reallocs, 2);
EXPECT_EQ(unexpected_frees, 2);
// Go once more after disabling everything, should have no effect.
stop_memory_checking();
mem = malloc(1024);
ASSERT_NE(mem, nullptr);
remem = realloc(mem, 2048);
ASSERT_NE(remem, nullptr);
if (!remem) {free(mem);}
free(remem);
EXPECT_EQ(unexpected_mallocs, 2);
EXPECT_EQ(unexpected_reallocs, 2);
EXPECT_EQ(unexpected_frees, 2);
}