Merge pull request #5 from ros2/rcl_refactor

refactor rcl into a functional library

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

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_

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_

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/rcl.h>
+ *
+ *    rcl_node_t node = rcl_get_zero_initialized_node();
+ *    rcl_node_options_t node_ops = rcl_node_get_default_options();
+ *    rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops);
+ *    // ... error handling
+ *    rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
+ *    ret = rcl_guard_condition_init(
+ *      &guard_condition, &node, rcl_guard_condition_get_default_options());
+ *    // ... error handling, and on shutdown do deinitialization:
+ *    ret = rcl_guard_condition_fini(&guard_condition, &node);
+ *    // ... error handling for rcl_guard_condition_fini()
+ *    ret = rcl_node_fini(&node);
+ *    // ... error handling for rcl_deinitialize_node()
+ *
+ * This function does allocate heap memory.
+ * This function is not thread-safe.
+ * This function is lock-free.
+ *
+ * \TODO(wjwwood): does this function need a node to be passed to it? (same for fini)
+ *
+ * \param[inout] guard_condition preallocated guard_condition structure
+ * \param[in] node valid rcl node handle
+ * \param[in] options the guard_condition's options
+ * \return RCL_RET_OK if guard_condition was initialized successfully, or
+ *         RCL_RET_ALREADY_INIT if the guard condition is already initialized, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_BAD_ALLOC if allocating memory failed, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_guard_condition_init(
+  rcl_guard_condition_t * guard_condition,
+  const rcl_node_t * node,
+  const rcl_guard_condition_options_t options);
+
+/// Finalize a rcl_guard_condition_t.
+/* After calling, calls to rcl_guard_condition_trigger() will fail when using
+ * this guard condition.
+ * However, the given node handle is still valid.
+ *
+ * This function does free heap memory and can allocate memory on errors.
+ * This function is not thread-safe with rcl_guard_condition_trigger().
+ * This function is lock-free.
+ *
+ * \param[inout] guard_condition handle to the guard_condition to be finalized
+ * \param[in] node handle to the node used to create the guard_condition
+ * \return RCL_RET_OK if guard_condition was finalized successfully, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node);
+
+/// Return the default options in a rcl_guard_condition_options_t struct.
+/* This function does not allocate heap memory.
+ * This function is thread-safe.
+ * This function is lock-free.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_guard_condition_options_t
+rcl_guard_condition_get_default_options();
+
+/// Trigger a rcl guard condition.
+/* This function can fail, and therefore return NULL, if the:
+ *   - guard condition is NULL
+ *   - guard condition is invalid (never called init or called fini)
+ *
+ * A guard condition can be triggered from any thread.
+ *
+ * This function does not allocate heap memory, but can on errors.
+ * This function is thread-safe with itself, but cannot be called concurrently
+ * with rcl_guard_condition_fini() on the same guard condition.
+ * This function is lock-free, but the underlying system calls may not be.
+ *
+ * \param[in] guard_condition handle to the guard_condition to be triggered
+ * \return RCL_RET_OK if the guard condition was triggered, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition);
+
+/// Return the rmw guard condition handle.
+/* The handle returned is a pointer to the internally held rmw handle.
+ * This function can fail, and therefore return NULL, if the:
+ *   - guard_condition is NULL
+ *   - guard_condition is invalid (never called init, called fini, or invalid node)
+ *
+ * The returned handle is made invalid if the guard condition is finalized or
+ * if rcl_shutdown() is called.
+ * The returned handle is not guaranteed to be valid for the life time of the
+ * guard condition as it may be finalized and recreated itself.
+ * Therefore it is recommended to get the handle from the guard condition using
+ * this function each time it is needed and avoid use of the handle
+ * concurrently with functions that might change it.
+ *
+ * \param[in] guard_condition pointer to the rcl guard_condition
+ * \return rmw guard_condition handle if successful, otherwise NULL
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rmw_guard_condition_t *
+rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition);
+
+#if __cplusplus
+}
+#endif
+
+#endif  // RCL__GUARD_CONDITION_H_

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_

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 <stdint.h>
+
+#include "rcl/allocator.h"
+#include "rcl/macros.h"
+#include "rcl/types.h"
+#include "rcl/visibility_control.h"
+
+#define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID SIZE_MAX
+
+struct rcl_node_impl_t;
+
+/// Handle for a ROS node.
+typedef struct rcl_node_t
+{
+  /// Private implementation pointer.
+  struct rcl_node_impl_t * impl;
+} rcl_node_t;
+
+typedef struct rcl_node_options_t
+{
+  // bool anonymous_name;
+  // rmw_qos_profile_t parameter_qos;
+  /// If true, no parameter infrastructure will be setup.
+  // bool no_parameters;
+  /// If set, then this value overrides the ROS_DOMAIN_ID environment variable.
+  /* It defaults to RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID, which will cause the
+   * node to use the ROS domain ID set in the ROS_DOMAIN_ID environment
+   * variable, or on some systems 0 if the environment variable is not set.
+   *
+   * \TODO(wjwwood): Should we put a limit on the ROS_DOMAIN_ID value, that way
+   *                 we can have a safe value for the default
+   *                 RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID? (currently max size_t)
+   */
+  size_t domain_id;
+  /// Custom allocator used for internal allocations.
+  rcl_allocator_t allocator;
+} rcl_node_options_t;
+
+/// Return a rcl_node_t struct with members initialized to NULL.
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_node_t
+rcl_get_zero_initialized_node();
+
+/// Initialize a ROS node.
+/* Calling this on a rcl_node_t makes it a valid node handle until rcl_shutdown
+ * is called or until rcl_node_fini is called on it.
+ *
+ * After calling the ROS node object can be used to create other middleware
+ * primitives like publishers, services, parameters, and etc.
+ *
+ * The name of the node cannot coincide with another node of the same name.
+ * If a node of the same name is already in the domain, it will be shutdown.
+ *
+ * A node contains infrastructure for ROS parameters, which include advertising
+ * publishers and service servers.
+ * This function will create those external parameter interfaces even if
+ * parameters are not used later.
+ *
+ * The rcl_node_t given must be allocated and zero initalized.
+ * Passing an rcl_node_t which has already had this function called on it, more
+ * recently than rcl_node_fini, will fail.
+ * An allocated rcl_node_t with uninitialized memory is undefined behavior.
+ *
+ * Expected usage:
+ *
+ *    rcl_node_t node = rcl_get_zero_initialized_node();
+ *    rcl_node_options_t * node_ops = rcl_node_get_default_options();
+ *    rcl_ret_t ret = rcl_node_init(&node, "node_name", node_ops);
+ *    // ... error handling and then use the node, but finally deinitialize it:
+ *    ret = rcl_node_fini(&node);
+ *    // ... error handling for rcl_node_fini()
+ *
+ * This function allocates heap memory.
+ * This function is not thread-safe.
+ * This function is lock-free so long as the C11's stdatomic.h function
+ * atomic_is_lock_free() returns true for atomic_uint_least64_t.
+ *
+ * \pre the node handle must be allocated, zero initialized, and invalid
+ * \post the node handle is valid and can be used to in other rcl_* functions
+ *
+ * \param[inout] node a preallocated node structure
+ * \param[in] name the name of the node
+ * \param[in] options the node options; pass null to use default options
+ * \return RCL_RET_OK if node was initialized successfully, or
+ *         RCL_RET_ALREADY_INIT if the node has already be initialized, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_BAD_ALLOC if allocating memory failed, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options);
+
+/// Finalized a rcl_node_t.
+/* Shuts down any automatically created infrastructure and deallocates memory.
+ * After calling, the rcl_node_t can be safely deallocated.
+ *
+ * Any middleware primitives created by the user, e.g. publishers, services, etc.,
+ * are invalid after deinitialization.
+ *
+ * This function manipulates heap memory.
+ * This function is not thread-safe.
+ * This function is lock-free so long as the C11's stdatomic.h function
+ * atomic_is_lock_free() returns true for atomic_uint_least64_t.
+ *
+ * \param[in] node handle to the node to be finalized
+ * \return RCL_RET_OK if node was finalized successfully, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_node_fini(rcl_node_t * node);
+
+/// Return the default node options in a rcl_node_options_t.
+RCL_PUBLIC
+rcl_node_options_t
+rcl_node_get_default_options();
+
+/// Get the name of the node.
+/* This function returns the node's internal name string.
+ * This function can fail, and therefore return NULL, if:
+ *   - node is NULL
+ *   - node has not been initialized (the implementation is invalid)
+ *
+ * The returned string is only valid as long as the given rcl_node_t is valid.
+ * The value of the string may change if the value in the rcl_node_t changes,
+ * and therefore copying the string is recommended if this is a concern.
+ *
+ * This function does not manipulate heap memory.
+ * This function is thread-safe for different nodes.
+ * This function is lock-free.
+ *
+ * \param[in] node pointer to the node
+ * \return name string if successful, otherwise NULL
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+const char *
+rcl_node_get_name(const rcl_node_t * node);
+
+/// Return the rcl node options.
+/* This function returns the node's internal options struct.
+ * This function can fail, and therefore return NULL, if:
+ *   - node is NULL
+ *   - node has not been initialized (the implementation is invalid)
+ *
+ * The returned struct is only valid as long as the given rcl_node_t is valid.
+ * The values in the struct may change if the options of the rcl_node_t changes,
+ * and therefore copying the struct is recommended if this is a concern.
+ *
+ * This function does not manipulate heap memory.
+ * This function is thread-safe for different nodes.
+ * This function is lock-free.
+ *
+ * \param[in] node pointer to the node
+ * \return options struct if successful, otherwise NULL
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+const rcl_node_options_t *
+rcl_node_get_options(const rcl_node_t * node);
+
+/// Return the ROS domain ID that the node is using.
+/* This function returns the ROS domain ID that the node is in.
+ *
+ * This function should be used to determine what domain_id was used rather
+ * than checking the domin_id field in the node options, because if
+ * RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID is used when creating the node then
+ * it is not changed after creation, but this function will return the actual
+ * domain_id used.
+ *
+ * The domain_id field must point to an allocated size_t object to which the
+ * ROS domain ID will be written.
+ *
+ * This function does not manipulate heap memory.
+ * This function is thread-safe for different nodes.
+ * This function is lock-free.
+ *
+ * \param[in] node the handle to the node being queried
+ * \return RCL_RET_OK if node the domain ID was retrieved successfully, or
+ *         RCL_RET_NODE_INVALID if the node is invalid, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id);
+
+/// Return the rmw node handle.
+/* The handle returned is a pointer to the internally held rmw handle.
+ * This function can fail, and therefore return NULL, if:
+ *   - node is NULL
+ *   - node has not been initialized (the implementation is invalid)
+ *
+ * The returned handle is made invalid if the node is finalized or if
+ * rcl_shutdown() is called.
+ * The returned handle is not guaranteed to be valid for the life time of the
+ * node as it may be finalized and recreated itself.
+ * Therefore it is recommended to get the handle from the node using
+ * this function each time it is needed and avoid use of the handle
+ * concurrently with functions that might change it.
+ *
+ * This function does not manipulate heap memory.
+ * This function is thread-safe for different nodes.
+ * This function is lock-free.
+ *
+ * \param[in] node pointer to the rcl node
+ * \return rmw node handle if successful, otherwise NULL
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rmw_node_t *
+rcl_node_get_rmw_handle(const rcl_node_t * node);
+
+/// Return the associated rcl instance id.
+/* This id is stored when rcl_node_init is called and can be compared with the
+ * value returned by rcl_get_instance_id() to check if this node was created in
+ * the current rcl context (since the latest call to rcl_init().
+ *
+ * This function can fail, and therefore return 0, if:
+ *   - node is NULL
+ *   - node has not been initialized (the implementation is invalid)
+ *
+ * This function will succeed, however, even if rcl_shutdown has been called
+ * since the node was created.
+ *
+ * This function does not manipulate heap memory.
+ * This function is thread-safe for different nodes.
+ * This function is lock-free.
+ *
+ * \param[in] node pointer to the rcl node
+ * \return rcl instance id captured at node creation or 0 if there was an error
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+uint64_t
+rcl_node_get_rcl_instance_id(const rcl_node_t * node);
+
+#if __cplusplus
+}
+#endif
+
+#endif  // RCL__NODE_H_

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

rcl/include/rcl/rcl.h

@@ -15,140 +15,113 @@
 #ifndef RCL__RCL_H_
 #define RCL__RCL_H_
 
-#include <stdbool.h>  // For bool
+#if __cplusplus
-#include <stddef.h>  // For size_t
+extern "C"
+{
+#endif
 
-// For rosidl_message_type_support_t
+#include "rcl/macros.h"
-#include <rosidl_generator_c/message_type_support.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 of rcl.
-
+/* Unless otherwise noted, this must be called before using any rcl functions.
-/// Global initialization for rcl; should be called once per process.
+ *
+ * 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.
+/// Signal global shutdown of rcl.
-rcl_node_t *
+/* This function does not have to be called on exit, but does have to be called
-rcl_create_node(const char * name);
+ * making a repeat call to rcl_init.
-
+ *
-/// Destroys a rcl_node_t.
+ * 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.
+/// Returns an uint64_t number that is unique for the latest rcl_init call.
-rcl_publisher_t *
+/* If called before rcl_init or after rcl_shutdown then 0 will be returned.
-rcl_create_publisher(
+ *
-  const rcl_node_t * node,
+ * This function does not allocate memory.
-  const rosidl_message_type_support_t * type_support,
+ * This function is thread-safe.
-  const char * topic_name,
+ * This function is lock-free so long as the C11's stdatomic.h function
-  size_t queue_size);
+ * 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.
+/// Return true if rcl is currently initialized, otherwise false.
-rcl_ret_t
+/* This function does not allocate memory.
-rcl_destroy_publisher(rcl_publisher_t * publisher);
+ * 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.
+#if __cplusplus
-rcl_ret_t
+}
-rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
+#endif
-
-/// 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);
 
 #endif  // RCL__RCL_H_

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

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_

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

rcl/include/rcl/types.h

@@ -19,40 +19,28 @@
 
 typedef rmw_ret_t rcl_ret_t;
 #define RCL_RET_OK RMW_RET_OK
-
+#define RCL_RET_ERROR RMW_RET_ERROR
-typedef rmw_node_t rcl_node_t;
+#define RCL_RET_TIMEOUT RMW_RET_TIMEOUT
-typedef rmw_publisher_t rcl_publisher_t;
+// rcl specific ret codes start at 100
-typedef rmw_subscription_t rcl_subscription_t;
+#define RCL_RET_ALREADY_INIT 100
-typedef rmw_guard_condition_t rcl_guard_condition_t;
+#define RCL_RET_NOT_INIT 101
-typedef rmw_subscriptions_t rcl_subscriptions_t;
+#define RCL_RET_BAD_ALLOC 102
-typedef rmw_guard_conditions_t rcl_guard_conditions_t;
+#define RCL_RET_INVALID_ARGUMENT 103
-
+// rcl node specific ret codes in 2XX
-typedef struct rcl_callback_group_t
+#define RCL_RET_NODE_INVALID 200
-{
+// rcl publisher specific ret codes in 3XX
-  rcl_node_t ** node;
+#define RCL_RET_PUBLISHER_INVALID 300
-} rcl_callback_group_t;
+// rcl subscription specific ret codes in 4XX
-
+#define RCL_RET_SUBSCRIPTION_INVALID 400
-typedef struct rcl_subscription_info_t
+// rcl service client specific ret codes in 5XX
-{
+// rcl service server specific ret codes in 6XX
-  rcl_subscription_t ** subscription;
+// rcl guard condition specific ret codes in 7XX
-  rcl_callback_group_t ** callback_group;
+// rcl timer specific ret codes in 8XX
-} rcl_subscription_info_t;
+#define RCL_RET_TIMER_INVALID 800
-
+#define RCL_RET_TIMER_CANCELED 801
-typedef struct rcl_timer_info_t
+// rcl wait and wait set specific ret codes in 9XX
-{
+#define RCL_RET_WAIT_SET_INVALID 900
-  rcl_guard_condition_t ** guard_condition;
+#define RCL_RET_WAIT_SET_EMPTY 901
-  rcl_callback_group_t ** callback_group;
+#define RCL_RET_WAIT_SET_FULL 902
-} 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;
 
 #endif  // RCL__TYPES_H_

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_

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

rcl/package.xml

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

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

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 <stdlib.h>
+
+#if defined(WIN32)
+# define WINDOWS_ENV_BUFFER_SIZE 2048
+static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE];
+#endif  // defined(WIN32)
+
+rcl_ret_t
+rcl_impl_getenv(const char * env_name, const char ** env_value)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(env_name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(env_value, RCL_RET_INVALID_ARGUMENT);
+  *env_value = NULL;
+#if !defined(WIN32)
+  *env_value = getenv(env_name);
+  if (*env_value == NULL) {
+    *env_value = "";
+  }
+#else  // !defined(WIN32)
+  size_t required_size;
+  errno_t ret = getenv_s(&required_size, __env_buffer, sizeof(__env_buffer), env_name);
+  if (ret != 0) {
+    RCL_SET_ERROR_MSG("value in env variable too large to read in");
+    return RCL_RET_ERROR;
+  }
+  __env_buffer[WINDOWS_ENV_BUFFER_SIZE - 1] = '\0';
+  *env_value = __env_buffer;
+#endif  // !defined(WIN32)
+  return RCL_RET_OK;
+}
+
+#if __cplusplus
+}
+#endif

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_

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

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 <limits.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rcl/rcl.h"
+#include "rmw/rmw.h"
+
+#include "./common.h"
+
+typedef struct rcl_node_impl_t
+{
+  rcl_node_options_t options;
+  rmw_node_t * rmw_node_handle;
+  uint64_t rcl_instance_id;
+  size_t actual_domain_id;
+} rcl_node_impl_t;
+
+rcl_node_t
+rcl_get_zero_initialized_node()
+{
+  static rcl_node_t null_node = {0};
+  return null_node;
+}
+
+rcl_ret_t
+rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options)
+{
+  size_t domain_id = 0;
+  const char * ros_domain_id;
+  rcl_ret_t ret;
+  rcl_ret_t fail_ret = RCL_RET_ERROR;
+  RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  if (node->impl) {
+    RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized");
+    return RCL_RET_ALREADY_INIT;
+  }
+  // Make sure rcl has been initialized.
+  if (!rcl_ok()) {
+    RCL_SET_ERROR_MSG("rcl_init() has not been called");
+    return RCL_RET_NOT_INIT;
+  }
+  const rcl_allocator_t * allocator = &options->allocator;
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+  // Allocate space for the implementation struct.
+  node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state);
+  RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+  // Initialize node impl.
+  // node name
+  size_t name_len = strlen(name);
+  if (name_len == 0) {
+    RCL_SET_ERROR_MSG("node name cannot be empty string");
+    goto fail;
+  }
+  // node options (assume it is trivially copyable)
+  node->impl->options = *options;
+  // node rmw_node_handle
+  if (node->impl->options.domain_id == RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID) {
+    // Find the domain ID set by the environment.
+    ret = rcl_impl_getenv("ROS_DOMAIN_ID", &ros_domain_id);
+    if (ret != RCL_RET_OK) {
+      goto fail;
+    }
+    if (ros_domain_id) {
+      unsigned long number = strtoul(ros_domain_id, NULL, 0);  // NOLINT(runtime/int)
+      if (number == ULONG_MAX) {
+        RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number");
+        goto fail;
+      }
+      domain_id = (size_t)number;
+    }
+  } else {
+    domain_id = node->impl->options.domain_id;
+  }
+  node->impl->actual_domain_id = domain_id;
+  node->impl->rmw_node_handle = rmw_create_node(name, domain_id);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail);
+  node->impl->rcl_instance_id = rcl_get_instance_id();
+  return RCL_RET_OK;
+fail:
+  if (node->impl) {
+    allocator->deallocate(node->impl, allocator->state);
+  }
+  return fail_ret;
+}
+
+rcl_ret_t
+rcl_node_fini(rcl_node_t * node)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  if (!node->impl) {
+    // Repeat calls to fini or calling fini on a zero initialized node is ok.
+    return RCL_RET_OK;
+  }
+  rcl_ret_t result = RCL_RET_OK;
+  rmw_ret_t ret = rmw_destroy_node(node->impl->rmw_node_handle);
+  if (ret != RMW_RET_OK) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    result = RCL_RET_ERROR;
+  }
+  rcl_allocator_t allocator = node->impl->options.allocator;
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+  allocator.deallocate(node->impl, allocator.state);
+  node->impl = NULL;
+  return result;
+}
+
+rcl_node_options_t
+rcl_node_get_default_options()
+{
+  static rcl_node_options_t default_options = {
+    .domain_id = RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID,
+  };
+  // Must set the allocator after because it is not a compile time constant.
+  default_options.allocator = rcl_get_default_allocator();
+  return default_options;
+}
+
+const char *
+rcl_node_get_name(const rcl_node_t * node)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
+  if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
+    RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
+    return NULL;
+  }
+  return node->impl->rmw_node_handle->name;
+}
+
+const rcl_node_options_t *
+rcl_node_get_options(const rcl_node_t * node)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
+  if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
+    RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
+    return NULL;
+  }
+  return &node->impl->options;
+}
+
+rcl_ret_t
+rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    node->impl, "node implementation is invalid", return RCL_RET_NODE_INVALID);
+  if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
+    RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
+    return RCL_RET_NODE_INVALID;
+  }
+  *domain_id = node->impl->actual_domain_id;
+  return RCL_RET_OK;
+}
+
+rmw_node_t *
+rcl_node_get_rmw_handle(const rcl_node_t * node)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL);
+  if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
+    RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
+    return NULL;
+  }
+  return node->impl->rmw_node_handle;
+}
+
+uint64_t
+rcl_node_get_rcl_instance_id(const rcl_node_t * node)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, 0);
+  RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return 0);
+  return node->impl->rcl_instance_id;
+}
+
+#if __cplusplus
+}
+#endif

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 <string.h>
+
+#include "./common.h"
+#include "rmw/rmw.h"
+
+typedef struct rcl_publisher_impl_t
+{
+  rcl_publisher_options_t options;
+  rmw_publisher_t * rmw_handle;
+} rcl_publisher_impl_t;
+
+rcl_publisher_t
+rcl_get_zero_initialized_publisher()
+{
+  static rcl_publisher_t null_publisher = {0};
+  return null_publisher;
+}
+
+rcl_ret_t
+rcl_publisher_init(
+  rcl_publisher_t * publisher,
+  const rcl_node_t * node,
+  const rosidl_message_type_support_t * type_support,
+  const char * topic_name,
+  const rcl_publisher_options_t * options)
+{
+  rcl_ret_t fail_ret = RCL_RET_ERROR;
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+  if (publisher->impl) {
+    RCL_SET_ERROR_MSG("publisher already initialized, or memory was unintialized");
+    return RCL_RET_ALREADY_INIT;
+  }
+  const rcl_allocator_t * allocator = &options->allocator;
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+  // Allocate space for the implementation struct.
+  publisher->impl = (rcl_publisher_impl_t *)allocator->allocate(
+    sizeof(rcl_publisher_impl_t), allocator->state);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+  // Fill out implementation struct.
+  // rmw handle (create rmw publisher)
+  // TODO(wjwwood): pass along the allocator to rmw when it supports it
+  publisher->impl->rmw_handle = rmw_create_publisher(
+    rcl_node_get_rmw_handle(node),
+    type_support,
+    topic_name,
+    &rmw_qos_profile_default);
+  if (!publisher->impl->rmw_handle) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    goto fail;
+  }
+  // options
+  publisher->impl->options = *options;
+  return RCL_RET_OK;
+fail:
+  if (publisher->impl) {
+    allocator->deallocate(publisher->impl, allocator->state);
+  }
+  return fail_ret;
+}
+
+rcl_ret_t
+rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
+{
+  rcl_ret_t result = RCL_RET_OK;
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  if (publisher->impl) {
+    rmw_ret_t ret =
+      rmw_destroy_publisher(rcl_node_get_rmw_handle(node), publisher->impl->rmw_handle);
+    if (ret != RMW_RET_OK) {
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      result = RCL_RET_ERROR;
+    }
+    rcl_allocator_t allocator = publisher->impl->options.allocator;
+    allocator.deallocate(publisher->impl, allocator.state);
+  }
+  return result;
+}
+
+rcl_publisher_options_t
+rcl_publisher_get_default_options()
+{
+  static rcl_publisher_options_t default_options;
+  // Must set the allocator and qos after because they are not a compile time constant.
+  default_options.qos = rmw_qos_profile_default;
+  default_options.allocator = rcl_get_default_allocator();
+  return default_options;
+}
+
+rcl_ret_t
+rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "publisher is invalid", return RCL_RET_PUBLISHER_INVALID);
+  if (rmw_publish(publisher->impl->rmw_handle, ros_message) != RMW_RET_OK) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    return RCL_RET_ERROR;
+  }
+  return RCL_RET_OK;
+}
+
+const char *
+rcl_publisher_get_topic_name(const rcl_publisher_t * publisher)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "publisher is invalid", return NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl->rmw_handle, "publisher is invalid", return NULL);
+  return publisher->impl->rmw_handle->topic_name;
+}
+
+const rcl_publisher_options_t *
+rcl_publisher_get_options(const rcl_publisher_t * publisher)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "publisher is invalid", return NULL);
+  return &publisher->impl->options;
+}
+
+rmw_publisher_t *
+rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "publisher is invalid", return NULL);
+  return publisher->impl->rmw_handle;
+}
+
+#if __cplusplus
+}
+#endif

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

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 <stdatomic.h>
+#endif
+
+#define rcl_atomic_load(object, out) (out) = atomic_load(object)
+
+#define rcl_atomic_compare_exchange_strong(object, out, expected, desired) \
+  (out) = atomic_compare_exchange_strong(object, expected, desired)
+
+#define rcl_atomic_exchange(object, out, desired) (out) = atomic_exchange(object, desired)
+
+#define rcl_atomic_store(object, desired) atomic_store(object, desired)
+
+#else  // !defined(WIN32)
+
+#include "./stdatomic_helper/win32/stdatomic.h"
+
+#define rcl_atomic_load(object, out) rcl_win32_atomic_load(object, out)
+
+#define rcl_atomic_compare_exchange_strong(object, out, expected, desired) \
+  rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired)
+
+#define rcl_atomic_exchange(object, out, desired) rcl_win32_atomic_exchange(object, out, desired)
+
+#define rcl_atomic_store(object, desired) rcl_win32_atomic_store(object, desired)
+
+#endif  // !defined(WIN32)
+
+static inline bool
+rcl_atomic_load_bool(atomic_bool * a_bool)
+{
+  bool result = false;
+  rcl_atomic_load(a_bool, result);
+  return result;
+}
+
+static inline uint64_t
+rcl_atomic_load_uint64_t(atomic_uint_least64_t * a_uint64_t)
+{
+  uint64_t result = 0;
+  rcl_atomic_load(a_uint64_t, result);
+  return result;
+}
+
+static inline uintptr_t
+rcl_atomic_load_uintptr_t(atomic_uintptr_t * a_uintptr_t)
+{
+  uintptr_t result = 0;
+  rcl_atomic_load(a_uintptr_t, result);
+  return result;
+}
+
+static inline bool
+rcl_atomic_compare_exchange_strong_uint_least64_t(
+  atomic_uint_least64_t * a_uint_least64_t, uint64_t * expected, uint64_t desired)
+{
+  bool result;
+  rcl_atomic_compare_exchange_strong(a_uint_least64_t, result, expected, desired);
+  return result;
+}
+
+static inline bool
+rcl_atomic_exchange_bool(atomic_bool * a_bool, bool desired)
+{
+  bool result;
+  rcl_atomic_exchange(a_bool, result, desired);
+  return result;
+}
+
+static inline uint64_t
+rcl_atomic_exchange_uint64_t(atomic_uint_least64_t * a_uint64_t, uint64_t desired)
+{
+  uint64_t result;
+  rcl_atomic_exchange(a_uint64_t, result, desired);
+  return result;
+}
+
+static inline uint64_t
+rcl_atomic_exchange_uintptr_t(atomic_uintptr_t * a_uintptr_t, uintptr_t desired)
+{
+  uintptr_t result;
+  rcl_atomic_exchange(a_uintptr_t, result, desired);
+  return result;
+}
+
+#endif  // RCL__STDATOMIC_HELPER_H_

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

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

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

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)

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

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

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

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

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=$<TARGET_FILE:${PROJECT_NAME}_memory_tools_interpose>)
+endif()
+add_library(${PROJECT_NAME}_memory_tools SHARED memory_tools.cpp)
+if(NOT WIN32)
+  set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11")
+endif()
+if(UNIX AND NOT APPLE)
+  list(APPEND extra_test_libraries dl)
+  list(APPEND extra_memory_tools_env DL_PRELOAD=$<TARGET_FILE:${PROJECT_NAME}_memory_tools>)
+endif()
+target_link_libraries(${PROJECT_NAME}_memory_tools ${extra_test_libraries})
+target_compile_definitions(${PROJECT_NAME}_memory_tools
+  PRIVATE "RCL_MEMORY_TOOLS_BUILDING_DLL")
+list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools)
+
+if(NOT WIN32)
+  ament_add_gtest(test_memory_tools test_memory_tools.cpp ENV ${extra_memory_tools_env})
+  if(TARGET test_memory_tools)
+    target_include_directories(test_memory_tools PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+    )
+    if(NOT WIN32)
+      set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11")
+    endif()
+    target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries})
+  endif()
+endif()
+
+ament_add_gtest(test_allocator rcl/test_allocator.cpp ENV ${extra_memory_tools_env})
+if(TARGET test_allocator)
+  target_include_directories(test_allocator PUBLIC
+    ${rcl_interfaces_INCLUDE_DIRS}
+    ${rmw_INCLUDE_DIRS}
+  )
+  if(NOT WIN32)
+    set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11")
+  endif()
+  target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries})
+endif()
+
+ament_add_gtest(test_time rcl/test_time.cpp ENV ${extra_memory_tools_env})
+if(TARGET test_time)
+  target_include_directories(test_time PUBLIC
+    ${rcl_interfaces_INCLUDE_DIRS}
+    ${rmw_INCLUDE_DIRS}
+  )
+  if(NOT WIN32)
+    set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11")
+  endif()
+  target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries})
+endif()
+
+ament_add_gtest(test_common
+  rcl/test_common.cpp
+  ENV
+    ${extra_memory_tools_env}
+    EMPTY_TEST=
+    NORMAL_TEST=foo
+)
+if(TARGET test_common)
+  target_include_directories(test_common PUBLIC
+    ${rcl_interfaces_INCLUDE_DIRS}
+    ${rmw_INCLUDE_DIRS}
+  )
+  if(NOT WIN32)
+    set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11")
+  endif()
+  target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries})
+endif()
+
+ament_add_gtest(test_rcl rcl/test_rcl.cpp ENV ${extra_memory_tools_env})
+if(TARGET test_rcl)
+  target_include_directories(test_rcl PUBLIC
+    ${rcl_interfaces_INCLUDE_DIRS}
+    ${rmw_INCLUDE_DIRS}
+  )
+  if(NOT WIN32)
+    set_target_properties(test_rcl PROPERTIES COMPILE_FLAGS "-std=c++11")
+  endif()
+  target_link_libraries(test_rcl ${PROJECT_NAME} ${extra_test_libraries})
+endif()
+
+ament_add_gtest(test_node rcl/test_node.cpp ENV ${extra_memory_tools_env})
+if(TARGET test_node)
+  target_include_directories(test_node PUBLIC
+    ${rcl_interfaces_INCLUDE_DIRS}
+    ${rmw_INCLUDE_DIRS}
+  )
+  if(NOT WIN32)
+    set_target_properties(test_node PROPERTIES COMPILE_FLAGS "-std=c++11")
+  endif()
+  target_link_libraries(test_node ${PROJECT_NAME} ${extra_test_libraries})
+endif()

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

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 <stddef.h>
+
+#include <functional>
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RCL_MEMORY_TOOLS_EXPORT __attribute__ ((dllexport))
+    #define RCL_MEMORY_TOOLS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RCL_MEMORY_TOOLS_EXPORT __declspec(dllexport)
+    #define RCL_MEMORY_TOOLS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RCL_MEMORY_TOOLS_BUILDING_DLL
+    #define RCL_MEMORY_TOOLS_PUBLIC RCL_MEMORY_TOOLS_EXPORT
+  #else
+    #define RCL_MEMORY_TOOLS_PUBLIC RCL_MEMORY_TOOLS_IMPORT
+  #endif
+  #define RCL_MEMORY_TOOLS_PUBLIC_TYPE RCL_MEMORY_TOOLS_PUBLIC
+  #define RCL_MEMORY_TOOLS_LOCAL
+#else
+  #define RCL_MEMORY_TOOLS_EXPORT __attribute__ ((visibility("default")))
+  #define RCL_MEMORY_TOOLS_IMPORT
+  #if __GNUC__ >= 4
+    #define RCL_MEMORY_TOOLS_PUBLIC __attribute__ ((visibility("default")))
+    #define RCL_MEMORY_TOOLS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RCL_MEMORY_TOOLS_PUBLIC
+    #define RCL_MEMORY_TOOLS_LOCAL
+  #endif
+  #define RCL_MEMORY_TOOLS_PUBLIC_TYPE
+#endif
+
+typedef std::function<void ()> UnexpectedCallbackType;
+
+RCL_MEMORY_TOOLS_PUBLIC
+void
+start_memory_checking();
+
+#define ASSERT_NO_MALLOC(statements) \
+  assert_no_malloc_begin(); statements; assert_no_malloc_end();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+assert_no_malloc_begin();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+assert_no_malloc_end();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+set_on_unexpected_malloc_callback(UnexpectedCallbackType callback);
+
+#define ASSERT_NO_REALLOC(statements) \
+  assert_no_realloc_begin(); statements; assert_no_realloc_end();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+assert_no_realloc_begin();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+assert_no_realloc_end();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+set_on_unexpected_realloc_callback(UnexpectedCallbackType callback);
+
+#define ASSERT_NO_FREE(statements) \
+  assert_no_free_begin(); statements; assert_no_free_end();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+assert_no_free_begin();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+assert_no_free_end();
+RCL_MEMORY_TOOLS_PUBLIC
+void
+set_on_unexpected_free_callback(UnexpectedCallbackType callback);
+
+RCL_MEMORY_TOOLS_PUBLIC
+void
+stop_memory_checking();
+
+RCL_MEMORY_TOOLS_PUBLIC
+void
+memory_checking_thread_init();
+
+#endif  // MEMORY_TOOLS_HPP_

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

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<void *>(newf), \
+    reinterpret_cast<void *>(oldf), \
+  }
+
+// End Interpose.
+
+#include "./memory_tools_common.cpp"
+
+void osx_start_memory_checking()
+{
+  // No loading required, it is handled by DYLD_INSERT_LIBRARIES and dynamic library interposing.
+  MALLOC_PRINTF("starting memory checking...\n");
+  enabled.store(true);
+}
+
+void osx_stop_memory_checking()
+{
+  MALLOC_PRINTF("stopping memory checking...\n");
+  enabled.store(false);
+}
+
+OSX_INTERPOSE(custom_malloc, malloc);
+OSX_INTERPOSE(custom_realloc, realloc);
+OSX_INTERPOSE(custom_free, free);

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

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

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

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

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

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

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