Store the node pointer in publisher and subscription

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

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.
  *
  * <hr>
@@ -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

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.

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

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_

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

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_

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,13 +68,24 @@ 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()
   {
     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;

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,13 +73,24 @@ 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()
   {
     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);