diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c index fcc0b9c982..cc89fca860 100644 --- a/rcl/src/rcl/graph.c +++ b/rcl/src/rcl/graph.c @@ -130,6 +130,9 @@ rcl_count_publishers( const char * topic_name, size_t * count) { + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } const rcl_node_options_t * node_options = rcl_node_get_options(node); if (!node_options) { return RCL_RET_NODE_INVALID; // shouldn't happen, but error is already set if so @@ -146,6 +149,9 @@ rcl_count_subscribers( const char * topic_name, size_t * count) { + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } const rcl_node_options_t * node_options = rcl_node_get_options(node); if (!node_options) { return RCL_RET_NODE_INVALID; // shouldn't happen, but error is already set if so @@ -162,6 +168,9 @@ rcl_service_server_is_available( const rcl_client_t * client, bool * is_available) { + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } const rcl_node_options_t * node_options = rcl_node_get_options(node); if (!node_options) { return RCL_RET_NODE_INVALID; // shouldn't happen, but error is already set if so diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 5249633aaa..c7e2610da8 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -51,6 +51,7 @@ bool is_connext = class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test { public: + rcl_context_t * old_context_ptr; rcl_context_t * context_ptr; rcl_node_t * old_node_ptr; rcl_node_t * node_ptr; @@ -64,20 +65,20 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; }); - this->context_ptr = new rcl_context_t; - *this->context_ptr = rcl_get_zero_initialized_context(); - ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + this->old_context_ptr = new rcl_context_t; + *this->old_context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->old_context_ptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; this->old_node_ptr = new rcl_node_t; *this->old_node_ptr = rcl_get_zero_initialized_node(); const char * old_name = "old_node_name"; rcl_node_options_t node_options = rcl_node_get_default_options(); - ret = rcl_node_init(this->old_node_ptr, old_name, "", this->context_ptr, &node_options); + ret = rcl_node_init(this->old_node_ptr, old_name, "", this->old_context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_shutdown(this->context_ptr); // after this, the old_node_ptr should be invalid - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_context_fini(this->context_ptr); + ret = rcl_shutdown(this->old_context_ptr); // after this, the old_node_ptr should be invalid EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->context_ptr = new rcl_context_t; *this->context_ptr = rcl_get_zero_initialized_context(); ret = rcl_init(0, nullptr, &init_options, this->context_ptr); @@ -112,6 +113,10 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_context_fini(this->context_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + delete this->context_ptr; + ret = rcl_context_fini(this->old_context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + delete this->old_context_ptr; } }; diff --git a/rcl/test/rcl/test_guard_condition.cpp b/rcl/test/rcl/test_guard_condition.cpp index a4c4c5c2e4..312ffb7704 100644 --- a/rcl/test/rcl/test_guard_condition.cpp +++ b/rcl/test/rcl/test_guard_condition.cpp @@ -178,10 +178,8 @@ TEST_F( options_with_failing_allocator.allocator.zero_allocate = failing_calloc; ret = rcl_guard_condition_init(&guard_condition, &context, options_with_failing_allocator); ASSERT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC"; - // The error will not be set because the allocator will not work. - // It should, however, print a message to the screen and get the bad alloc ret code. - // ASSERT_TRUE(rcl_error_is_set()); - // rcl_reset_error(); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); // Try fini with invalid arguments. ret = rcl_guard_condition_fini(nullptr); diff --git a/rcl/test/rcl/test_init.cpp b/rcl/test/rcl/test_init.cpp index 60ce7eefaa..9043501074 100644 --- a/rcl/test/rcl/test_init.cpp +++ b/rcl/test/rcl/test_init.cpp @@ -108,7 +108,7 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_ok_and_s rcl_context_t context = rcl_get_zero_initialized_context(); // A shutdown before any init has been called should fail. ret = rcl_shutdown(&context); - EXPECT_EQ(RCL_RET_NOT_INIT, ret); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); ASSERT_FALSE(rcl_context_is_valid(&context)); // If argc is not 0, but argv is, it should be an invalid argument. @@ -164,12 +164,17 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_ok_and_s ret = rcl_shutdown(&context); EXPECT_EQ(ret, RCL_RET_OK); ASSERT_FALSE(rcl_context_is_valid(&context)); + // Then a repeated shutdown should fail. + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_ALREADY_SHUTDOWN); + ASSERT_FALSE(rcl_context_is_valid(&context)); + rcl_reset_error(); ret = rcl_context_fini(&context); EXPECT_EQ(ret, RCL_RET_OK); context = rcl_get_zero_initialized_context(); // A repeat call to shutdown should not work. ret = rcl_shutdown(&context); - EXPECT_EQ(RCL_RET_NOT_INIT, ret); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); ASSERT_FALSE(rcl_context_is_valid(&context)); // Repeat, but valid, calls to rcl_init() should fail. @@ -205,6 +210,7 @@ TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_get_instance_id_a EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); EXPECT_EQ(0u, rcl_context_get_instance_id(&context)); ASSERT_FALSE(rcl_context_is_valid(&context)); + rcl_reset_error(); // A non-zero instance id should be returned after a valid init. rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index b923f45476..0a5d2f95a5 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -92,8 +92,8 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; }); - rcl_context_t context = rcl_get_zero_initialized_context(); - ret = rcl_init(0, nullptr, &init_options, &context); + rcl_context_t invalid_context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &invalid_context); 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(); @@ -101,13 +101,13 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) const char * namespace_ = "/ns"; 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, namespace_, &context, &default_options); + ret = rcl_node_init(&invalid_node, name, namespace_, &invalid_context, &default_options); if (is_windows && is_opensplice) { // On Windows with OpenSplice, setting the domain id is not expected to work. ASSERT_NE(RCL_RET_OK, ret); // So retry with the default domain id setting (uses the environment as is). default_options.domain_id = rcl_node_get_default_options().domain_id; - ret = rcl_node_init(&invalid_node, name, namespace_, &context, &default_options); + ret = rcl_node_init(&invalid_node, name, namespace_, &invalid_context, &default_options); ASSERT_EQ(RCL_RET_OK, ret); } else { // This is the normal check (not windows and windows if not opensplice) @@ -118,11 +118,12 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) rcl_ret_t ret = rcl_node_fini(&invalid_node); EXPECT_EQ(RCL_RET_OK, ret); }); - ret = rcl_shutdown(&context); // Shutdown to invalidate the node. - ASSERT_EQ(RCL_RET_OK, ret); - ret = rcl_context_fini(&context); + ret = rcl_shutdown(&invalid_context); // Shutdown to invalidate the node. ASSERT_EQ(RCL_RET_OK, ret); - context = rcl_get_zero_initialized_context(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&invalid_context)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); ret = rcl_init(0, nullptr, &init_options, &context); ASSERT_EQ(RCL_RET_OK, ret); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ @@ -149,9 +150,15 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) is_valid = rcl_node_is_valid(&zero_node); EXPECT_FALSE(is_valid); rcl_reset_error(); + + // invalid node will be true for rcl_node_is_valid_except_context, but false for valid only + is_valid = rcl_node_is_valid_except_context(&invalid_node); + EXPECT_TRUE(is_valid); + rcl_reset_error(); is_valid = rcl_node_is_valid(&invalid_node); EXPECT_FALSE(is_valid); rcl_reset_error(); + is_valid = rcl_node_is_valid(&node); EXPECT_TRUE(is_valid); rcl_reset_error(); @@ -164,14 +171,17 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) EXPECT_EQ(nullptr, actual_node_name); rcl_reset_error(); actual_node_name = rcl_node_get_name(&invalid_node); - EXPECT_EQ(nullptr, actual_node_name); + EXPECT_TRUE(actual_node_name ? true : false); + if (actual_node_name) { + EXPECT_STREQ(name, actual_node_name); + } rcl_reset_error(); EXPECT_NO_MEMORY_OPERATIONS({ actual_node_name = rcl_node_get_name(&node); }); EXPECT_TRUE(actual_node_name ? true : false); if (actual_node_name) { - EXPECT_EQ(std::string(name), std::string(actual_node_name)); + EXPECT_STREQ(name, actual_node_name); } // Test rcl_node_get_namespace(). const char * actual_node_namespace; @@ -182,7 +192,10 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) EXPECT_EQ(nullptr, actual_node_namespace); rcl_reset_error(); actual_node_namespace = rcl_node_get_namespace(&invalid_node); - EXPECT_EQ(nullptr, actual_node_namespace); + EXPECT_TRUE(actual_node_namespace ? true : false); + if (actual_node_namespace) { + EXPECT_STREQ(namespace_, actual_node_namespace); + } rcl_reset_error(); EXPECT_NO_MEMORY_OPERATIONS({ actual_node_namespace = rcl_node_get_namespace(&node); @@ -200,7 +213,10 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) EXPECT_EQ(nullptr, actual_node_logger_name); rcl_reset_error(); actual_node_logger_name = rcl_node_get_logger_name(&invalid_node); - EXPECT_EQ(nullptr, actual_node_logger_name); + EXPECT_TRUE(actual_node_logger_name ? true : false); + if (actual_node_logger_name) { + EXPECT_EQ("ns." + std::string(name), std::string(actual_node_logger_name)); + } rcl_reset_error(); EXPECT_NO_MEMORY_OPERATIONS({ actual_node_logger_name = rcl_node_get_logger_name(&node); @@ -218,7 +234,11 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) EXPECT_EQ(nullptr, actual_options); rcl_reset_error(); actual_options = rcl_node_get_options(&invalid_node); - EXPECT_EQ(nullptr, actual_options); + 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); + } rcl_reset_error(); EXPECT_NO_MEMORY_OPERATIONS({ actual_options = rcl_node_get_options(&node); @@ -239,8 +259,7 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) ASSERT_TRUE(rcl_error_is_set()); rcl_reset_error(); ret = rcl_node_get_domain_id(&invalid_node, &actual_domain_id); - EXPECT_EQ(RCL_RET_NODE_INVALID, ret); - ASSERT_TRUE(rcl_error_is_set()); + EXPECT_EQ(RCL_RET_OK, ret); rcl_reset_error(); EXPECT_NO_MEMORY_OPERATIONS({ ret = rcl_node_get_domain_id(&node, &actual_domain_id); @@ -259,7 +278,7 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) EXPECT_EQ(nullptr, node_handle); rcl_reset_error(); node_handle = rcl_node_get_rmw_handle(&invalid_node); - EXPECT_EQ(nullptr, node_handle); + EXPECT_NE(nullptr, node_handle); rcl_reset_error(); EXPECT_NO_MEMORY_OPERATIONS({ node_handle = rcl_node_get_rmw_handle(&node); @@ -274,8 +293,7 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) EXPECT_EQ(0u, instance_id); rcl_reset_error(); instance_id = rcl_node_get_rcl_instance_id(&invalid_node); - EXPECT_NE(0u, instance_id); - EXPECT_NE(42u, instance_id); + EXPECT_EQ(0u, instance_id); rcl_reset_error(); EXPECT_NO_MEMORY_OPERATIONS({ instance_id = rcl_node_get_rcl_instance_id(&node); @@ -290,7 +308,7 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) EXPECT_EQ(nullptr, graph_guard_condition); rcl_reset_error(); graph_guard_condition = rcl_node_get_graph_guard_condition(&invalid_node); - EXPECT_EQ(nullptr, graph_guard_condition); + EXPECT_NE(nullptr, graph_guard_condition); rcl_reset_error(); EXPECT_NO_MEMORY_OPERATIONS({ graph_guard_condition = rcl_node_get_graph_guard_condition(&node); @@ -361,10 +379,8 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_life_cycle) options_with_failing_allocator.allocator.reallocate = failing_realloc; ret = rcl_node_init(&node, name, namespace_, &context, &options_with_failing_allocator); EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC"; - // The error will not be set because the allocator will not work. - // It should, however, print a message to the screen and get the bad alloc ret code. - // ASSERT_TRUE(rcl_error_is_set()); - // rcl_reset_error(); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); // Try fini with invalid arguments. ret = rcl_node_fini(nullptr);