diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 8e83769..a1658d5 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -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() diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h new file mode 100644 index 0000000..ce32052 --- /dev/null +++ b/rcl/include/rcl/allocator.h @@ -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_ diff --git a/rcl/include/rcl/error_handling.h b/rcl/include/rcl/error_handling.h new file mode 100644 index 0000000..1e81e95 --- /dev/null +++ b/rcl/include/rcl/error_handling.h @@ -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_ diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h new file mode 100644 index 0000000..c1f6c1a --- /dev/null +++ b/rcl/include/rcl/guard_condition.h @@ -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_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_ diff --git a/rcl/include/rcl/macros.h b/rcl/include/rcl/macros.h new file mode 100644 index 0000000..2399307 --- /dev/null +++ b/rcl/include/rcl/macros.h @@ -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_ diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h new file mode 100644 index 0000000..60b75c1 --- /dev/null +++ b/rcl/include/rcl/node.h @@ -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 + +#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_ diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h new file mode 100644 index 0000000..6cc5d8b --- /dev/null +++ b/rcl/include/rcl/publisher.h @@ -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 + * #include + * rosidl_message_type_support_t * string_ts = + * ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String); + * + * For C++ a template function is used: + * + * #include + * #include + * rosidl_message_type_support_t * string_ts = + * rosidl_generator_cpp::get_message_type_support_handle(); + * + * 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 + * #include + * #include + * + * 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_ diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 5ee1fe9..4b42d39 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -15,140 +15,113 @@ #ifndef RCL__RCL_H_ #define RCL__RCL_H_ -#include // For bool -#include // For size_t +#if __cplusplus +extern "C" +{ +#endif -// For rosidl_message_type_support_t -#include +#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_ diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h new file mode 100644 index 0000000..a417248 --- /dev/null +++ b/rcl/include/rcl/subscription.h @@ -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 + * #include + * rosidl_message_type_support_t * string_ts = + * ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String); + * + * For C++ a template function is used: + * + * #include + * #include + * rosidl_message_type_support_t * string_ts = + * rosidl_generator_cpp::get_message_type_support_handle(); + * + * 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 + * #include + * #include + * + * 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_ diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h new file mode 100644 index 0000000..60ccf5d --- /dev/null +++ b/rcl/include/rcl/time.h @@ -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_ diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h new file mode 100644 index 0000000..8753356 --- /dev/null +++ b/rcl/include/rcl/timer.h @@ -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 + +#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 + * + * 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_ diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index d9135bb..ba2d8a3 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -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_ diff --git a/rcl/include/rcl/visibility_control.h b/rcl/include/rcl/visibility_control.h new file mode 100644 index 0000000..85e3b16 --- /dev/null +++ b/rcl/include/rcl/visibility_control.h @@ -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_ diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h new file mode 100644 index 0000000..c36300d --- /dev/null +++ b/rcl/include/rcl/wait.h @@ -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 +#include + +#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_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_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_ diff --git a/rcl/package.xml b/rcl/package.xml index 034064c..5f921a3 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -10,12 +10,22 @@ ament_cmake - rmw - rmw + rcl_interfaces + rmw_implementation_cmake + rosidl_generator_c + rcl_interfaces + rosidl_generator_c + + rosidl_default_runtime + + rmw_implementation + + ament_cmake_gtest ament_lint_auto ament_lint_common + rmw ament_cmake diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c new file mode 100644 index 0000000..32c6406 --- /dev/null +++ b/rcl/src/rcl/allocator.c @@ -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 + +#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; +} diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c new file mode 100644 index 0000000..e4db769 --- /dev/null +++ b/rcl/src/rcl/common.c @@ -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 + +#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 diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h new file mode 100644 index 0000000..e5c8027 --- /dev/null +++ b/rcl/src/rcl/common.h @@ -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_ diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c new file mode 100644 index 0000000..b4d30b3 --- /dev/null +++ b/rcl/src/rcl/guard_condition.c @@ -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 diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c new file mode 100644 index 0000000..ccb1201 --- /dev/null +++ b/rcl/src/rcl/node.c @@ -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 +#include +#include + +#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 diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c new file mode 100644 index 0000000..808a2ca --- /dev/null +++ b/rcl/src/rcl/publisher.c @@ -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 + +#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 diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c new file mode 100644 index 0000000..6809407 --- /dev/null +++ b/rcl/src/rcl/rcl.c @@ -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 + +#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 diff --git a/rcl/src/rcl/stdatomic_helper.h b/rcl/src/rcl/stdatomic_helper.h new file mode 100644 index 0000000..1c1c7a7 --- /dev/null +++ b/rcl/src/rcl/stdatomic_helper.h @@ -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 +#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_ diff --git a/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h b/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h new file mode 100644 index 0000000..dd0c674 --- /dev/null +++ b/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h @@ -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 + * David Chisnall + * 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 +#include + +#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 diff --git a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h new file mode 100644 index 0000000..d814300 --- /dev/null +++ b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h @@ -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 + * David Chisnall + * 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 + +#include +#include + +// 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_ diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c new file mode 100644 index 0000000..2e58be0 --- /dev/null +++ b/rcl/src/rcl/subscription.c @@ -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 diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c new file mode 100644 index 0000000..7226b2c --- /dev/null +++ b/rcl/src/rcl/time.c @@ -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) diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c new file mode 100644 index 0000000..fd6cbc4 --- /dev/null +++ b/rcl/src/rcl/time_unix.c @@ -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 +#include +#endif // defined(__MACH__) +#include +#include +#include + +#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, ×pec_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, ×pec_now); +#else // defined(CLOCK_MONOTONIC_RAW) + clock_gettime(CLOCK_MONOTONIC, ×pec_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 diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c new file mode 100644 index 0000000..919cf46 --- /dev/null +++ b/rcl/src/rcl/time_win32.c @@ -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 + +#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 diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c new file mode 100644 index 0000000..30d73c9 --- /dev/null +++ b/rcl/src/rcl/timer.c @@ -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 diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c new file mode 100644 index 0000000..eb87cdc --- /dev/null +++ b/rcl/src/rcl/wait.c @@ -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 +#include +#include + +#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 diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt new file mode 100644 index 0000000..291fcd9 --- /dev/null +++ b/rcl/test/CMakeLists.txt @@ -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=$) +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=$) +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() diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools.cpp new file mode 100644 index 0000000..17fd508 --- /dev/null +++ b/rcl/test/memory_tools.cpp @@ -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 +#include +#include + +#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 + +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 ... diff --git a/rcl/test/memory_tools.hpp b/rcl/test/memory_tools.hpp new file mode 100644 index 0000000..001c110 --- /dev/null +++ b/rcl/test/memory_tools.hpp @@ -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 + +#include + +// 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 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_ diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp new file mode 100644 index 0000000..c7ea833 --- /dev/null +++ b/rcl/test/memory_tools_common.cpp @@ -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 + +#include +#include + +#if defined(__APPLE__) +#include +#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 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(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(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(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;} diff --git a/rcl/test/memory_tools_osx_interpose.cpp b/rcl/test/memory_tools_osx_interpose.cpp new file mode 100644 index 0000000..91579cf --- /dev/null +++ b/rcl/test/memory_tools_osx_interpose.cpp @@ -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(newf), \ + reinterpret_cast(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); diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp new file mode 100644 index 0000000..b9ca12a --- /dev/null +++ b/rcl/test/rcl/test_allocator.cpp @@ -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 + +#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); +} diff --git a/rcl/test/rcl/test_common.cpp b/rcl/test/rcl/test_common.cpp new file mode 100644 index 0000000..d7571cf --- /dev/null +++ b/rcl/test/rcl/test_common.cpp @@ -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 + +#include + +#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)); +} diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp new file mode 100644 index 0000000..dc93b84 --- /dev/null +++ b/rcl/test/rcl/test_node.cpp @@ -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 + +#include + +#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(); +} diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp new file mode 100644 index 0000000..7b8d0af --- /dev/null +++ b/rcl/test/rcl/test_rcl.cpp @@ -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 + +#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(malloc(2 * sizeof(char *))); + if (!this->argv) { + throw std::bad_alloc(); + } + static const size_t size = 10; + this->argv[0] = reinterpret_cast(malloc(size * sizeof(char))); + snprintf(this->argv[0], size, "foo"); + this->argv[1] = reinterpret_cast(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()); +} diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp new file mode 100644 index 0000000..1e89f10 --- /dev/null +++ b/rcl/test/rcl/test_time.cpp @@ -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 + +#include + +#include +#include + +#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(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(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"; +} diff --git a/rcl/test/scope_exit.hpp b/rcl/test/scope_exit.hpp new file mode 100644 index 0000000..056e1fa --- /dev/null +++ b/rcl/test/scope_exit.hpp @@ -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 + +template +struct ScopeExit +{ + explicit ScopeExit(Callable callable) + : callable_(callable) {} + ~ScopeExit() {callable_(); } + +private: + Callable callable_; +}; + +template +ScopeExit +make_scope_exit(Callable callable) +{ + return ScopeExit(callable); +} + +#define SCOPE_EXIT(code) make_scope_exit([&]() {code; }) + +#endif // SCOPE_EXIT_HPP_ diff --git a/rcl/test/test_memory_tools.cpp b/rcl/test/test_memory_tools.cpp new file mode 100644 index 0000000..dcddaa3 --- /dev/null +++ b/rcl/test/test_memory_tools.cpp @@ -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 + +#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); +}