diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 9850810..60b75c1 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -43,7 +43,7 @@ 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; + // 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 @@ -93,7 +93,10 @@ rcl_get_zero_initialized_node(); * 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 @@ -119,7 +122,10 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o * 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 @@ -146,6 +152,10 @@ rcl_node_get_default_options(); * 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 */ @@ -164,6 +174,10 @@ rcl_node_get_name(const rcl_node_t * node); * 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 */ @@ -184,6 +198,10 @@ rcl_node_get_options(const rcl_node_t * node); * 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 @@ -209,6 +227,9 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); * 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 @@ -230,6 +251,10 @@ rcl_node_get_rmw_handle(const rcl_node_t * node); * 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 */ diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 0257473..4b42d39 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -80,7 +80,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); * - No new work (executing callbacks) will be done in executors. * - Currently running work in executors will be finished. * - * This function does not allocate heap memory. + * 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. diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index e27c5e9..ccb1201 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -33,10 +33,11 @@ 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_uninitialized_node() +rcl_get_zero_initialized_node() { static rcl_node_t null_node = {0}; return null_node; @@ -56,6 +57,11 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o 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); @@ -71,23 +77,27 @@ 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 options + // node options (assume it is trivially copyable) node->impl->options = *options; // node rmw_node_handle - // First determine the ROS_DOMAIN_ID. - // The result of rcl_impl_getenv on Windows is only valid until the next call to rcl_impl_getenv. - 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"); + 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; } - domain_id = (size_t)number; + 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); @@ -118,6 +128,7 @@ rcl_node_fini(rcl_node_t * node) 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; } @@ -125,7 +136,6 @@ rcl_node_options_t rcl_node_get_default_options() { 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. @@ -157,6 +167,21 @@ rcl_node_get_options(const rcl_node_t * node) 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) { diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp new file mode 100644 index 0000000..2f2acdb --- /dev/null +++ b/rcl/test/rcl/test_node.cpp @@ -0,0 +1,307 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/rcl.h" +#include "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); + 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(); +}