From 827d59729f266d9449d8fcb9752707bbd052527d Mon Sep 17 00:00:00 2001
From: Tomoya Fujita <Tomoya.Fujita@sony.com>
Date: Thu, 21 Sep 2023 18:44:25 -0700
Subject: [PATCH] Decouple rosout publisher init from node init. (#1121)

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>
---
 rclpy/src/rclpy/node.cpp | 36 +++++++++++++++++++++++++++++-------
 1 file changed, 29 insertions(+), 7 deletions(-)

diff --git a/rclpy/src/rclpy/node.cpp b/rclpy/src/rclpy/node.cpp
index 50a00e000..c083dea36 100644
--- a/rclpy/src/rclpy/node.cpp
+++ b/rclpy/src/rclpy/node.cpp
@@ -17,6 +17,8 @@
 #include <rcl_action/rcl_action.h>
 #include <rcl/error_handling.h>
 #include <rcl/graph.h>
+#include <rcl/logging.h>
+#include <rcl/logging_rosout.h>
 #include <rcl/types.h>
 #include <rcl_interfaces/msg/parameter_type.h>
 #include <rcl_yaml_param_parser/parser.h>
@@ -439,9 +441,24 @@ Node::Node(
 
   rcl_node_ = std::shared_ptr<rcl_node_t>(
     new rcl_node_t,
-    [](rcl_node_t * node)
+    [enable_rosout](rcl_node_t * node)
     {
-      rcl_ret_t ret = rcl_node_fini(node);
+      rcl_ret_t ret;
+      {
+        rclpy::LoggingGuard scoped_logging_guard;
+        if (rcl_logging_rosout_enabled() && enable_rosout) {
+          ret = rcl_logging_rosout_fini_publisher_for_node(node);
+          if (ret != RCL_RET_OK) {
+            // Warning should use line number of the current stack frame
+            int stack_level = 1;
+            PyErr_WarnFormat(
+              PyExc_RuntimeWarning, stack_level, "Failed to fini rosout publisher: %s",
+              rcl_get_error_string().str);
+            rcl_reset_error();
+          }
+        }
+      }
+      ret = rcl_node_fini(node);
       if (RCL_RET_OK != ret) {
         // Warning should use line number of the current stack frame
         int stack_level = 1;
@@ -459,11 +476,8 @@ Node::Node(
   options.arguments = arguments;
   options.enable_rosout = enable_rosout;
 
-  {
-    rclpy::LoggingGuard scoped_logging_guard;
-    ret = rcl_node_init(
-      rcl_node_.get(), node_name, namespace_, context.rcl_ptr(), &options);
-  }
+  ret = rcl_node_init(
+    rcl_node_.get(), node_name, namespace_, context.rcl_ptr(), &options);
 
   if (RCL_RET_BAD_ALLOC == ret) {
     rcl_reset_error();
@@ -478,6 +492,14 @@ Node::Node(
   if (RCL_RET_OK != ret) {
     throw RCLError("error creating node");
   }
+
+  if (rcl_logging_rosout_enabled() && enable_rosout) {
+    rclpy::LoggingGuard scoped_logging_guard;
+    ret = rcl_logging_rosout_init_publisher_for_node(rcl_node_.get());
+    if (ret != RCL_RET_OK) {
+      throw RCLError("failed to initialize rosout publisher");
+    }
+  }
 }
 
 py::list