use parent logger

rcl/include/rcl/logging_rosout.h

@@ -188,6 +188,69 @@ rcl_logging_rosout_output_handler(
   const char * format,
   va_list * args);
 
+/// Add a subordinate logger based on a logger
+/**
+ * Calling this will use the existing publisher of `logger_name` on a node to create an subordinate
+ * logger that will be used by the logging system to publish all log messages from that Node's
+ * logger.
+ *
+ * If a subordinate logger already exists, it will NOT be created.
+ *
+ * It is expected that after creating a subordinate logger with this function
+ * rcl_logging_rosout_remove_sublogger() will be called for the node to cleanup
+ * the subordinate logger while the publisher of `logger_name` is still valid.
+ *
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] logger_name a logger_name that has a corresponding rosout publisher on a node
+ * \param[in] sublogger_name a sublogger name
+ * \return #RCL_RET_OK if the subordinate logger was created successfully, or
+ * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCL_RET_SUBLOGGER_ALREADY_EXIST if the subordinate logger already exists, or
+ * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or
+ * \return #RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_logging_rosout_add_sublogger(
+  const char * logger_name, const char * sublogger_name);
+
+/// Remove a subordinate logger and cleans up allocated resources
+/**
+ * Calling this will destroy the subordinate logger based on
+ * `logger_name+RCUTILS_LOGGING_SEPARATOR_STRING+sublogger_name` on that node and remove it from
+ * the logging system so that no more Log messages are published to this function.
+ *
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] logger_name a logger_name that has a corresponding rosout publisher on a node
+ * \param[in] sublogger_name a sublogger name
+ * \return #RCL_RET_OK if the subordinate logger was finalized successfully, or
+ * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or
+ * \return #RCL_RET_ERROR if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_logging_rosout_remove_sublogger(
+  const char * logger_name, const char * sublogger_name);
+
 #ifdef __cplusplus
 }
 #endif

rcl/include/rcl/types.h

@@ -50,6 +50,8 @@ typedef rmw_ret_t rcl_ret_t;
 #define RCL_RET_UNKNOWN_SUBSTITUTION 105
 /// rcl_shutdown() already called return code.
 #define RCL_RET_ALREADY_SHUTDOWN 106
+/// sublogger already exist.
+#define RCL_RET_SUBLOGGER_ALREADY_EXIST 107
 
 // rcl node specific ret codes in 2XX
 /// Invalid rcl_node_t given return code.

rcl/src/rcl/logging_rosout.c

@@ -22,6 +22,7 @@
 #include "rcl/visibility_control.h"
 #include "rcl_interfaces/msg/log.h"
 #include "rcutils/allocator.h"
+#include "rcutils/format_string.h"
 #include "rcutils/logging_macros.h"
 #include "rcutils/macros.h"
 #include "rcutils/types/hash_map.h"
@@ -74,6 +75,7 @@ typedef struct rosout_map_entry_t
 static rcutils_hash_map_t __logger_map;
 static bool __is_initialized = false;
 static rcl_allocator_t __rosout_allocator;
+static rcutils_array_list_t __sublogger_names;
 
 rcl_ret_t rcl_logging_rosout_init(const rcl_allocator_t * allocator)
 {
@@ -87,13 +89,52 @@ rcl_ret_t rcl_logging_rosout_init(const rcl_allocator_t * allocator)
     rcutils_hash_map_init(
       &__logger_map, 2, sizeof(const char *), sizeof(rosout_map_entry_t),
       rcutils_hash_map_string_hash_func, rcutils_hash_map_string_cmp_func, allocator));
+  if (RCL_RET_OK != status) {
+    return status;
+  }
+
+  __sublogger_names = rcutils_get_zero_initialized_array_list();
+  status = rcl_ret_from_rcutils_ret(
+    rcutils_array_list_init(
+      &__sublogger_names, 2, sizeof(const char **), allocator));
+  if (RCL_RET_OK != status) {
+    status = rcl_ret_from_rcutils_ret(rcutils_hash_map_fini(&__logger_map));
+    return status;
+  }
+
   if (RCL_RET_OK == status) {
     __rosout_allocator = *allocator;
     __is_initialized = true;
   }
   return status;
 }
 
+static rcl_ret_t
+_rcl_logging_rosout_remove_logger_map(rcl_node_t * node)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+
+  rcl_ret_t status = RCL_RET_OK;
+  char * previous_key = NULL;
+  char * key = NULL;
+  rosout_map_entry_t entry;
+  rcutils_ret_t hashmap_ret = rcutils_hash_map_get_next_key_and_data(
+    &__logger_map, NULL, &key, &entry);
+  while (RCL_RET_OK == status && RCUTILS_RET_OK == hashmap_ret) {
+    if (entry.node == node) {
+      status = rcl_ret_from_rcutils_ret(rcutils_hash_map_unset(&__logger_map, &key));
+      previous_key = NULL;
+    } else {
+      previous_key = key;
+    }
+    if (RCL_RET_OK == status) {
+      hashmap_ret = rcutils_hash_map_get_next_key_and_data(
+        &__logger_map, previous_key ? &previous_key : NULL, &key, &entry);
+    }
+  }
+  return RCL_RET_OK;
+}
+
 rcl_ret_t rcl_logging_rosout_fini()
 {
   if (!__is_initialized) {
@@ -102,6 +143,8 @@ rcl_ret_t rcl_logging_rosout_fini()
   rcl_ret_t status = RCL_RET_OK;
   char * key = NULL;
   rosout_map_entry_t entry;
+  size_t array_size;
+  char * sublogger_name = NULL;
 
   // fini all the outstanding publishers
   rcutils_ret_t hashmap_ret = rcutils_hash_map_get_next_key_and_data(
@@ -111,7 +154,8 @@ rcl_ret_t rcl_logging_rosout_fini()
     status = rcl_publisher_fini(&entry.publisher, entry.node);
 
     if (RCL_RET_OK == status) {
-      status = rcl_ret_from_rcutils_ret(rcutils_hash_map_unset(&__logger_map, &key));
+      // delete all entries using this node
+      status = rcl_ret_from_rcutils_ret(_rcl_logging_rosout_remove_logger_map(entry.node));
     }
 
     if (RCL_RET_OK == status) {
@@ -126,6 +170,24 @@ rcl_ret_t rcl_logging_rosout_fini()
     status = rcl_ret_from_rcutils_ret(rcutils_hash_map_fini(&__logger_map));
   }
 
+  if (RCL_RET_OK == status) {
+    status = rcl_ret_from_rcutils_ret(rcutils_array_list_get_size(&__sublogger_names, &array_size));
+    if (RCL_RET_OK == status) {
+      for (size_t i = 0; i < array_size; ++i) {
+        status = rcl_ret_from_rcutils_ret(
+          rcutils_array_list_get(&__sublogger_names, i, &sublogger_name));
+        if (RCL_RET_OK == status) {
+          __rosout_allocator.deallocate(sublogger_name, __rosout_allocator.state);
+          sublogger_name = NULL;
+        }
+      }
+    }
+
+    if (RCL_RET_OK == status) {
+      status = rcl_ret_from_rcutils_ret(rcutils_array_list_fini(&__sublogger_names));
+    }
+  }
+
   if (RCL_RET_OK == status) {
     __is_initialized = false;
   }
@@ -215,11 +277,12 @@ rcl_ret_t rcl_logging_rosout_fini_publisher_for_node(rcl_node_t * node)
 
   // fini the publisher and remove the entry from the map
   status = rcl_ret_from_rcutils_ret(rcutils_hash_map_get(&__logger_map, &logger_name, &entry));
-  if (RCL_RET_OK == status) {
+  if (RCL_RET_OK == status && node == entry.node) {
     status = rcl_publisher_fini(&entry.publisher, entry.node);
   }
   if (RCL_RET_OK == status) {
-    status = rcl_ret_from_rcutils_ret(rcutils_hash_map_unset(&__logger_map, &logger_name));
+    // delete all entries using this node
+    status = rcl_ret_from_rcutils_ret(_rcl_logging_rosout_remove_logger_map(entry.node));
   }
 
   return status;
@@ -292,3 +355,133 @@ void rcl_logging_rosout_output_handler(
     }
   }
 }
+
+static rcl_ret_t
+_rcl_logging_rosout_get_sublogger_name(
+  const char * logger_name, const char * sublogger_name, char ** full_sublogger_name)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(logger_name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(sublogger_name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(full_sublogger_name, RCL_RET_INVALID_ARGUMENT);
+
+  if (logger_name[0] == '\0' || sublogger_name[0] == '\0') {
+    RCL_SET_ERROR_MSG("logger name or sub-logger name can't be empty.");
+    return RCL_RET_INVALID_ARGUMENT;
+  }
+
+  *full_sublogger_name = rcutils_format_string(
+    __rosout_allocator, "%s%s%s",
+    logger_name, RCUTILS_LOGGING_SEPARATOR_STRING, sublogger_name);
+  if (NULL == *full_sublogger_name) {
+    RCL_SET_ERROR_MSG("Failed to allocate a full sublogger name.");
+    return RCL_RET_BAD_ALLOC;
+  }
+
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_logging_rosout_add_sublogger(
+  const char * logger_name, const char * sublogger_name)
+{
+  if (!__is_initialized) {
+    return RCL_RET_OK;
+  }
+  rcl_ret_t status = RCL_RET_OK;
+  char * full_sublogger_name = NULL;
+  rosout_map_entry_t entry;
+
+  status =
+    _rcl_logging_rosout_get_sublogger_name(logger_name, sublogger_name, &full_sublogger_name);
+  if (RCL_RET_OK != status) {
+    // Error already set
+    return status;
+  }
+
+  rcutils_ret_t rcutils_ret = rcutils_hash_map_get(&__logger_map, &logger_name, &entry);
+  if (RCUTILS_RET_OK != rcutils_ret) {
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("The entry of logger '%s' not exist.", logger_name);
+    status = RCL_RET_ERROR;
+    goto cleanup;
+  }
+
+  if (rcutils_hash_map_key_exists(&__logger_map, &full_sublogger_name)) {
+    status = RCL_RET_SUBLOGGER_ALREADY_EXIST;
+    goto cleanup;
+  }
+
+  status = rcl_ret_from_rcutils_ret(
+    rcutils_hash_map_set(&__logger_map, &full_sublogger_name, &entry));
+  if (RCL_RET_OK != status) {
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Failed to add publisher to map for logger '%s'.", full_sublogger_name);
+    goto cleanup;
+  }
+
+  status = rcl_ret_from_rcutils_ret(
+    rcutils_array_list_add(&__sublogger_names, &full_sublogger_name));
+  if (RCL_RET_OK != status) {
+    goto cleanup;
+  }
+
+  return status;
+
+cleanup:
+  __rosout_allocator.deallocate(full_sublogger_name, __rosout_allocator.state);
+  return status;
+}
+
+rcl_ret_t
+rcl_logging_rosout_remove_sublogger(
+  const char * logger_name, const char * sublogger_name)
+{
+  if (!__is_initialized) {
+    return RCL_RET_OK;
+  }
+  rcl_ret_t status = RCL_RET_OK;
+  char * full_sublogger_name = NULL;
+  size_t array_size;
+  char * sublogger_name_removed = NULL;
+
+  status =
+    _rcl_logging_rosout_get_sublogger_name(logger_name, sublogger_name, &full_sublogger_name);
+  if (RCL_RET_OK != status) {
+    // Error already set
+    return status;
+  }
+
+  // remove the entry from the map
+  if (!rcutils_hash_map_key_exists(&__logger_map, &full_sublogger_name)) {
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("Sub-logger '%s' not exist.", full_sublogger_name);
+    __rosout_allocator.deallocate(full_sublogger_name, __rosout_allocator.state);
+    return RCL_RET_ERROR;
+  }
+
+  status = rcl_ret_from_rcutils_ret(rcutils_hash_map_unset(&__logger_map, &full_sublogger_name));
+  if (RCL_RET_OK == status) {
+    // remove the corresponding name from __sublogger_names
+    status = rcl_ret_from_rcutils_ret(rcutils_array_list_get_size(&__sublogger_names, &array_size));
+    if (RCL_RET_OK == status) {
+      for (size_t i = 0; i < array_size; ++i) {
+        status = rcl_ret_from_rcutils_ret(
+          rcutils_array_list_get(&__sublogger_names, i, &sublogger_name_removed));
+        if (RCL_RET_OK == status) {
+          if (strcmp(full_sublogger_name, sublogger_name_removed) == 0) {
+            __rosout_allocator.deallocate(sublogger_name_removed, __rosout_allocator.state);
+            sublogger_name_removed = NULL;
+            status = rcl_ret_from_rcutils_ret(rcutils_array_list_remove(&__sublogger_names, i));
+            break;
+          }
+        }
+      }
+    }
+  }
+  __rosout_allocator.deallocate(full_sublogger_name, __rosout_allocator.state);
+
+  return status;
+}
+
+
+#ifdef __cplusplus
+}
+#endif