From 49ca1bd9c90b269be69dcad2c7ad1ec528364499 Mon Sep 17 00:00:00 2001 From: burekn Date: Thu, 6 Dec 2018 19:08:02 -0800 Subject: [PATCH 1/8] Implementing the rosout logging feature. --- rcl/CMakeLists.txt | 1 + rcl/include/rcl/logging.h | 1 + rcl/include/rcl/logging_rosout.h | 169 ++++++++++++++++++++++ rcl/package.xml | 1 + rcl/src/rcl/init.c | 4 + rcl/src/rcl/logging.c | 63 ++++----- rcl/src/rcl/logging_rosout.c | 236 +++++++++++++++++++++++++++++++ rcl/src/rcl/node.c | 18 ++- 8 files changed, 458 insertions(+), 35 deletions(-) create mode 100644 rcl/include/rcl/logging_rosout.h create mode 100644 rcl/src/rcl/logging_rosout.c 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..78b2c3dcf --- /dev/null +++ b/rcl/include/rcl/logging_rosout.h @@ -0,0 +1,169 @@ +// Copyright 2017 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_ALREADY_INIT` if the feature has already 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 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 that + * 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_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_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_NOT_INIT` if the publisher does not exist for the provided node, 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_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 location The pointer to the location struct or NULL + * \param severity The severity level + * \param name The name of the logger, must be null terminated c string + * \param timestamp The timestamp for when the log message was made + * \param 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..78ed47fb6 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -174,6 +174,10 @@ 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. %i", rcl_ret); + 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..a7c869537 --- /dev/null +++ b/rcl/src/rcl/logging_rosout.c @@ -0,0 +1,236 @@ +// Copyright 2017 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_H_ +#define RCL__LOGGING_H_ + + +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcl/publisher.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 "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; \ + } + +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(); + 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; + const char * key = NULL; + rosout_map_entry_t entry; + + // fini all the outstanding publishers + status = rcutils_hash_map_get_next_key_and_data(&__logger_map, NULL, &key, &entry); + while (RCUTILS_RET_OK == status) { + // Teardown publisher + status = rcl_publisher_fini(&entry.publisher, entry.node); + + if (RCL_RET_OK == status) { + status = rcutils_hash_map_get_next_key_and_data(&__logger_map, key, &key, &entry); + } + } + if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES == status) { + status = RCL_RET_OK; + } + + if (RCL_RET_OK == status) { + 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_INVALID_ARGUMENT); + 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; + 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_INVALID_ARGUMENT); + 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_NOT_INIT; + } + + // fini the publisher and remove the entry from the map + 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) { + 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; + } + status = rcutils_hash_map_get(&__logger_map, &name, &entry); + if (RCL_RET_OK == status) { + 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); + status = rcutils_char_array_vsprintf(&msg_array, format, args_clone); + va_end(args_clone); + + + rcl_interfaces__msg__Log * log_message = rcl_interfaces__msg__Log__create(); + if (NULL != log_message) { + log_message->stamp.sec = (timestamp / 1000000000); + log_message->stamp.nanosec = (timestamp % 1000000000); + log_message->level = severity; + log_message->line = 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); + + rcl_interfaces__msg__Log__destroy(log_message); + } + + 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 + +#endif // RCL__LOGGING_H_ 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; From f7fe5a1eb32f6fd2b6d6237fb6a4456fee5013ca Mon Sep 17 00:00:00 2001 From: burekn Date: Fri, 4 Jan 2019 11:25:30 -0800 Subject: [PATCH 2/8] fixup for PR --- rcl/include/rcl/logging_rosout.h | 27 +++++++++------- rcl/src/rcl/logging_rosout.c | 54 ++++++++++++++++++-------------- 2 files changed, 45 insertions(+), 36 deletions(-) diff --git a/rcl/include/rcl/logging_rosout.h b/rcl/include/rcl/logging_rosout.h index 78b2c3dcf..4631bccca 100644 --- a/rcl/include/rcl/logging_rosout.h +++ b/rcl/include/rcl/logging_rosout.h @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// 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. @@ -73,14 +73,14 @@ RCL_WARN_UNUSED rcl_ret_t rcl_logging_rosout_fini(); -/// Creates a rosout publisher for a node and registers to be used by the logging system +/// 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 that + * 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. * @@ -96,7 +96,7 @@ rcl_logging_rosout_fini(); * \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_INVALID_ARGUMENT` if any arguments are invalid, 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. */ @@ -123,7 +123,7 @@ rcl_logging_rosout_init_publisher_for_node( * \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_NOT_INIT` if the publisher does not exist for the provided node, or - * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, 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. */ @@ -150,17 +150,20 @@ rcl_logging_rosout_fini_publisher_for_node( * Uses Atomics | No * Lock-Free | Yes * - * \param location The pointer to the location struct or NULL - * \param severity The severity level - * \param name The name of the logger, must be null terminated c string - * \param timestamp The timestamp for when the log message was made - * \param log_str The string to be logged + * \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); + int severity, + const char * name, + rcutils_time_point_value_t timestamp, + const char * format, + va_list * args); #ifdef __cplusplus } diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index a7c869537..09acffe8b 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// 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. @@ -12,14 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCL__LOGGING_H_ -#define RCL__LOGGING_H_ - - #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" @@ -111,7 +108,7 @@ rcl_ret_t rcl_logging_rosout_init_publisher_for_node( 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_INVALID_ARGUMENT); + 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."); @@ -155,7 +152,7 @@ rcl_ret_t rcl_logging_rosout_fini_publisher_for_node( rcl_ret_t status = RCL_RET_OK; // Verify input and make sure it's initialized - RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + 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; @@ -201,21 +198,32 @@ void rcl_logging_rosout_output_handler( va_copy(args_clone, *args); status = rcutils_char_array_vsprintf(&msg_array, format, args_clone); va_end(args_clone); - - - rcl_interfaces__msg__Log * log_message = rcl_interfaces__msg__Log__create(); - if (NULL != log_message) { - log_message->stamp.sec = (timestamp / 1000000000); - log_message->stamp.nanosec = (timestamp % 1000000000); - log_message->level = severity; - log_message->line = 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); - - rcl_interfaces__msg__Log__destroy(log_message); + 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 = RCL_NS_TO_S(timestamp); + log_message->stamp.nanosec = (timestamp % RCL_S_TO_NS(1)); + log_message->level = severity; + log_message->line = 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); + } } status = rcutils_char_array_fini(&msg_array); @@ -232,5 +240,3 @@ void rcl_logging_rosout_output_handler( #ifdef __cplusplus } #endif - -#endif // RCL__LOGGING_H_ From 36e2e3b55197b931af1c06306b1add9a68f4c607 Mon Sep 17 00:00:00 2001 From: burekn Date: Fri, 4 Jan 2019 14:24:12 -0800 Subject: [PATCH 3/8] Fixup for PR. --- rcl/include/rcl/logging_rosout.h | 1 - rcl/src/rcl/logging_rosout.c | 59 ++++++++++++++++++++++++-------- 2 files changed, 45 insertions(+), 15 deletions(-) diff --git a/rcl/include/rcl/logging_rosout.h b/rcl/include/rcl/logging_rosout.h index 4631bccca..3659fbac1 100644 --- a/rcl/include/rcl/logging_rosout.h +++ b/rcl/include/rcl/logging_rosout.h @@ -41,7 +41,6 @@ extern "C" * * \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_ALREADY_INIT` if the feature has already 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. diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index 09acffe8b..f9c25b4e8 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -23,6 +23,7 @@ #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 @@ -40,6 +41,33 @@ extern "C" 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) { \ + RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \ + } \ + 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; @@ -59,8 +87,9 @@ rcl_ret_t rcl_logging_rosout_init( return RCL_RET_OK; } __logger_map = rcutils_get_zero_initialized_hash_map(); - 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); + 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; @@ -76,21 +105,23 @@ rcl_ret_t rcl_logging_rosout_fini() rosout_map_entry_t entry; // fini all the outstanding publishers - status = rcutils_hash_map_get_next_key_and_data(&__logger_map, NULL, &key, &entry); - while (RCUTILS_RET_OK == status) { + RCL_RET_FROM_RCUTIL_RET(status, + rcutils_hash_map_get_next_key_and_data(&__logger_map, NULL, &key, &entry)); + rcutils_ret_t hashmap_ret; + 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) { - status = rcutils_hash_map_get_next_key_and_data(&__logger_map, key, &key, &entry); + hashmap_ret = rcutils_hash_map_get_next_key_and_data(&__logger_map, key, &key, &entry); } } - if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES == status) { - status = RCL_RET_OK; + if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != hashmap_ret) { + RCL_RET_FROM_RCUTIL_RET(status, hashmap_ret); } if (RCL_RET_OK == status) { - status = rcutils_hash_map_fini(&__logger_map); + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_fini(&__logger_map)); } if (RCL_RET_OK == status) { @@ -130,7 +161,7 @@ rcl_ret_t rcl_logging_rosout_init_publisher_for_node( // Add the new publisher to the map if (RCL_RET_OK == status) { new_entry.node = node; - status = rcutils_hash_map_set(&__logger_map, &logger_name, &new_entry); + 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 @@ -162,12 +193,12 @@ rcl_ret_t rcl_logging_rosout_fini_publisher_for_node( } // fini the publisher and remove the entry from the map - status = rcutils_hash_map_get(&__logger_map, &logger_name, &entry); + 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) { - status = rcutils_hash_map_unset(&__logger_map, &logger_name); + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &logger_name)); } return status; @@ -183,7 +214,7 @@ void rcl_logging_rosout_output_handler( if (!__is_initialized) { return; } - status = rcutils_hash_map_get(&__logger_map, &name, &entry); + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_get(&__logger_map, &name, &entry)); if (RCL_RET_OK == status) { char msg_buf[1024] = ""; rcutils_char_array_t msg_array = { @@ -196,7 +227,7 @@ void rcl_logging_rosout_output_handler( va_list args_clone; va_copy(args_clone, *args); - status = rcutils_char_array_vsprintf(&msg_array, format, args_clone); + 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: "); @@ -226,7 +257,7 @@ void rcl_logging_rosout_output_handler( } } - status = rcutils_char_array_fini(&msg_array); + 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); From 5ba5456bdca144fffd8c2136c62b24afe2fb3a3b Mon Sep 17 00:00:00 2001 From: burekn Date: Fri, 4 Jan 2019 17:58:43 -0800 Subject: [PATCH 4/8] Fixing broken unit tests. --- rcl/include/rcl/logging_rosout.h | 1 - rcl/src/rcl/init.c | 4 +++- rcl/src/rcl/logging_rosout.c | 20 +++++++++++++------- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/rcl/include/rcl/logging_rosout.h b/rcl/include/rcl/logging_rosout.h index 3659fbac1..2f702f0a4 100644 --- a/rcl/include/rcl/logging_rosout.h +++ b/rcl/include/rcl/logging_rosout.h @@ -121,7 +121,6 @@ rcl_logging_rosout_init_publisher_for_node( * * \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_NOT_INIT` if the publisher does not exist for the provided node, 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. diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c index 78ed47fb6..a869c73f0 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -176,7 +176,9 @@ rcl_shutdown(rcl_context_t * context) 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. %i", rcl_ret); + "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_rosout.c b/rcl/src/rcl/logging_rosout.c index f9c25b4e8..6de9e2a11 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -45,7 +45,11 @@ extern "C" { \ rcutils_ret_t rcutils_ret = rcutils_expr; \ if (RCUTILS_RET_OK != rcutils_ret) { \ - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \ + 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: \ @@ -105,15 +109,17 @@ rcl_ret_t rcl_logging_rosout_fini() rosout_map_entry_t entry; // fini all the outstanding publishers - RCL_RET_FROM_RCUTIL_RET(status, - rcutils_hash_map_get_next_key_and_data(&__logger_map, NULL, &key, &entry)); - rcutils_ret_t hashmap_ret; + 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) { + const char * prev_key = key; hashmap_ret = rcutils_hash_map_get_next_key_and_data(&__logger_map, key, &key, &entry); + + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &prev_key)); } } if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != hashmap_ret) { @@ -189,7 +195,7 @@ rcl_ret_t rcl_logging_rosout_fini_publisher_for_node( return RCL_RET_ERROR; } if (!rcutils_hash_map_key_exists(&__logger_map, &logger_name)) { - return RCL_RET_NOT_INIT; + return RCL_RET_OK; } // fini the publisher and remove the entry from the map @@ -214,8 +220,8 @@ void rcl_logging_rosout_output_handler( if (!__is_initialized) { return; } - RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_get(&__logger_map, &name, &entry)); - if (RCL_RET_OK == status) { + 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, From 81f191f97a24180a22f6010a4610b243a591dee9 Mon Sep 17 00:00:00 2001 From: burekn Date: Tue, 8 Jan 2019 08:47:21 -0800 Subject: [PATCH 5/8] Minor fixup for PR. --- rcl/src/rcl/logging_rosout.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index 6de9e2a11..1a819a4ad 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -117,7 +117,7 @@ rcl_ret_t rcl_logging_rosout_fini() if (RCL_RET_OK == status) { const char * prev_key = key; - hashmap_ret = rcutils_hash_map_get_next_key_and_data(&__logger_map, key, &key, &entry); + hashmap_ret = rcutils_hash_map_get_next_key_and_data(&__logger_map, NULL, &key, &entry); RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &prev_key)); } From 810dc4e306454d6b075ebeea8a78ac9c85c5b62a Mon Sep 17 00:00:00 2001 From: burekn Date: Thu, 10 Jan 2019 16:29:40 -0800 Subject: [PATCH 6/8] Fixing bug in rosout teardown. --- rcl/src/rcl/logging_rosout.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index 1a819a4ad..543183c09 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -116,10 +116,11 @@ rcl_ret_t rcl_logging_rosout_fini() status = rcl_publisher_fini(&entry.publisher, entry.node); if (RCL_RET_OK == status) { - const char * prev_key = key; - hashmap_ret = rcutils_hash_map_get_next_key_and_data(&__logger_map, NULL, &key, &entry); + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &key)); + } - RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &prev_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) { From a43c12d4a6082bd56518c0e0375b061f0850dee0 Mon Sep 17 00:00:00 2001 From: burekn Date: Thu, 10 Jan 2019 16:47:28 -0800 Subject: [PATCH 7/8] Fixing unit test that broke because the rosout node wasn't getting counted. --- rcl/test/rcl/test_graph.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) 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()); From feb04ec4be0b6d14b426b875d60de67fa2ac8a07 Mon Sep 17 00:00:00 2001 From: burekn Date: Fri, 11 Jan 2019 08:10:02 -0800 Subject: [PATCH 8/8] Fixing warnings on Windows. --- rcl/src/rcl/logging_rosout.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index 543183c09..067f7e945 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -105,7 +105,7 @@ rcl_ret_t rcl_logging_rosout_fini() { RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED rcl_ret_t status = RCL_RET_OK; - const char * key = NULL; + char * key = NULL; rosout_map_entry_t entry; // fini all the outstanding publishers @@ -244,10 +244,10 @@ void rcl_logging_rosout_output_handler( } else { rcl_interfaces__msg__Log * log_message = rcl_interfaces__msg__Log__create(); if (NULL != log_message) { - log_message->stamp.sec = RCL_NS_TO_S(timestamp); + 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 = location->line_number; + 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);