diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt
index 06a6c6e45..1f659dfc0 100644
--- a/rcl/CMakeLists.txt
+++ b/rcl/CMakeLists.txt
@@ -41,6 +41,7 @@ set(${PROJECT_NAME}_sources
src/rcl/init_options.c
src/rcl/lexer.c
src/rcl/lexer_lookahead.c
+ src/rcl/logging_rosout.c
src/rcl/logging.c
src/rcl/node.c
src/rcl/publisher.c
diff --git a/rcl/include/rcl/logging.h b/rcl/include/rcl/logging.h
index e648d5e18..c73f64211 100644
--- a/rcl/include/rcl/logging.h
+++ b/rcl/include/rcl/logging.h
@@ -41,6 +41,7 @@ extern "C"
* Lock-Free | Yes
*
* \param global_args The global arguments for the system
+ * \param allocator Used to allocate memory used by the logging system
* \return `RCL_RET_OK` if successful, or
* \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
* \return `RCL_RET_ERR` if a general error occurs
diff --git a/rcl/include/rcl/logging_rosout.h b/rcl/include/rcl/logging_rosout.h
new file mode 100644
index 000000000..2f702f0a4
--- /dev/null
+++ b/rcl/include/rcl/logging_rosout.h
@@ -0,0 +1,170 @@
+// Copyright 2018-2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCL__LOGGING_ROSOUT_H_
+#define RCL__LOGGING_ROSOUT_H_
+
+#include "rcl/allocator.h"
+#include "rcl/error_handling.h"
+#include "rcl/node.h"
+#include "rcl/types.h"
+#include "rcl/visibility_control.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/// Initializes the rcl_logging_rosout features
+/**
+ * Calling this will initialize the rcl_logging_rosout features. This function must be called
+ * before any other rcl_logging_rosout_* functions can be called.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] allocator The allocator used for metadata related to the rcl_logging_rosout features
+ * \return `RCL_RET_OK` if the rcl_logging_rosout features are successfully initialized, 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_LOCAL
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_logging_rosout_init(
+ const rcl_allocator_t * allocator);
+
+/// Uninitializes the rcl_logging_rosout features
+/**
+ * Calling this will set the rcl_logging_rosout features into the an unitialized state that is
+ * functionally the same as before rcl_logging_rosout_init was called.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \return `RCL_RET_OK` if the rcl_logging_rosout feature was successfully unitialized, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_LOCAL
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_logging_rosout_fini();
+
+/// Creates a rosout publisher for a node and registers it to be used by the logging system
+/**
+ * Calling this for an rcl_node_t will create a new publisher on that node that will be
+ * used by the logging system to publish all log messages from that Node's logger.
+ *
+ * If a publisher already exists for this node then a new publisher will NOT be created.
+ *
+ * It is expected that after creating a rosout publisher with this function
+ * rcl_logging_destroy_rosout_publisher_for_node() will be called for the node to cleanup
+ * the publisher while the Node is still valid.
+ *
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] node a valid rcl_node_t that the publisher will be created on
+ * \return `RCL_RET_OK` if the publisher was created successfully, or
+ * \return `RCL_RET_ALREADY_INIT` if the publisher has already exists, or
+ * \return `RCL_RET_NODE_INVALID` 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_LOCAL
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_logging_rosout_init_publisher_for_node(
+ rcl_node_t * node);
+
+/// Deregisters a rosout publisher for a node and cleans up allocated resources
+/**
+ * Calling this for an rcl_node_t will destroy the rosout publisher on that node and remove it from
+ * the logging system so that no more Log messages are published to this function.
+ *
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] node a valid rcl_node_t that the publisher will be created on
+ * \return `RCL_RET_OK` if the publisher was created successfully, or
+ * \return `RCL_RET_NODE_INVALID` 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_LOCAL
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_logging_rosout_fini_publisher_for_node(
+ rcl_node_t * node);
+
+/// The output handler outputs log messages to rosout topics.
+/**
+ * When called with a logger name and log message this function will attempt to
+ * find a rosout publisher correlated with the logger name and publish a Log
+ * message out via that publisher. If there is no publisher directly correlated
+ * with the logger then nothing will be done.
+ *
+ * This function is meant to be registered with the logging functions for rcutils
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] location The pointer to the location struct or NULL
+ * \param[in] severity The severity level
+ * \param[in] name The name of the logger, must be null terminated c string
+ * \param[in] timestamp The timestamp for when the log message was made
+ * \param[in] log_str The string to be logged
+ */
+RCL_PUBLIC
+void rcl_logging_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);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // RCL__LOGGING_ROSOUT_H_
diff --git a/rcl/package.xml b/rcl/package.xml
index 25677b20e..671181db4 100644
--- a/rcl/package.xml
+++ b/rcl/package.xml
@@ -21,6 +21,7 @@
rcutils
rosidl_generator_c
+ rcl_interfaces
ament_cmake
rcutils
rosidl_default_runtime
diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c
index a9151211a..a869c73f0 100644
--- a/rcl/src/rcl/init.c
+++ b/rcl/src/rcl/init.c
@@ -174,6 +174,12 @@ rcl_shutdown(rcl_context_t * context)
return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
}
+ rcl_ret_t rcl_ret = rcl_logging_fini();
+ RCUTILS_LOG_ERROR_EXPRESSION_NAMED(RCL_RET_OK != rcl_ret, ROS_PACKAGE_NAME,
+ "Failed to fini logging, rcl_ret_t: %i, rcl_error_str: %s", rcl_ret,
+ rcl_get_error_string().str);
+ rcl_reset_error();
+
return RCL_RET_OK;
}
diff --git a/rcl/src/rcl/logging.c b/rcl/src/rcl/logging.c
index 6358be515..3d31e1e7d 100644
--- a/rcl/src/rcl/logging.c
+++ b/rcl/src/rcl/logging.c
@@ -27,6 +27,7 @@ extern "C"
#include "rcl/error_handling.h"
#include "rcl/logging.h"
#include "rcl/logging_external_interface.h"
+#include "rcl/logging_rosout.h"
#include "rcl/macros.h"
#include "rcutils/logging.h"
#include "rcutils/time.h"
@@ -38,6 +39,9 @@ static rcutils_logging_output_handler_t
static uint8_t g_rcl_logging_num_out_handlers = 0;
static rcl_allocator_t g_logging_allocator;
+static bool g_rcl_logging_stdout_enabled = false;
+static bool g_rcl_logging_rosout_enabled = false;
+static bool g_rcl_logging_ext_lib_enabled = false;
/**
* An output function that sends to multiple output appenders.
@@ -59,16 +63,6 @@ rcl_logging_ext_lib_output_handler(
int severity, const char * name, rcutils_time_point_value_t timestamp,
const char * format, va_list * args);
-/**
- * An output function that sends to the rosout topic.
- */
-static
-void
-rcl_logging_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);
-
rcl_ret_t
rcl_logging_configure(const rcl_arguments_t * global_args, const rcl_allocator_t * allocator)
{
@@ -78,24 +72,27 @@ rcl_logging_configure(const rcl_arguments_t * global_args, const rcl_allocator_t
g_logging_allocator = *allocator;
int default_level = global_args->impl->log_level;
const char * config_file = global_args->impl->external_log_config_file;
- bool enable_stdout = !global_args->impl->log_stdout_disabled;
- bool enable_rosout = !global_args->impl->log_rosout_disabled;
- bool enable_ext_lib = !global_args->impl->log_ext_lib_disabled;
+ g_rcl_logging_stdout_enabled = !global_args->impl->log_stdout_disabled;
+ g_rcl_logging_rosout_enabled = !global_args->impl->log_rosout_disabled;
+ g_rcl_logging_ext_lib_enabled = !global_args->impl->log_ext_lib_disabled;
rcl_ret_t status = RCL_RET_OK;
g_rcl_logging_num_out_handlers = 0;
if (default_level >= 0) {
rcutils_logging_set_default_logger_level(default_level);
}
- if (enable_stdout) {
+ if (g_rcl_logging_stdout_enabled) {
g_rcl_logging_out_handlers[g_rcl_logging_num_out_handlers++] =
rcutils_logging_console_output_handler;
}
- if (enable_rosout) {
- g_rcl_logging_out_handlers[g_rcl_logging_num_out_handlers++] =
- rcl_logging_rosout_output_handler;
+ if (g_rcl_logging_rosout_enabled) {
+ status = rcl_logging_rosout_init(allocator);
+ if (RCL_RET_OK == status) {
+ g_rcl_logging_out_handlers[g_rcl_logging_num_out_handlers++] =
+ rcl_logging_rosout_output_handler;
+ }
}
- if (enable_ext_lib) {
+ if (g_rcl_logging_ext_lib_enabled) {
status = rcl_logging_external_initialize(config_file);
if (RCL_RET_OK == status) {
rcl_logging_external_set_logger_level(NULL, default_level);
@@ -107,6 +104,20 @@ rcl_logging_configure(const rcl_arguments_t * global_args, const rcl_allocator_t
return status;
}
+rcl_ret_t rcl_logging_fini()
+{
+ rcl_ret_t status = RCL_RET_OK;
+ rcutils_logging_set_output_handler(rcutils_logging_console_output_handler);
+
+ if (g_rcl_logging_rosout_enabled) {
+ status = rcl_logging_rosout_fini();
+ }
+ if (RCL_RET_OK == status && g_rcl_logging_ext_lib_enabled) {
+ status = rcl_logging_external_shutdown();
+ }
+
+ return status;
+}
static
void
@@ -185,22 +196,6 @@ rcl_logging_ext_lib_output_handler(
}
}
-static
-void
-rcl_logging_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)
-{
- // TODO(nburek): Placeholder for rosout topic feature
- RCL_UNUSED(location);
- RCL_UNUSED(severity);
- RCL_UNUSED(name);
- RCL_UNUSED(timestamp);
- RCL_UNUSED(format);
- RCL_UNUSED(args);
-}
-
#ifdef __cplusplus
}
#endif
diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c
new file mode 100644
index 000000000..067f7e945
--- /dev/null
+++ b/rcl/src/rcl/logging_rosout.c
@@ -0,0 +1,280 @@
+// Copyright 2018-2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rcl/allocator.h"
+#include "rcl/error_handling.h"
+#include "rcl/node.h"
+#include "rcl/publisher.h"
+#include "rcl/time.h"
+#include "rcl/types.h"
+#include "rcl/visibility_control.h"
+#include "rcl_interfaces/msg/log.h"
+#include "rcutils/allocator.h"
+#include "rcutils/macros.h"
+#include "rcutils/types/hash_map.h"
+#include "rcutils/types/rcutils_ret.h"
+#include "rosidl_generator_c/string_functions.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#define ROSOUT_TOPIC_NAME "rosout"
+
+/* Return RCL_RET_OK from this macro because we won't check throughout rcl if rosout is
+ * initialized or not and in the case it's not we want things to continue working.
+ */
+#define RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED \
+ if (!__is_initialized) { \
+ return RCL_RET_OK; \
+ }
+
+#define RCL_RET_FROM_RCUTIL_RET(rcl_ret_var, rcutils_expr) \
+ { \
+ rcutils_ret_t rcutils_ret = rcutils_expr; \
+ if (RCUTILS_RET_OK != rcutils_ret) { \
+ if (rcutils_error_is_set()) { \
+ RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \
+ } else { \
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("rcutils_ret_t code: %i", rcutils_ret); \
+ } \
+ } \
+ switch (rcutils_ret) { \
+ case RCUTILS_RET_OK: \
+ rcl_ret_var = RCL_RET_OK; \
+ break; \
+ case RCUTILS_RET_ERROR: \
+ rcl_ret_var = RCL_RET_ERROR; \
+ break; \
+ case RCUTILS_RET_BAD_ALLOC: \
+ rcl_ret_var = RCL_RET_BAD_ALLOC; \
+ break; \
+ case RCUTILS_RET_INVALID_ARGUMENT: \
+ rcl_ret_var = RCL_RET_INVALID_ARGUMENT; \
+ break; \
+ case RCUTILS_RET_NOT_INITIALIZED: \
+ rcl_ret_var = RCL_RET_NOT_INIT; \
+ break; \
+ default: \
+ rcl_ret_var = RCUTILS_RET_ERROR; \
+ } \
+ }
+
+typedef struct rosout_map_entry_t
+{
+ rcl_node_t * node;
+ rcl_publisher_t publisher;
+} rosout_map_entry_t;
+
+static rcutils_hash_map_t __logger_map;
+static bool __is_initialized = false;
+static rcl_allocator_t __rosout_allocator;
+
+rcl_ret_t rcl_logging_rosout_init(
+ const rcl_allocator_t * allocator)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT);
+ rcl_ret_t status = RCL_RET_OK;
+ if (__is_initialized) {
+ return RCL_RET_OK;
+ }
+ __logger_map = rcutils_get_zero_initialized_hash_map();
+ RCL_RET_FROM_RCUTIL_RET(status,
+ 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) {
+ __rosout_allocator = *allocator;
+ __is_initialized = true;
+ }
+ return status;
+}
+
+rcl_ret_t rcl_logging_rosout_fini()
+{
+ RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED
+ rcl_ret_t status = RCL_RET_OK;
+ char * key = NULL;
+ rosout_map_entry_t entry;
+
+ // fini all the outstanding publishers
+ 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) {
+ // Teardown publisher
+ status = rcl_publisher_fini(&entry.publisher, entry.node);
+
+ if (RCL_RET_OK == status) {
+ RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &key));
+ }
+
+ if (RCL_RET_OK == status) {
+ hashmap_ret = rcutils_hash_map_get_next_key_and_data(&__logger_map, NULL, &key, &entry);
+ }
+ }
+ if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != hashmap_ret) {
+ RCL_RET_FROM_RCUTIL_RET(status, hashmap_ret);
+ }
+
+ if (RCL_RET_OK == status) {
+ RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_fini(&__logger_map));
+ }
+
+ if (RCL_RET_OK == status) {
+ __is_initialized = false;
+ }
+ return status;
+}
+
+rcl_ret_t rcl_logging_rosout_init_publisher_for_node(
+ rcl_node_t * node)
+{
+ RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED
+ const char * logger_name = NULL;
+ rosout_map_entry_t new_entry;
+ rcl_ret_t status = RCL_RET_OK;
+
+ // Verify input and make sure it's not already initialized
+ RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_NODE_INVALID);
+ logger_name = rcl_node_get_logger_name(node);
+ if (NULL == logger_name) {
+ RCL_SET_ERROR_MSG("Logger name was null.");
+ return RCL_RET_ERROR;
+ }
+ if (rcutils_hash_map_key_exists(&__logger_map, &logger_name)) {
+ RCL_SET_ERROR_MSG("Logger already initialized for node.");
+ return RCL_RET_ALREADY_INIT;
+ }
+
+ // Create a new Log message publisher on the node
+ const rosidl_message_type_support_t * type_support =
+ rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Log();
+ rcl_publisher_options_t options = rcl_publisher_get_default_options();
+ new_entry.publisher = rcl_get_zero_initialized_publisher();
+ status =
+ rcl_publisher_init(&new_entry.publisher, node, type_support, ROSOUT_TOPIC_NAME, &options);
+
+ // Add the new publisher to the map
+ if (RCL_RET_OK == status) {
+ new_entry.node = node;
+ RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_set(&__logger_map, &logger_name, &new_entry));
+ if (RCL_RET_OK != status) {
+ RCL_SET_ERROR_MSG("Failed to add publisher to map.");
+ // We failed to add to the map so destroy the publisher that we created
+ rcl_ret_t fini_status = rcl_publisher_fini(&new_entry.publisher, new_entry.node);
+ // ignore the return status in favor of the failure from set
+ RCL_UNUSED(fini_status);
+ }
+ }
+
+ return status;
+}
+
+rcl_ret_t rcl_logging_rosout_fini_publisher_for_node(
+ rcl_node_t * node)
+{
+ RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED
+ rosout_map_entry_t entry;
+ const char * logger_name = NULL;
+ rcl_ret_t status = RCL_RET_OK;
+
+ // Verify input and make sure it's initialized
+ RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_NODE_INVALID);
+ logger_name = rcl_node_get_logger_name(node);
+ if (NULL == logger_name) {
+ return RCL_RET_ERROR;
+ }
+ if (!rcutils_hash_map_key_exists(&__logger_map, &logger_name)) {
+ return RCL_RET_OK;
+ }
+
+ // fini the publisher and remove the entry from the map
+ RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_get(&__logger_map, &logger_name, &entry));
+ if (RCL_RET_OK == status) {
+ status = rcl_publisher_fini(&entry.publisher, entry.node);
+ }
+ if (RCL_RET_OK == status) {
+ RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &logger_name));
+ }
+
+ return status;
+}
+
+void rcl_logging_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)
+{
+ rosout_map_entry_t entry;
+ rcl_ret_t status = RCL_RET_OK;
+ if (!__is_initialized) {
+ return;
+ }
+ rcutils_ret_t rcutils_ret = rcutils_hash_map_get(&__logger_map, &name, &entry);
+ if (RCUTILS_RET_OK == rcutils_ret) {
+ char msg_buf[1024] = "";
+ rcutils_char_array_t msg_array = {
+ .buffer = msg_buf,
+ .owns_buffer = false,
+ .buffer_length = 0u,
+ .buffer_capacity = sizeof(msg_buf),
+ .allocator = __rosout_allocator
+ };
+
+ va_list args_clone;
+ va_copy(args_clone, *args);
+ RCL_RET_FROM_RCUTIL_RET(status, rcutils_char_array_vsprintf(&msg_array, format, args_clone));
+ va_end(args_clone);
+ if (RCL_RET_OK != status) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to format log string: ");
+ RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str);
+ rcl_reset_error();
+ RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+ } else {
+ rcl_interfaces__msg__Log * log_message = rcl_interfaces__msg__Log__create();
+ if (NULL != log_message) {
+ log_message->stamp.sec = (int32_t) RCL_NS_TO_S(timestamp);
+ log_message->stamp.nanosec = (timestamp % RCL_S_TO_NS(1));
+ log_message->level = severity;
+ log_message->line = (int32_t) location->line_number;
+ rosidl_generator_c__String__assign(&log_message->name, name);
+ rosidl_generator_c__String__assign(&log_message->msg, msg_array.buffer);
+ rosidl_generator_c__String__assign(&log_message->file, location->file_name);
+ rosidl_generator_c__String__assign(&log_message->function, location->function_name);
+ status = rcl_publish(&entry.publisher, log_message);
+ if (RCL_RET_OK != status) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to publish log message to rosout: ");
+ RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str);
+ rcl_reset_error();
+ RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+ }
+
+ rcl_interfaces__msg__Log__destroy(log_message);
+ }
+ }
+
+ RCL_RET_FROM_RCUTIL_RET(status, rcutils_char_array_fini(&msg_array));
+ if (RCL_RET_OK != status) {
+ RCUTILS_SAFE_FWRITE_TO_STDERR("failed to fini char_array: ");
+ RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str);
+ rcl_reset_error();
+ RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+ }
+ }
+}
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c
index 5b2d46b00..df7445b92 100644
--- a/rcl/src/rcl/node.c
+++ b/rcl/src/rcl/node.c
@@ -26,6 +26,7 @@ extern "C"
#include "rcl/arguments.h"
#include "rcl/error_handling.h"
+#include "rcl/logging_rosout.h"
#include "rcl/rcl.h"
#include "rcl/remap.h"
#include "rcutils/filesystem.h"
@@ -421,12 +422,22 @@ rcl_node_init(
// error message already set
goto fail;
}
+ // The initialization for the rosout publisher requires the node to be in initialized to a point
+ // that it can create new topic publishers
+ 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");
ret = RCL_RET_OK;
goto cleanup;
fail:
if (node->impl) {
if (node->impl->logger_name) {
+ ret = rcl_logging_rosout_fini_publisher_for_node(node);
+ RCUTILS_LOG_ERROR_EXPRESSION_NAMED((ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT),
+ ROS_PACKAGE_NAME, "Failed to fini publisher for node: %i", ret);
allocator->deallocate((char *)node->impl->logger_name, allocator->state);
}
if (node->impl->rmw_node_handle) {
@@ -485,12 +496,17 @@ 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_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;
+ }
rmw_ret_t rmw_ret = rmw_destroy_node(node->impl->rmw_node_handle);
if (rmw_ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
result = RCL_RET_ERROR;
}
- rcl_ret_t rcl_ret = rcl_guard_condition_fini(node->impl->graph_guard_condition);
+ rcl_ret = rcl_guard_condition_fini(node->impl->graph_guard_condition);
if (rcl_ret != RCL_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
result = RCL_RET_ERROR;
diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp
index d86f34be0..1afa45773 100644
--- a/rcl/test/rcl/test_graph.cpp
+++ b/rcl/test/rcl/test_graph.cpp
@@ -316,7 +316,7 @@ check_graph_state(
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition, nullptr);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
+ std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(400);
RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME,
" state wrong, waiting up to '%s' nanoseconds for graph changes... ",
std::to_string(time_to_sleep.count()).c_str());
@@ -536,7 +536,7 @@ class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEME
rcl_wait_set_add_guard_condition(wait_set_ptr, rcl_node_get_graph_guard_condition(
node), NULL);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
+ std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(400);
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME,
" state wrong, waiting up to '%s' nanoseconds for graph changes... ",
std::to_string(time_to_sleep.count()).c_str());
@@ -575,19 +575,19 @@ TEST_F(NodeGraphMultiNodeFixture, test_node_info_subscriptions)
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
rcl_reset_error();
- VerifySubsystemCount(expected_node_state{0, 1, 0}, expected_node_state{0, 1, 0});
+ VerifySubsystemCount(expected_node_state{1, 1, 0}, expected_node_state{1, 1, 0});
// Destroy the node's subscriber
ret = rcl_subscription_fini(&sub, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
rcl_reset_error();
- VerifySubsystemCount(expected_node_state{0, 0, 0}, expected_node_state{0, 1, 0});
+ VerifySubsystemCount(expected_node_state{1, 0, 0}, expected_node_state{1, 1, 0});
// Destroy the remote node's subdscriber
ret = rcl_subscription_fini(&sub2, this->remote_node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
rcl_reset_error();
- VerifySubsystemCount(expected_node_state{0, 0, 0}, expected_node_state{0, 0, 0});
+ VerifySubsystemCount(expected_node_state{1, 0, 0}, expected_node_state{1, 0, 0});
}
TEST_F(NodeGraphMultiNodeFixture, test_node_info_publishers)
@@ -600,14 +600,14 @@ TEST_F(NodeGraphMultiNodeFixture, test_node_info_publishers)
ret = rcl_publisher_init(&pub, this->node_ptr, ts, this->topic_name.c_str(), &pub_ops);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
rcl_reset_error();
- VerifySubsystemCount(expected_node_state{1, 0, 0}, expected_node_state{0, 0, 0});
+ VerifySubsystemCount(expected_node_state{2, 0, 0}, expected_node_state{1, 0, 0});
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Destroyed publisher");
// Destroy the publisher.
ret = rcl_publisher_fini(&pub, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
rcl_reset_error();
- VerifySubsystemCount(expected_node_state{0, 0, 0}, expected_node_state{0, 0, 0});
+ VerifySubsystemCount(expected_node_state{1, 0, 0}, expected_node_state{1, 0, 0});
}
TEST_F(NodeGraphMultiNodeFixture, test_node_info_services)
@@ -619,12 +619,12 @@ TEST_F(NodeGraphMultiNodeFixture, test_node_info_services)
auto ts1 = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, Primitives);
ret = rcl_service_init(&service, this->node_ptr, ts1, service_name, &service_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- VerifySubsystemCount(expected_node_state{0, 0, 1}, expected_node_state{0, 0, 0});
+ VerifySubsystemCount(expected_node_state{1, 0, 1}, expected_node_state{1, 0, 0});
// Destroy service.
ret = rcl_service_fini(&service, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- VerifySubsystemCount(expected_node_state{0, 0, 0}, expected_node_state{0, 0, 0});
+ VerifySubsystemCount(expected_node_state{1, 0, 0}, expected_node_state{1, 0, 0});
}
/*
@@ -724,7 +724,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi
std::thread topic_thread(
[this, &topic_changes_promise]() {
// sleep
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
// create the publisher
rcl_publisher_t pub = rcl_get_zero_initialized_publisher();
rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options();
@@ -733,7 +733,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi
"/chatter_test_graph_guard_condition_topics", &pub_ops);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
// sleep
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
// create the subscription
rcl_subscription_t sub = rcl_get_zero_initialized_subscription();
rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options();
@@ -742,12 +742,12 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi
"/chatter_test_graph_guard_condition_topics", &sub_ops);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
// sleep
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
// destroy the subscription
ret = rcl_subscription_fini(&sub, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
// sleep
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
// destroy the publication
ret = rcl_publisher_fini(&pub, this->node_ptr);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
@@ -767,7 +767,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition, NULL);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200);
+ std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(400);
RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME,
"waiting up to '%s' nanoseconds for graph changes",
std::to_string(time_to_sleep.count()).c_str());