feat(agnocastlib): add --enable-rosout-logs full support

Signed-off-by: PeterWrighten <peterwrighten@gmail.com>

Author: PeterWrighten

src/agnocastlib/include/agnocast/node/agnocast_context.hpp

@@ -26,8 +26,11 @@ class Context
     return parsed_arguments_.is_valid() ? parsed_arguments_.get() : nullptr;
   }
 
+  bool is_rosout_enabled() const { return enable_rosout_logs_; }
+
 private:
   bool initialized_ = false;
+  bool enable_rosout_logs_ = false;
   ParsedArguments parsed_arguments_;
 };
 

src/agnocastlib/include/agnocast/node/agnocast_rosout.hpp

@@ -0,0 +1,12 @@
+#pragma once
+
+namespace agnocast
+{
+class Node;
+
+// Sets up a global /rosout publisher and installs a custom rcutils output handler
+// that chains the existing handler and publishes log messages to /rosout.
+// Only the first call creates the publisher; subsequent calls are no-ops.
+void setup_rosout_handler(Node * node);
+
+}  // namespace agnocast

src/agnocastlib/src/node/agnocast_context.cpp

@@ -48,19 +48,22 @@ void Context::init(int argc, char const * const * argv)
     rcl_reset_error();
   }
 
-  // Apply --disable-stdout-logs if present within --ros-args scope.
-  // There is no public rcl API to extract this flag from rcl_arguments_t, so we scan argv directly.
-  // rcl_logging_configure() cannot be used as it would initialize spdlog and attempt to set up a
-  // rosout publisher (which requires rcl_node_t), neither of which exist in agnocast.
+  // Apply --disable-stdout-logs and detect --enable-rosout-logs within --ros-args scope.
+  // There is no public rcl API to extract these flags from rcl_arguments_t, so we scan argv
+  // directly. rcl_logging_configure() cannot be used as it would initialize spdlog and attempt to
+  // set up a rosout publisher (which requires rcl_node_t), neither of which exist in agnocast.
   bool in_ros_args = false;
   for (const auto & arg : args) {
     if (arg == "--ros-args") {
       in_ros_args = true;
     } else if (arg == "--") {
       in_ros_args = false;
-    } else if (in_ros_args && arg == "--disable-stdout-logs") {
-      rcutils_logging_set_output_handler(noop_log_output_handler);
-      break;
+    } else if (in_ros_args) {
+      if (arg == "--disable-stdout-logs") {
+        rcutils_logging_set_output_handler(noop_log_output_handler);
+      } else if (arg == "--enable-rosout-logs") {
+        enable_rosout_logs_ = true;
+      }
     }
   }
 

src/agnocastlib/src/node/agnocast_node.cpp

@@ -3,6 +3,7 @@
 #include "agnocast/agnocast_tracepoint_wrapper.h"
 #include "agnocast/node/agnocast_arguments.hpp"
 #include "agnocast/node/agnocast_context.hpp"
+#include "agnocast/node/agnocast_rosout.hpp"
 
 #include <rcl/time.h>
 
@@ -41,6 +42,13 @@ Node::Node(
   node_time_source_ = std::make_shared<node_interfaces::NodeTimeSource>(node_clock_, this);
 
   node_services_ = std::make_shared<node_interfaces::NodeServices>(node_base_);
+
+  // Set up rosout publisher and handler if --enable-rosout-logs was specified.
+  // Must be after node_base_, node_topics_, and node_parameters_ are initialized
+  // (required by the publisher constructor). Only the first Node triggers setup.
+  if (g_context.is_rosout_enabled()) {
+    setup_rosout_handler(this);
+  }
 }
 
 }  // namespace agnocast

src/agnocastlib/src/node/agnocast_rosout.cpp

@@ -0,0 +1,117 @@
+#include "agnocast/node/agnocast_rosout.hpp"
+
+#include "agnocast/agnocast_publisher.hpp"
+#include "agnocast/node/agnocast_node.hpp"
+
+#include <rcl_interfaces/msg/log.hpp>
+#include <rcutils/logging.h>
+
+#include <atomic>
+#include <cstdarg>
+#include <cstdio>
+#include <mutex>
+
+namespace agnocast
+{
+
+static Publisher<rcl_interfaces::msg::Log>::SharedPtr g_rosout_pub;
+static std::mutex g_rosout_mtx;
+static std::atomic<bool> g_in_handler{false};
+static std::atomic<bool> g_rosout_initialized{false};
+static rcutils_logging_output_handler_t g_original_handler = nullptr;
+
+static uint8_t severity_to_log_level(int severity)
+{
+  switch (severity) {
+    case RCUTILS_LOG_SEVERITY_DEBUG:
+      return rcl_interfaces::msg::Log::DEBUG;
+    case RCUTILS_LOG_SEVERITY_INFO:
+      return rcl_interfaces::msg::Log::INFO;
+    case RCUTILS_LOG_SEVERITY_WARN:
+      return rcl_interfaces::msg::Log::WARN;
+    case RCUTILS_LOG_SEVERITY_ERROR:
+      return rcl_interfaces::msg::Log::ERROR;
+    case RCUTILS_LOG_SEVERITY_FATAL:
+      return rcl_interfaces::msg::Log::FATAL;
+    default:
+      return rcl_interfaces::msg::Log::INFO;
+  }
+}
+
+static void rosout_output_handler(
+  const rcutils_log_location_t * location, int severity, const char * name,
+  rcutils_time_point_value_t timestamp, const char * format, va_list * args)
+{
+  // Chain the original handler (console or noop, depending on --disable-stdout-logs).
+  // Pass a copy because the handler may consume the va_list via vsnprintf.
+  if (g_original_handler) {
+    va_list args_for_original;
+    va_copy(args_for_original, *args);
+    g_original_handler(location, severity, name, timestamp, format, &args_for_original);
+    va_end(args_for_original);
+  }
+
+  // Recursion guard: publishing may trigger log calls internally
+  bool expected = false;
+  if (!g_in_handler.compare_exchange_strong(expected, true)) {
+    return;
+  }
+
+  {
+    std::lock_guard<std::mutex> lock(g_rosout_mtx);
+    if (g_rosout_pub) {
+      auto loaned_msg = g_rosout_pub->borrow_loaned_message();
+
+      loaned_msg->stamp.sec = static_cast<int32_t>(timestamp / 1000000000LL);
+      loaned_msg->stamp.nanosec = static_cast<uint32_t>(timestamp % 1000000000LL);
+      loaned_msg->level = severity_to_log_level(severity);
+      loaned_msg->name = (name != nullptr) ? name : "";
+
+      // Format the message string from va_list
+      va_list args_copy;
+      va_copy(args_copy, *args);
+      int len = vsnprintf(nullptr, 0, format, args_copy);
+      va_end(args_copy);
+      if (len >= 0) {
+        std::string msg_str(static_cast<size_t>(len), '\0');
+        va_copy(args_copy, *args);
+        vsnprintf(&msg_str[0], static_cast<size_t>(len) + 1, format, args_copy);
+        va_end(args_copy);
+        loaned_msg->msg = std::move(msg_str);
+      }
+
+      if (location != nullptr) {
+        loaned_msg->file = (location->file_name != nullptr) ? location->file_name : "";
+        loaned_msg->function = (location->function_name != nullptr) ? location->function_name : "";
+        loaned_msg->line = location->line_number;
+      }
+
+      g_rosout_pub->publish(std::move(loaned_msg));
+    }
+  }
+
+  g_in_handler.store(false);
+}
+
+void setup_rosout_handler(Node * node)
+{
+  // Only set up once per process, even with multiple nodes
+  bool expected = false;
+  if (!g_rosout_initialized.compare_exchange_strong(expected, true)) {
+    return;
+  }
+
+  auto pub = node->create_publisher<rcl_interfaces::msg::Log>(
+    "/rosout", rclcpp::QoS(100).reliable().transient_local());
+
+  {
+    std::lock_guard<std::mutex> lock(g_rosout_mtx);
+    g_rosout_pub = pub;
+  }
+
+  // Capture the current handler (console or noop) before replacing
+  g_original_handler = rcutils_logging_get_output_handler();
+  rcutils_logging_set_output_handler(rosout_output_handler);
+}
+
+}  // namespace agnocast