diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 934ef4ae1..748ce7b15 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -47,6 +47,7 @@ set(${PROJECT_NAME}_sources src/rcl/localhost.c src/rcl/logging_rosout.c src/rcl/logging.c + src/rcl/log_level.c src/rcl/node.c src/rcl/node_options.c src/rcl/publisher.c diff --git a/rcl/include/rcl/arguments.h b/rcl/include/rcl/arguments.h index f29da70f8..bd2c25d96 100644 --- a/rcl/include/rcl/arguments.h +++ b/rcl/include/rcl/arguments.h @@ -16,6 +16,7 @@ #define RCL__ARGUMENTS_H_ #include "rcl/allocator.h" +#include "rcl/log_level.h" #include "rcl/macros.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -77,9 +78,9 @@ rcl_get_zero_initialized_arguments(void); * Parameter override rule parsing is supported via `-p/--param` flags e.g. `--param name:=value` * or `-p name:=value`. * - * The default log level will be parsed as `--log-level level`, where `level` is a name - * representing one of the log levels in the `RCUTILS_LOG_SEVERITY` enum, e.g. `info`, `debug`, - * `warn`, not case sensitive. + * The default log level will be parsed as `--log-level level` and logger levels will be parsed as + * multiple `--log-level name:=level`, where `level` is a name representing one of the log levels + * in the `RCUTILS_LOG_SEVERITY` enum, e.g. `info`, `debug`, `warn`, not case sensitive. * If multiple of these rules are found, the last one parsed will be used. * * If an argument does not appear to be a valid ROS argument e.g. a `-r/--remap` flag followed by @@ -343,6 +344,32 @@ rcl_remove_ros_arguments( int * nonros_argc, const char ** nonros_argv[]); +/// Return log levels parsed from the command line. +/** + * Log levels are parsed directly from command line arguments. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] arguments An arguments structure that has been parsed. + * \param[out] log_levels Log levels as parsed from command line arguments. + * The output must be finished by the caller if the function successes. + * \return `RCL_RET_OK` if everything goes correctly, or + * \return `RCL_RET_INVALID_ARGUMENT` if any function arguments are invalid, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_get_log_levels( + const rcl_arguments_t * arguments, + rcl_log_levels_t * log_levels); + /// Copy one arguments structure into another. /** *
diff --git a/rcl/include/rcl/log_level.h b/rcl/include/rcl/log_level.h new file mode 100644 index 000000000..14a8ead5d --- /dev/null +++ b/rcl/include/rcl/log_level.h @@ -0,0 +1,191 @@ +// Copyright 2020 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__LOG_LEVEL_H_ +#define RCL__LOG_LEVEL_H_ + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// typedef for RCUTILS_LOG_SEVERITY; +typedef enum RCUTILS_LOG_SEVERITY rcl_log_severity_t; + +/// A logger item to specify a name and a log level. +typedef struct rcl_logger_setting_t +{ + /// Name for the logger. + const char * name; + /// Minimum log level severity of the logger. + rcl_log_severity_t level; +} rcl_logger_setting_t; + +/// Hold default logger level and other logger setting. +typedef struct rcl_log_levels_t +{ + /// Minimum default logger level severity. + rcl_log_severity_t default_logger_level; + /// Array of logger setting. + struct rcl_logger_setting_t * logger_settings; + /// Number of logger settings. + size_t num_logger_settings; + /// Capacity of logger settings. + size_t capacity_logger_settings; + /// Allocator used to allocate objects in this struct. + rcl_allocator_t allocator; +} rcl_log_levels_t; + +/// Return a rcl_log_levels_t struct with members initialized to zero value. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \return a rcl_log_levels_t struct with members initialized to zero value. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_log_levels_t +rcl_get_zero_initialized_log_levels(); + +/// Initialize a log levels structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] log_levels The structure to be initialized. + * \param[in] allocator Memory allocator to be used and assigned into log_levels. + * \param[in] logger_count Number of logger settings to be allocated. + * This reserves memory for logger_settings, but doesn't initialize it. + * \return `RCL_RET_OK` if the structure was initialized successfully, or + * \return `RCL_RET_INVALID_ARGUMENT` if log_levels is NULL, or + * log_levels contains initialized memory, or + * allocator is invalid, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_log_levels_init( + rcl_log_levels_t * log_levels, const rcl_allocator_t * allocator, size_t logger_count); + +/// Copy one log levels structure into another. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] src The structure to be copied. + * Its allocator is used to copy memory into the new structure. + * \param[out] dst A log levels structure to be copied into. + * \return `RCL_RET_OK` if the structure was copied successfully, or + * \return `RCL_RET_INVALID_ARGUMENT` if src is NULL, or + * if src allocator is invalid, or + * if dst is NULL, or + * if dst contains already allocated memory, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_log_levels_copy(const rcl_log_levels_t * src, rcl_log_levels_t * dst); + +/// Reclaim resources held inside rcl_log_levels_t structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] log_levels The structure which its resources have to be deallocated. + * \return `RCL_RET_OK` if the memory was successfully freed, or + * \return `RCL_RET_INVALID_ARGUMENT` if log_levels is NULL, or + * if ist allocator is invalid and the structure contains initialized memory. + */ +RCL_PUBLIC +rcl_ret_t +rcl_log_levels_fini(rcl_log_levels_t * log_levels); + +/// Shrink log levels structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] log_levels The structure to be shrunk. + * \return `RCL_RET_OK` if the memory was successfully shrunk, or + * \return `RCL_RET_INVALID_ARGUMENT` if log_levels is NULL or if its allocator is invalid, or + * \return `RCL_RET_BAD_ALLOC` if reallocating memory failed. + */ +RCL_PUBLIC +rcl_ret_t +rcl_log_levels_shrink_to_size(rcl_log_levels_t * log_levels); + +/// Add logger setting with a name and a level. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] log_levels The structure where to set the logger log level. + * \param[in] logger_name Name for the logger, a copy of it will be stored in the structure. + * \param[in] log_level Minimum log level severity to be set for logger_name. + * \return `RCL_RET_OK` if add logger setting successfully, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or + * \return `RCL_RET_INVALID_ARGUMENT` if log_levels is NULL, or + * if log_levels was not initialized, or + * if log_levels allocator is invalid, or + * if logger_name is NULL, or + * \return `RCL_RET_ERROR` if the log_levels structure is already full. + */ +RCL_PUBLIC +rcl_ret_t +rcl_log_levels_add_logger_setting( + rcl_log_levels_t * log_levels, const char * logger_name, rcl_log_severity_t log_level); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__LOG_LEVEL_H_ diff --git a/rcl/src/rcl/arguments.c b/rcl/src/rcl/arguments.c index 112d57ad6..6484b6a70 100644 --- a/rcl/src/rcl/arguments.c +++ b/rcl/src/rcl/arguments.c @@ -136,13 +136,26 @@ rcl_arguments_get_param_overrides( return RCL_RET_OK; } +rcl_ret_t +rcl_arguments_get_log_levels( + const rcl_arguments_t * arguments, + rcl_log_levels_t * log_levels) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arguments, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(arguments->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &arguments->impl->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + return rcl_log_levels_copy(&arguments->impl->log_levels, log_levels); +} + /// Parse an argument that may or may not be a log level rule. /** * \param[in] arg the argument to parse - * \param[in] allocator an allocator to use - * \param[in,out] log_level parsed log level represented by `RCUTILS_LOG_SEVERITY` enum + * \param[in,out] log_levels parsed a default logger level or a logger setting * \return RCL_RET_OK if a valid log level was parsed, or - * \return RCL_RET_INVALID_LOG_LEVEL if the argument is not a valid rule, or + * \return RCL_RET_INVALID_LOG_LEVEL_RULE if the argument is not a valid rule, or * \return RCL_RET_BAD_ALLOC if an allocation failed, or * \return RLC_RET_ERROR if an unspecified error occurred. */ @@ -150,8 +163,7 @@ RCL_LOCAL rcl_ret_t _rcl_parse_log_level( const char * arg, - rcl_allocator_t allocator, - int * log_level); + rcl_log_levels_t * log_levels); /// Parse an argument that may or may not be a log configuration file. /** @@ -295,6 +307,12 @@ rcl_parse_arguments( ret = RCL_RET_BAD_ALLOC; goto fail; } + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + ret = rcl_log_levels_init(&log_levels, &allocator, argc); + if (ret != RCL_RET_OK) { + goto fail; + } + args_impl->log_levels = log_levels; bool parsing_ros_args = false; for (int i = 0; i < argc; ++i) { @@ -402,10 +420,10 @@ rcl_parse_arguments( // Attempt to parse argument as log level configuration if (strcmp(RCL_LOG_LEVEL_FLAG, argv[i]) == 0) { if (i + 1 < argc) { - int log_level; - if (RCL_RET_OK == _rcl_parse_log_level(argv[i + 1], allocator, &log_level)) { + if (RCL_RET_OK == + _rcl_parse_log_level(argv[i + 1], &args_impl->log_levels)) + { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Got log level: %s\n", argv[i + 1]); - args_impl->log_level = log_level; ++i; // Skip flag here, for loop will skip value. continue; } @@ -642,6 +660,12 @@ rcl_parse_arguments( } } + // Shrink logger settings of log levels + ret = rcl_log_levels_shrink_to_size(&args_impl->log_levels); + if (ret != RCL_RET_OK) { + goto fail; + } + return RCL_RET_OK; fail: fail_ret = ret; @@ -923,6 +947,14 @@ rcl_arguments_fini( args->impl->num_remap_rules = 0; } + rcl_ret_t log_levels_ret = rcl_log_levels_fini(&args->impl->log_levels); + if (log_levels_ret != RCL_RET_OK) { + ret = log_levels_ret; + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Failed to finalize log levels while finalizing arguments. Continuing..."); + } + args->impl->allocator.deallocate(args->impl->unparsed_args, args->impl->allocator.state); args->impl->num_unparsed_args = 0; args->impl->unparsed_args = NULL; @@ -1596,21 +1628,157 @@ _rcl_parse_remap_begin_remap_rule( return ret; } +RCL_LOCAL +rcl_ret_t +_rcl_parse_log_level_name( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_allocator_t * allocator, + char ** logger_name) +{ + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(rcutils_allocator_is_valid(allocator)); + assert(NULL != logger_name); + assert(NULL == *logger_name); + + const char * name_start = rcl_lexer_lookahead2_get_text(lex_lookahead); + if (NULL == name_start) { + RCL_SET_ERROR_MSG("failed to get start of logger name"); + return RCL_RET_ERROR; + } + + rcl_ret_t ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + + while (RCL_LEXEME_SEPARATOR != lexeme) { + ret = rcl_lexer_lookahead2_expect(lex_lookahead, lexeme, NULL, NULL); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + + if (lexeme == RCL_LEXEME_EOF) { + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + return ret; + } + } + + // Copy logger name + const char * name_end = rcl_lexer_lookahead2_get_text(lex_lookahead); + const size_t length = (size_t)(name_end - name_start); + *logger_name = rcutils_strndup(name_start, length, *allocator); + if (NULL == *logger_name) { + RCL_SET_ERROR_MSG("failed to copy logger name"); + return RCL_RET_BAD_ALLOC; + } + + return RCL_RET_OK; +} + rcl_ret_t _rcl_parse_log_level( const char * arg, - rcl_allocator_t allocator, - int * log_level) + rcl_log_levels_t * log_levels) { RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(log_level, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels->logger_settings, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &log_levels->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); - rcutils_ret_t ret = rcutils_logging_severity_level_from_string(arg, allocator, log_level); - if (RCUTILS_RET_OK == ret) { - return RCL_RET_OK; + rcl_ret_t ret = RCL_RET_OK; + char * logger_name = NULL; + int level = 0; + rcutils_ret_t rcutils_ret = RCUTILS_RET_OK; + + rcl_lexer_lookahead2_t lex_lookahead = rcl_get_zero_initialized_lexer_lookahead2(); + + ret = rcl_lexer_lookahead2_init(&lex_lookahead, arg, *allocator); + if (RCL_RET_OK != ret) { + return ret; } - RCL_SET_ERROR_MSG("Argument does not use a valid severity level"); - return RCL_RET_ERROR; + + ret = _rcl_parse_log_level_name(&lex_lookahead, allocator, &logger_name); + if (RCL_RET_OK == ret) { + if (strlen(logger_name) == 0) { + RCL_SET_ERROR_MSG("Argument has an invalid logger item that name is empty"); + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + goto cleanup; + } + + ret = rcl_lexer_lookahead2_expect(&lex_lookahead, RCL_LEXEME_SEPARATOR, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + goto cleanup; + } + + const char * level_token; + size_t level_token_length; + ret = rcl_lexer_lookahead2_expect( + &lex_lookahead, RCL_LEXEME_TOKEN, &level_token, &level_token_length); + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + goto cleanup; + } + + ret = rcl_lexer_lookahead2_expect(&lex_lookahead, RCL_LEXEME_EOF, NULL, NULL); + if (RCL_RET_OK != ret) { + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + goto cleanup; + } + + rcutils_ret = rcutils_logging_severity_level_from_string( + level_token, *allocator, &level); + if (RCUTILS_RET_OK == rcutils_ret) { + ret = rcl_log_levels_add_logger_setting( + log_levels, logger_name, (rcl_log_severity_t)level); + if (ret != RCL_RET_OK) { + goto cleanup; + } + } + } else { + rcutils_ret = rcutils_logging_severity_level_from_string( + arg, *allocator, &level); + if (RCUTILS_RET_OK == rcutils_ret) { + if (log_levels->default_logger_level != (rcl_log_severity_t)level) { + if (log_levels->default_logger_level != RCUTILS_LOG_SEVERITY_UNSET) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Minimum default log level will be replaced from %d to %d", + log_levels->default_logger_level, level); + } + log_levels->default_logger_level = (rcl_log_severity_t)level; + } + ret = RCL_RET_OK; + } + } + + if (RCUTILS_RET_OK != rcutils_ret) { + RCL_SET_ERROR_MSG("Argument does not use a valid severity level"); + ret = RCL_RET_ERROR; + } + +cleanup: + if (logger_name) { + allocator->deallocate(logger_name, allocator->state); + } + rcl_ret_t rv = rcl_lexer_lookahead2_fini(&lex_lookahead); + if (RCL_RET_OK != rv) { + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini lookahead2 after error occurred"); + } else { + ret = rv; + } + } + + return ret; } rcl_ret_t @@ -1844,7 +2012,7 @@ _rcl_allocate_initialized_arguments_impl(rcl_arguments_t * args, rcl_allocator_t rcl_arguments_impl_t * args_impl = args->impl; args_impl->num_remap_rules = 0; args_impl->remap_rules = NULL; - args_impl->log_level = -1; + args_impl->log_levels = rcl_get_zero_initialized_log_levels(); args_impl->external_log_config_file = NULL; args_impl->unparsed_args = NULL; args_impl->num_unparsed_args = 0; diff --git a/rcl/src/rcl/arguments_impl.h b/rcl/src/rcl/arguments_impl.h index a7e85dd0f..18b8cd781 100644 --- a/rcl/src/rcl/arguments_impl.h +++ b/rcl/src/rcl/arguments_impl.h @@ -16,6 +16,7 @@ #define RCL__ARGUMENTS_IMPL_H_ #include "rcl/arguments.h" +#include "rcl/log_level.h" #include "rcl_yaml_param_parser/types.h" #include "./remap_impl.h" @@ -50,8 +51,8 @@ typedef struct rcl_arguments_impl_t /// Length of remap_rules. int num_remap_rules; - /// Default log level (represented by `RCUTILS_LOG_SEVERITY` enum) or -1 if not specified. - int log_level; + /// Log levels parsed from arguments. + rcl_log_levels_t log_levels; /// A file used to configure the external logging library char * external_log_config_file; /// A boolean value indicating if the standard out handler should be used for log output diff --git a/rcl/src/rcl/log_level.c b/rcl/src/rcl/log_level.c new file mode 100644 index 000000000..acd7ec234 --- /dev/null +++ b/rcl/src/rcl/log_level.c @@ -0,0 +1,185 @@ +// Copyright 2020 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/log_level.h" + +#include "rcl/error_handling.h" +#include "rcutils/allocator.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" + +rcl_log_levels_t +rcl_get_zero_initialized_log_levels() +{ + const rcl_log_levels_t log_levels = { + .default_logger_level = RCUTILS_LOG_SEVERITY_UNSET, + .logger_settings = NULL, + .num_logger_settings = 0, + .capacity_logger_settings = 0, + .allocator = {NULL, NULL, NULL, NULL, NULL}, + }; + return log_levels; +} + +rcl_ret_t +rcl_log_levels_init( + rcl_log_levels_t * log_levels, const rcl_allocator_t * allocator, size_t logger_count) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (log_levels->logger_settings != NULL) { + RCL_SET_ERROR_MSG("invalid logger settings"); + return RCL_RET_INVALID_ARGUMENT; + } + + log_levels->default_logger_level = RCUTILS_LOG_SEVERITY_UNSET; + log_levels->logger_settings = NULL; + log_levels->num_logger_settings = 0; + log_levels->capacity_logger_settings = logger_count; + log_levels->allocator = *allocator; + + if (logger_count > 0) { + log_levels->logger_settings = allocator->allocate( + sizeof(rcl_logger_setting_t) * logger_count, allocator->state); + if (NULL == log_levels->logger_settings) { + RCL_SET_ERROR_MSG("Error allocating memory"); + return RCL_RET_BAD_ALLOC; + } + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_log_levels_copy(const rcl_log_levels_t * src, rcl_log_levels_t * dst) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(src, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(dst, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &src->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (dst->logger_settings != NULL) { + RCL_SET_ERROR_MSG("invalid logger settings"); + return RCL_RET_INVALID_ARGUMENT; + } + + dst->logger_settings = allocator->allocate( + sizeof(rcl_logger_setting_t) * (src->num_logger_settings), allocator->state); + if (NULL == dst->logger_settings) { + RCL_SET_ERROR_MSG("Error allocating memory"); + return RCL_RET_BAD_ALLOC; + } + + dst->default_logger_level = src->default_logger_level; + dst->capacity_logger_settings = src->capacity_logger_settings; + dst->allocator = src->allocator; + for (size_t i = 0; i < src->num_logger_settings; ++i) { + dst->logger_settings[i].name = + rcutils_strdup(src->logger_settings[i].name, *allocator); + if (NULL == dst->logger_settings[i].name) { + dst->num_logger_settings = i; + if (RCL_RET_OK != rcl_log_levels_fini(dst)) { + RCL_SET_ERROR_MSG("Error while finalizing log levels due to another error"); + } + return RCL_RET_BAD_ALLOC; + } + dst->logger_settings[i].level = src->logger_settings[i].level; + } + dst->num_logger_settings = src->num_logger_settings; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_log_levels_fini(rcl_log_levels_t * log_levels) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &log_levels->allocator; + if (log_levels->logger_settings) { + // check allocator here, so it's safe to finish a zero initialized rcl_log_levels_t + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + for (size_t i = 0; i < log_levels->num_logger_settings; ++i) { + allocator->deallocate((void *)log_levels->logger_settings[i].name, allocator->state); + } + log_levels->num_logger_settings = 0; + + allocator->deallocate(log_levels->logger_settings, allocator->state); + log_levels->logger_settings = NULL; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_log_levels_shrink_to_size(rcl_log_levels_t * log_levels) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &log_levels->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (0U == log_levels->num_logger_settings) { + allocator->deallocate(log_levels->logger_settings, allocator->state); + log_levels->logger_settings = NULL; + log_levels->capacity_logger_settings = 0; + } else if (log_levels->num_logger_settings < log_levels->capacity_logger_settings) { + rcl_logger_setting_t * new_logger_settings = allocator->reallocate( + log_levels->logger_settings, + sizeof(rcl_logger_setting_t) * log_levels->num_logger_settings, + allocator->state); + if (NULL == new_logger_settings) { + return RCL_RET_BAD_ALLOC; + } + log_levels->logger_settings = new_logger_settings; + log_levels->capacity_logger_settings = log_levels->num_logger_settings; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_log_levels_add_logger_setting( + rcl_log_levels_t * log_levels, const char * logger_name, rcl_log_severity_t log_level) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels->logger_settings, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(logger_name, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &log_levels->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + // check if there exists a logger with the same name + rcl_logger_setting_t * logger_setting = NULL; + for (size_t i = 0; i < log_levels->num_logger_settings; ++i) { + if (strcmp(log_levels->logger_settings[i].name, logger_name) == 0) { + logger_setting = &log_levels->logger_settings[i]; + if (logger_setting->level != log_level) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Minimum log level of logger [%s] will be replaced from %d to %d", + logger_name, logger_setting->level, log_level); + logger_setting->level = log_level; + } + return RCL_RET_OK; + } + } + + if (log_levels->num_logger_settings >= log_levels->capacity_logger_settings) { + RCL_SET_ERROR_MSG("No capacity to store a logger setting"); + return RCL_RET_ERROR; + } + + char * name = rcutils_strdup(logger_name, *allocator); + if (NULL == name) { + RCL_SET_ERROR_MSG("failed to copy logger name"); + return RCL_RET_BAD_ALLOC; + } + + logger_setting = &log_levels->logger_settings[log_levels->num_logger_settings]; + logger_setting->name = name; + logger_setting->level = log_level; + log_levels->num_logger_settings += 1; + return RCL_RET_OK; +} diff --git a/rcl/src/rcl/logging.c b/rcl/src/rcl/logging.c index 71c778388..972b685f6 100644 --- a/rcl/src/rcl/logging.c +++ b/rcl/src/rcl/logging.c @@ -63,7 +63,8 @@ rcl_logging_configure_with_output_handler( RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); RCUTILS_LOGGING_AUTOINIT g_logging_allocator = *allocator; - int default_level = global_args->impl->log_level; + int default_level = -1; + rcl_log_levels_t * log_levels = &global_args->impl->log_levels; const char * config_file = global_args->impl->external_log_config_file; g_rcl_logging_stdout_enabled = !global_args->impl->log_stdout_disabled; g_rcl_logging_rosout_enabled = !global_args->impl->log_rosout_disabled; @@ -71,8 +72,18 @@ rcl_logging_configure_with_output_handler( rcl_ret_t status = RCL_RET_OK; g_rcl_logging_num_out_handlers = 0; - if (default_level >= 0) { + if (log_levels) { + default_level = (int)log_levels->default_logger_level; rcutils_logging_set_default_logger_level(default_level); + + for (size_t i = 0; i < log_levels->num_logger_settings; ++i) { + rcutils_ret_t rcutils_status = rcutils_logging_set_logger_level( + log_levels->logger_settings[i].name, + (int)log_levels->logger_settings[i].level); + if (RCUTILS_RET_OK != rcutils_status) { + return RCL_RET_ERROR; + } + } } if (g_rcl_logging_stdout_enabled) { g_rcl_logging_out_handlers[g_rcl_logging_num_out_handlers++] = diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 383da2dc5..ae1e42af1 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(test_msgs REQUIRED) find_package(mimick_vendor REQUIRED) find_package(rcpputils REQUIRED) +find_package(rcutils REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(osrf_testing_tools_cpp REQUIRED) @@ -394,6 +395,13 @@ rcl_add_custom_gtest(test_common LIBRARIES ${PROJECT_NAME} ) +rcl_add_custom_gtest(test_log_level + SRCS rcl/test_log_level.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" +) + # Install test resources install(DIRECTORY ${test_resources_dir_name} DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/rcl/test/rcl/test_log_level.cpp b/rcl/test/rcl/test_log_level.cpp new file mode 100644 index 000000000..cddd8bac4 --- /dev/null +++ b/rcl/test/rcl/test_log_level.cpp @@ -0,0 +1,206 @@ +// Copyright 2020 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 + +#include "rcl/rcl.h" +#include "rcl/log_level.h" +#include "rcl/error_handling.h" +#include "rcutils/logging.h" + +#include "./arg_macros.hpp" + +int setup_and_parse_log_level_args(const char * log_level_string) +{ + rcl_arguments_t local_arguments = rcl_get_zero_initialized_arguments(); + const char * local_argv[] = {"process_name", "--ros-args", "--log-level", log_level_string}; + unsigned int local_argc = (sizeof(local_argv) / sizeof(const char *)); + return rcl_parse_arguments( + local_argc, local_argv, rcl_get_default_allocator(), &local_arguments); +} + +TEST(TestLogLevel, error_log_level) { + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args(":=debug")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("debug,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=debug,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=debug,,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args(":")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args(":=")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl=")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl=debug")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=:=")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl=debug,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:,")); + rcl_reset_error(); +} + +#define GET_LOG_LEVEL_FROM_ARGUMENTS(log_levels, ...) \ + { \ + rcl_ret_t ret; \ + rcl_arguments_t local_arguments = rcl_get_zero_initialized_arguments(); \ + const char * local_argv[] = {__VA_ARGS__}; \ + unsigned int local_argc = (sizeof(local_argv) / sizeof(const char *)); \ + ret = rcl_parse_arguments( \ + local_argc, local_argv, rcl_get_default_allocator(), &local_arguments); \ + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + ret = rcl_arguments_get_log_levels(&local_arguments, &log_levels); \ + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( \ + { \ + ASSERT_EQ(RCL_RET_OK, rcl_arguments_fini(&local_arguments)); \ + }); \ + } + +TEST(TestLogLevel, no_log_level) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS(log_levels, "process_name"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(0ul, log_levels.num_logger_settings); +} + +TEST(TestLogLevel, default_log_level) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.default_logger_level); + EXPECT_EQ(0ul, log_levels.num_logger_settings); +} + +TEST(TestLogLevel, logger_log_level_debug) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "rcl:=debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, logger_log_level_info) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "rcl:=info"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, multiple_log_level_with_default_at_front) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "debug", "--log-level", "rcl:=debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, multiple_log_level_with_default_at_back) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "rcl:=debug", "--log-level", "debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, multiple_log_level_rightmost_prevail) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "debug", "--log-level", "info", + "--log-level", "rcl:=debug", "--log-level", "rcl:=info"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, log_level_dot_logger_name) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "test.abc:=info"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("test.abc", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[0].level); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + // We need to do rcutils_logging_shutdown() since the tests above may + // have initialized logging. + if (rcutils_logging_shutdown() != RCUTILS_RET_OK) { + fprintf(stderr, "Failed shutting down rcutils logging\n"); + } + return ret; +}