Decouple rosout publisher init from node init. (#1065)

Signed-off-by: Tomoya Fujita <[email protected]>
ros2 · Sep 22, 2023 · 1260197 · 1260197
1 parent 7b9c1ec
commit 1260197
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 17 deletions.
diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c
@@ -282,16 +282,6 @@ rcl_node_init(
     goto fail;
   }
 
-  // The initialization for the rosout publisher requires the node to be initialized to a point
-  // that it can create new topic publishers
-  if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) {
-    ret = rcl_logging_rosout_init_publisher_for_node(node);
-    if (ret != RCL_RET_OK) {
-      // error message already set
-      goto fail;
-    }
-  }
-
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized");
   TRACETOOLS_TRACEPOINT(
     rcl_node_init,
@@ -363,13 +353,6 @@ rcl_node_fini(rcl_node_t * node)
   rcl_allocator_t allocator = node->impl->options.allocator;
   rcl_ret_t result = RCL_RET_OK;
   rcl_ret_t rcl_ret = RCL_RET_OK;
-  if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) {
-    rcl_ret = rcl_logging_rosout_fini_publisher_for_node(node);
-    if (rcl_ret != RCL_RET_OK && rcl_ret != RCL_RET_NOT_INIT) {
-      RCL_SET_ERROR_MSG("Unable to fini publisher for node.");
-      result = RCL_RET_ERROR;
-    }
-  }
   rcl_ret = rcl_node_type_cache_fini(node);
   if (rcl_ret != RCL_RET_OK) {
     RCL_SET_ERROR_MSG("Unable to fini type cache for node.");

diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp
@@ -31,6 +31,7 @@
 #include "rcl/error_handling.h"
 #include "rcl/graph.h"
 #include "rcl/logging.h"
+#include "rcl/logging_rosout.h"
 #include "rcl/rcl.h"
 
 #include "rcutils/logging_macros.h"
@@ -99,6 +100,10 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test
     const char * name = "test_graph_node";
     ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    if (rcl_logging_rosout_enabled() && node_options.enable_rosout) {
+      ret = rcl_logging_rosout_init_publisher_for_node(this->node_ptr);
+      ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    }
 
     this->wait_set_ptr = new rcl_wait_set_t;
     *this->wait_set_ptr = rcl_get_zero_initialized_wait_set();
@@ -118,6 +123,11 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test
     delete this->wait_set_ptr;
     EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
+    const rcl_node_options_t * node_ops = rcl_node_get_options(this->node_ptr);
+    if (rcl_logging_rosout_enabled() && node_ops->enable_rosout) {
+      ret = rcl_logging_rosout_fini_publisher_for_node(this->node_ptr);
+      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    }
     ret = rcl_node_fini(this->node_ptr);
     delete this->node_ptr;
     EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
@@ -949,6 +959,11 @@ class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEME
       remote_node_ptr, remote_node_name, "", this->remote_context_ptr,
       &node_options);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    if (rcl_logging_rosout_enabled() && node_options.enable_rosout) {
+      ret = rcl_logging_rosout_init_publisher_for_node(remote_node_ptr);
+      ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    }
+
     sub_func = std::bind(
       rcl_get_subscriber_names_and_types_by_node,
       std::placeholders::_1,
@@ -986,6 +1001,11 @@ class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEME
   {
     rcl_ret_t ret;
     CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION) ::TearDown();
+    const rcl_node_options_t * node_ops = rcl_node_get_options(this->remote_node_ptr);
+    if (rcl_logging_rosout_enabled() && node_ops->enable_rosout) {
+      ret = rcl_logging_rosout_fini_publisher_for_node(this->remote_node_ptr);
+      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    }
     ret = rcl_node_fini(this->remote_node_ptr);
 
     delete this->remote_node_ptr;

diff --git a/rcl/test/rcl/test_logging_rosout.cpp b/rcl/test/rcl/test_logging_rosout.cpp
@@ -101,6 +101,10 @@ class TestLoggingRosout : public ::testing::Test
     ret = rcl_node_init(
       this->node_ptr, name, namespace_, this->context_ptr, &node_options);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    if (rcl_logging_rosout_enabled() && node_options.enable_rosout) {
+      ret = rcl_logging_rosout_init_publisher_for_node(this->node_ptr);
+      ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    }
 
     // create rosout subscription
     const rosidl_message_type_support_t * ts =
@@ -119,6 +123,10 @@ class TestLoggingRosout : public ::testing::Test
     rcl_ret_t ret = rcl_subscription_fini(this->subscription_ptr, this->node_ptr);
     delete this->subscription_ptr;
     EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    if (rcl_logging_rosout_enabled() && node_options.enable_rosout) {
+      ret = rcl_logging_rosout_fini_publisher_for_node(this->node_ptr);
+      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    }
     ret = rcl_node_fini(this->node_ptr);
     delete this->node_ptr;
     EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;