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