From 5710542e01b747fd88f258dad382d45aaaa33090 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 19 May 2022 18:56:38 +0200 Subject: [PATCH 1/2] Store the node pointer in publisher and subscription The rcl_publisher_fini() and rcl_subscription_fini() function now check that the correct node was given to it. Signed-off-by: Nikolai Morin --- rcl/include/rcl/publisher.h | 2 +- rcl/include/rcl/subscription.h | 4 ++-- rcl/src/rcl/publisher.c | 6 ++++++ rcl/src/rcl/publisher_impl.h | 1 + rcl/src/rcl/subscription.c | 6 ++++++ rcl/src/rcl/subscription_impl.h | 1 + rcl/test/rcl/test_publisher.cpp | 30 +++++++++++++++++++++++++++--- rcl/test/rcl/test_subscription.cpp | 17 +++++++++++++++++ 8 files changed, 61 insertions(+), 6 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index c39f5cfc5..9e0420498 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -179,7 +179,7 @@ rcl_publisher_init( * \return #RCL_RET_OK if publisher was finalized successfully, or * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or - * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_NODE_INVALID if the node is not the node used to create the publisher, or * \return #RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 28bbf1713..d4b9c5c0c 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -175,7 +175,7 @@ rcl_subscription_init( * * After calling, calls to rcl_wait and rcl_take will fail when using this * subscription. - * Additioanlly rcl_wait will be interrupted if currently blocking. + * Additionally, rcl_wait will be interrupted if currently blocking. * However, the given node handle is still valid. * *
@@ -191,7 +191,7 @@ rcl_subscription_init( * \return #RCL_RET_OK if subscription was deinitialized successfully, or * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or - * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_NODE_INVALID if the node is not the node used to create the subscription, or * \return #RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 47582da3d..d8069ad99 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -105,6 +105,8 @@ rcl_publisher_init( publisher->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); // Fill out implementation struct. + // node + publisher->impl->node = node; // rmw handle (create rmw publisher) // TODO(wjwwood): pass along the allocator to rmw when it supports it publisher->impl->rmw_handle = rmw_create_publisher( @@ -176,6 +178,10 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing publisher"); if (publisher->impl) { + if (node != publisher->impl->node) { + RCL_SET_ERROR_MSG("fini called with incorrect node"); + return RCL_RET_INVALID_ARGUMENT; + } rcl_allocator_t allocator = publisher->impl->options.allocator; rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); if (!rmw_node) { diff --git a/rcl/src/rcl/publisher_impl.h b/rcl/src/rcl/publisher_impl.h index cd5ff2324..4fc4da9a1 100644 --- a/rcl/src/rcl/publisher_impl.h +++ b/rcl/src/rcl/publisher_impl.h @@ -25,6 +25,7 @@ struct rcl_publisher_impl_s rmw_qos_profile_t actual_qos; rcl_context_t * context; rmw_publisher_t * rmw_handle; + const rcl_node_t * node; }; #endif // RCL__PUBLISHER_IMPL_H_ diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index fd5984ded..b51befe9b 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -96,6 +96,8 @@ rcl_subscription_init( RCL_CHECK_FOR_NULL_WITH_MSG( subscription->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); // Fill out the implemenation struct. + // node + subscription->impl->node = node; // rmw_handle // TODO(wjwwood): pass allocator once supported in rmw api. subscription->impl->rmw_handle = rmw_create_subscription( @@ -172,6 +174,10 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) return RCL_RET_NODE_INVALID; // error already set } if (subscription->impl) { + if (node != subscription->impl->node) { + RCL_SET_ERROR_MSG("fini called with incorrect node"); + return RCL_RET_INVALID_ARGUMENT; + } rcl_allocator_t allocator = subscription->impl->options.allocator; rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); if (!rmw_node) { diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 0fe962ab4..0f4e2af73 100644 --- a/rcl/src/rcl/subscription_impl.h +++ b/rcl/src/rcl/subscription_impl.h @@ -24,6 +24,7 @@ struct rcl_subscription_impl_s rcl_subscription_options_t options; rmw_qos_profile_t actual_qos; rmw_subscription_t * rmw_handle; + const rcl_node_t * node; }; #endif // RCL__SUBSCRIPTION_IMPL_H_ diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index c77e78436..2bb4f50db 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/rcl/test/rcl/test_publisher.cpp @@ -45,6 +45,7 @@ class CLASSNAME (TestPublisherFixture, RMW_IMPLEMENTATION) : public ::testing::T public: rcl_context_t * context_ptr; rcl_node_t * node_ptr; + rcl_node_t * different_node_ptr; void SetUp() { rcl_ret_t ret; @@ -67,6 +68,14 @@ class CLASSNAME (TestPublisherFixture, RMW_IMPLEMENTATION) : public ::testing::T rcl_node_options_t node_options = rcl_node_get_default_options(); ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->different_node_ptr = new rcl_node_t; + *this->different_node_ptr = rcl_get_zero_initialized_node(); + constexpr char different_name[] = "different_test_publisher_node"; + ret = rcl_node_init( + this->different_node_ptr, different_name, "", this->context_ptr, + &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } void TearDown() @@ -74,6 +83,9 @@ class CLASSNAME (TestPublisherFixture, RMW_IMPLEMENTATION) : public ::testing::T rcl_ret_t ret = rcl_node_fini(this->node_ptr); delete this->node_ptr; EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_fini(this->different_node_ptr); + delete this->different_node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_shutdown(this->context_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_context_fini(this->context_ptr); @@ -241,20 +253,32 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_init_ // Try init a publisher already init ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &default_publisher_options); EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; - ret = rcl_publisher_fini(&publisher, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); // Pass invalid node to fini ret = rcl_publisher_fini(&publisher, nullptr); EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; rcl_reset_error(); + // Pass a different node to fini + ret = rcl_publisher_fini(&publisher, this->different_node_ptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Pass nullptr publisher to fini ret = rcl_publisher_fini(nullptr, this->node_ptr); EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, ret) << rcl_get_error_string().str; rcl_reset_error(); + // Pass a valid publisher to fini + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Pass an already fini'd publisher to fini + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Try passing null for publisher in init. ret = rcl_publisher_init(nullptr, this->node_ptr, ts, topic_name, &default_publisher_options); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 46bd15213..5acf5d33c 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -50,6 +50,7 @@ class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing public: rcl_context_t * context_ptr; rcl_node_t * node_ptr; + rcl_node_t * different_node_ptr; void SetUp() { rcl_ret_t ret; @@ -72,6 +73,14 @@ class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing rcl_node_options_t node_options = rcl_node_get_default_options(); ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->different_node_ptr = new rcl_node_t; + *this->different_node_ptr = rcl_get_zero_initialized_node(); + constexpr char different_name[] = "different_test_subscription_node"; + ret = rcl_node_init( + this->different_node_ptr, different_name, "", this->context_ptr, + &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } void TearDown() @@ -79,6 +88,9 @@ class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing rcl_ret_t ret = rcl_node_fini(this->node_ptr); delete this->node_ptr; EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_fini(this->different_node_ptr); + delete this->different_node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_shutdown(this->context_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_context_fini(this->context_ptr); @@ -256,6 +268,11 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcl_reset_error(); EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_subscription_fini(&subscription, &invalid_node)); rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_subscription_fini( + &subscription, + this->different_node_ptr)); + rcl_reset_error(); auto mock = mocking_utils::inject_on_return("lib:rcl", rmw_destroy_subscription, RMW_RET_ERROR); From 0e4dce8afaaee2723c31276886936707a885abb5 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Sat, 4 Jun 2022 17:44:18 +0200 Subject: [PATCH 2/2] Return new error code RCL_RET_INCORRECT_NODE on node mismatch Signed-off-by: Nikolai Morin --- rcl/include/rcl/publisher.h | 3 ++- rcl/include/rcl/subscription.h | 3 ++- rcl/include/rcl/types.h | 2 ++ rcl/src/rcl/publisher.c | 2 +- rcl/src/rcl/subscription.c | 2 +- rcl/test/rcl/test_publisher.cpp | 2 +- rcl/test/rcl/test_subscription.cpp | 2 +- 7 files changed, 10 insertions(+), 6 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 9e0420498..caa721761 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -179,7 +179,8 @@ rcl_publisher_init( * \return #RCL_RET_OK if publisher was finalized successfully, or * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or - * \return #RCL_RET_NODE_INVALID if the node is not the node used to create the publisher, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INCORRECT_NODE if the node is not the node used to create the publisher, or * \return #RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index d4b9c5c0c..2b06c9805 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -191,7 +191,8 @@ rcl_subscription_init( * \return #RCL_RET_OK if subscription was deinitialized successfully, or * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or - * \return #RCL_RET_NODE_INVALID if the node is not the node used to create the subscription, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INCORRECT_NODE if the node is not the node used to create the subscription, or * \return #RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index 15ef8b5ae..450e1077f 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -60,6 +60,8 @@ typedef rmw_ret_t rcl_ret_t; #define RCL_RET_NODE_INVALID_NAMESPACE 202 /// Failed to find node name #define RCL_RET_NODE_NAME_NON_EXISTENT 203 +/// Incorrect rcl_node_t given +#define RCL_RET_INCORRECT_NODE 204 // rcl publisher specific ret codes in 3XX /// Invalid rcl_publisher_t given return code. diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index d8069ad99..646a63375 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -180,7 +180,7 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) if (publisher->impl) { if (node != publisher->impl->node) { RCL_SET_ERROR_MSG("fini called with incorrect node"); - return RCL_RET_INVALID_ARGUMENT; + return RCL_RET_INCORRECT_NODE; } rcl_allocator_t allocator = publisher->impl->options.allocator; rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index b51befe9b..07f088d8c 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -176,7 +176,7 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) if (subscription->impl) { if (node != subscription->impl->node) { RCL_SET_ERROR_MSG("fini called with incorrect node"); - return RCL_RET_INVALID_ARGUMENT; + return RCL_RET_INCORRECT_NODE; } rcl_allocator_t allocator = subscription->impl->options.allocator; rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index 2bb4f50db..1b7789de0 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/rcl/test/rcl/test_publisher.cpp @@ -261,7 +261,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_init_ // Pass a different node to fini ret = rcl_publisher_fini(&publisher, this->different_node_ptr); - EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_INCORRECT_NODE, ret) << rcl_get_error_string().str; rcl_reset_error(); // Pass nullptr publisher to fini diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 5acf5d33c..40db4a42f 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -269,7 +269,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_subscription_fini(&subscription, &invalid_node)); rcl_reset_error(); EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, rcl_subscription_fini( + RCL_RET_INCORRECT_NODE, rcl_subscription_fini( &subscription, this->different_node_ptr)); rcl_reset_error();