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);