diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index c39f5cfc5..caa721761 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -180,6 +180,7 @@ rcl_publisher_init( * \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_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 28bbf1713..2b06c9805 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. * *
@@ -192,6 +192,7 @@ rcl_subscription_init( * \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_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 47582da3d..646a63375 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_INCORRECT_NODE; + } 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..07f088d8c 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_INCORRECT_NODE; + } 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..1b7789de0 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_INCORRECT_NODE, 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..40db4a42f 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_INCORRECT_NODE, 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);