diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index eef888492..34a860bcf 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -67,6 +67,7 @@ set(${PROJECT_NAME}_sources src/rcl/service.c src/rcl/service_event_publisher.c src/rcl/subscription.c + src/rcl/thread_attr.c src/rcl/time.c src/rcl/timer.c src/rcl/type_hash.c diff --git a/rcl/include/rcl/arguments.h b/rcl/include/rcl/arguments.h index 44c99f6a9..ee830bfe1 100644 --- a/rcl/include/rcl/arguments.h +++ b/rcl/include/rcl/arguments.h @@ -23,6 +23,7 @@ #include "rcl/types.h" #include "rcl/visibility_control.h" #include "rcl_yaml_param_parser/types.h" +#include "rcutils/thread_attr.h" #ifdef __cplusplus extern "C" @@ -83,6 +84,12 @@ typedef struct rcl_arguments_s /// logging (must be preceded with --enable- or --disable-). #define RCL_LOG_EXT_LIB_FLAG_SUFFIX "external-lib-logs" +/// The ROS flag that precedes the ROS thread attribute file path. +#define RCL_THREAD_ATTRS_FILE_FLAG "--thread-attrs-file" + +/// The ROS flag that precedes the ROS thread attribute. +#define RCL_THREAD_ATTRS_VALUE_FLAG "--thread-attrs-value" + /// Return a rcl_arguments_t struct with members initialized to `NULL`. RCL_PUBLIC RCL_WARN_UNUSED @@ -447,6 +454,24 @@ rcl_ret_t rcl_arguments_fini( rcl_arguments_t * args); +/// Return thread attribute parsed from the command line. +/** + * Thread attribute are parsed directly from command line arguments and + * thread attribute files provided in the command line. + * + * \param[in] arguments An arguments structure that has been parsed. + * \param[out] thread_attrs thread attribute as parsed from command line arguments. + * This structure must be finalized by the caller. + * \return #RCL_RET_OK if everything goes correctly, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_get_thread_attrs( + const rcl_arguments_t * arguments, + rcutils_thread_attrs_t ** thread_attrs); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/context.h b/rcl/include/rcl/context.h index 68500da81..4a40f4466 100644 --- a/rcl/include/rcl/context.h +++ b/rcl/include/rcl/context.h @@ -314,6 +314,16 @@ RCL_WARN_UNUSED rmw_context_t * rcl_context_get_rmw_context(rcl_context_t * context); +/// Returns pointer to the thread attribute list. +/** + * \param[in] context object from which the thread attribute list should be retrieved. + * \return pointer to thread attribute list if valid, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcutils_thread_attrs_t * +rcl_context_get_thread_attrs(const rcl_context_t * context); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/thread_attr.h b/rcl/include/rcl/thread_attr.h new file mode 100644 index 000000000..cdf107b1d --- /dev/null +++ b/rcl/include/rcl/thread_attr.h @@ -0,0 +1,66 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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. + +/// @file + +#ifndef RCL__THREAD_ATTR_H_ +#define RCL__THREAD_ATTR_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rcutils/thread_attr.h" + +extern const char * const RCL_THREAD_ATTR_VALUE_ENV_VAR; +extern const char * const RCL_THREAD_ATTR_FILE_ENV_VAR; + +/// Determine the default thread attribute from string, based on the environment. +/// \param[out] thread_attrs Must not be NULL. +/// \param[in] allocator memory allocator to be used +/// \return #RCL_RET_INVALID_ARGUMENT if an argument is invalid, or +/// \return #RCL_RET_ERROR in case of an unexpected error, or +/// \return #RCL_RET_BAD_ALLOC if allocating memory failed, or +/// \return #RCL_RET_OK. +RCL_PUBLIC +rcl_ret_t +rcl_get_default_thread_attrs_from_value( + rcutils_thread_attrs_t * thread_attrs, + rcl_allocator_t allocator); + +/// Determine the default thread attribute from file path, based on the environment. +/// \param[out] thread_attrs Must not be NULL. +/// \param[in] allocator memory allocator to be used +/// \return #RCL_RET_INVALID_ARGUMENT if an argument is invalid, or +/// \return #RCL_RET_ERROR in case of an unexpected error, or +/// \return #RCL_RET_BAD_ALLOC if allocating memory failed, or +/// \return #RCL_RET_OK. +RCL_PUBLIC +rcl_ret_t +rcl_get_default_thread_attrs_from_file( + rcutils_thread_attrs_t * thread_attrs, + rcl_allocator_t allocator); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__THREAD_ATTR_H_ diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index 753fc8eee..e3be92adc 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -111,6 +111,8 @@ typedef rmw_ret_t rcl_ret_t; #define RCL_RET_INVALID_PARAM_RULE 1010 /// Argument is not a valid log level rule #define RCL_RET_INVALID_LOG_LEVEL_RULE 1020 +/// Argument is not a valid thread attr rule +#define RCL_RET_INVALID_THREAD_ATTRS 1030 // rcl event specific ret codes in 20XX /// Invalid rcl_event_t given return code. diff --git a/rcl/src/rcl/arguments.c b/rcl/src/rcl/arguments.c index 8c3e7c0c7..2ed84b76b 100644 --- a/rcl/src/rcl/arguments.c +++ b/rcl/src/rcl/arguments.c @@ -25,6 +25,7 @@ #include "rcl/lexer_lookahead.h" #include "rcl/validate_topic_name.h" #include "rcl_yaml_param_parser/parser.h" +#include "rcl_yaml_param_parser/parser_thread_attr.h" #include "rcl_yaml_param_parser/types.h" #include "rcutils/allocator.h" #include "rcutils/error_handling.h" @@ -32,6 +33,7 @@ #include "rcutils/logging.h" #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" +#include "rcutils/thread_attr.h" #include "rmw/validate_namespace.h" #include "rmw/validate_node_name.h" @@ -286,6 +288,12 @@ rcl_parse_arguments( goto fail; } + args_impl->thread_attrs = rcutils_get_zero_initialized_thread_attrs(); + ret = rcutils_thread_attrs_init(&args_impl->thread_attrs, allocator); + if (RCL_RET_OK != ret) { + goto fail; + } + args_impl->parameter_overrides = rcl_yaml_node_struct_init(allocator); if (NULL == args_impl->parameter_overrides) { ret = RCL_RET_BAD_ALLOC; @@ -559,6 +567,75 @@ rcl_parse_arguments( RCL_DISABLE_FLAG_PREFIX, RCL_LOG_EXT_LIB_FLAG_SUFFIX, rcl_get_error_string().str); rcl_reset_error(); + // Attempt to parse argument as thread attribute flag + if (strcmp(RCL_THREAD_ATTRS_VALUE_FLAG, argv[i]) == 0) { + if (i + 1 < argc) { + if (args_impl->thread_attrs.num_attributes != 0) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Thread attributes already set: '%s %s'.", argv[i], argv[i + 1]); + ++i; + continue; + } + // Attempt to parse next argument as thread attribute rule + if (RCL_RET_OK == + rcl_parse_yaml_thread_attrs_value(argv[i + 1], &args_impl->thread_attrs)) + { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Got thread attributes value : %s\n", argv[i + 1]); + ++i; // Skip flag here, for loop will skip rule. + continue; + } + rcl_error_string_t prev_error_string = rcl_get_error_string(); + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse thread attributes value: '%s %s'. Error: %s", argv[i], argv[i + 1], + prev_error_string.str); + } else { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse trailing %s flag. No thread attributes value found.", argv[i]); + } + ret = RCL_RET_INVALID_ROS_ARGS; + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Arg %d (%s) is not a %s flag.", + i, argv[i], RCL_THREAD_ATTRS_VALUE_FLAG); + + // Attempt to parse argument as thread attribute file rule + if (strcmp(RCL_THREAD_ATTRS_FILE_FLAG, argv[i]) == 0) { + if (i + 1 < argc) { + if (args_impl->thread_attrs.num_attributes != 0) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Thread attributes already setted: '%s %s'.", argv[i], argv[i + 1]); + ++i; + continue; + } + // Attempt to parse next argument as thread attribute file rule + if ( + RCL_RET_OK == rcl_parse_yaml_thread_attrs_file( + argv[i + 1], &args_impl->thread_attrs)) + { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Got thread attributes file : %s\n", argv[i + 1]); + ++i; // Skip flag here, for loop will skip rule. + continue; + } + rcl_error_string_t prev_error_string = rcl_get_error_string(); + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse thread attributes file: '%s %s'. Error: %s", argv[i], argv[i + 1], + prev_error_string.str); + } else { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse trailing %s flag. No file path provided.", argv[i]); + } + ret = RCL_RET_INVALID_ROS_ARGS; + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Arg %d (%s) is not a %s flag.", + i, argv[i], RCL_THREAD_ATTRS_FILE_FLAG); + // Argument is an unknown ROS specific argument args_impl->unparsed_ros_args[args_impl->num_unparsed_ros_args] = i; ++(args_impl->num_unparsed_ros_args); @@ -925,6 +1002,18 @@ rcl_arguments_copy( return RCL_RET_BAD_ALLOC; } args_out->impl->enclave = enclave_copy; + + // Copy thread attributes + if (0 < args->impl->thread_attrs.num_attributes) { + rcutils_ret_t thread_attrs_ret = + rcutils_thread_attrs_copy(&args->impl->thread_attrs, &args_out->impl->thread_attrs); + if (RCUTILS_RET_OK != thread_attrs_ret) { + if (RCL_RET_OK != rcl_arguments_fini(args_out)) { + RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); + } + return RCL_RET_BAD_ALLOC; + } + } return RCL_RET_OK; } @@ -988,6 +1077,14 @@ rcl_arguments_fini( args->impl->external_log_config_file = NULL; } + rcl_ret_t thread_ret = rcutils_thread_attrs_fini(&args->impl->thread_attrs); + if (RCL_RET_OK != thread_ret) { + ret = thread_ret; + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Failed to finalize thread attribute while finalizing arguments. Continuing..."); + } + args->impl->allocator.deallocate(args->impl, args->impl->allocator.state); args->impl = NULL; return ret; @@ -2067,11 +2164,29 @@ _rcl_allocate_initialized_arguments_impl(rcl_arguments_t * args, rcl_allocator_t args_impl->log_rosout_disabled = false; args_impl->log_ext_lib_disabled = false; args_impl->enclave = NULL; + args_impl->thread_attrs = rcutils_get_zero_initialized_thread_attrs(); args_impl->allocator = *allocator; return RCL_RET_OK; } +rcl_ret_t +rcl_arguments_get_thread_attrs( + const rcl_arguments_t * arguments, + rcutils_thread_attrs_t ** thread_attrs) +{ + 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(thread_attrs, RCL_RET_INVALID_ARGUMENT); + + if (0 < arguments->impl->thread_attrs.num_attributes) { + *thread_attrs = &arguments->impl->thread_attrs; + return RCL_RET_OK; + } else { + return RCL_RET_ERROR; + } +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/arguments_impl.h b/rcl/src/rcl/arguments_impl.h index d3aedbb42..880775993 100644 --- a/rcl/src/rcl/arguments_impl.h +++ b/rcl/src/rcl/arguments_impl.h @@ -51,6 +51,9 @@ struct rcl_arguments_impl_s /// Length of remap_rules. int num_remap_rules; + /// thread attribute. + rcutils_thread_attrs_t thread_attrs; + /// Log levels parsed from arguments. rcl_log_levels_t log_levels; /// A file used to configure the external logging library diff --git a/rcl/src/rcl/context.c b/rcl/src/rcl/context.c index 5414cc5e9..cc8477b9e 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -24,6 +24,7 @@ extern "C" #include "./common.h" #include "./context_impl.h" #include "rcutils/stdatomic_helper.h" +#include "rcl_yaml_param_parser/parser_thread_attr.h" rcl_context_t rcl_get_zero_initialized_context(void) @@ -105,6 +106,22 @@ rcl_context_get_rmw_context(rcl_context_t * context) return &(context->impl->rmw_context); } +rcutils_thread_attrs_t * +rcl_context_get_thread_attrs(const rcl_context_t * context) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(context->impl, "context is zero-initialized", return NULL); + + rcutils_thread_attrs_t * attrs = NULL; + rcl_ret_t ret = rcl_arguments_get_thread_attrs(&context->global_arguments, &attrs); + if (RCL_RET_OK != ret) { + if (0 < context->impl->thread_attrs.num_attributes) { + attrs = &context->impl->thread_attrs; + } + } + return attrs; +} + rcl_ret_t __cleanup_context(rcl_context_t * context) { @@ -146,6 +163,21 @@ __cleanup_context(rcl_context_t * context) } } + // clean up thread_attrs_context + rcl_ret_t thread_attrs_context_fini_ret = + rcutils_thread_attrs_fini(&(context->impl->thread_attrs)); + if (RCL_RET_OK != thread_attrs_context_fini_ret) { + if (RCL_RET_OK == ret) { + ret = thread_attrs_context_fini_ret; + } + RCUTILS_SAFE_FWRITE_TO_STDERR( + "[rcl|context.c:" RCUTILS_STRINGIFY(__LINE__) + "] failed to finalize attr context while cleaning up context, memory may be leaked: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcutils_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + rcutils_reset_error(); + } + // clean up rmw_context if (NULL != context->impl->rmw_context.implementation_identifier) { rmw_ret_t rmw_context_fini_ret = rmw_context_fini(&(context->impl->rmw_context)); diff --git a/rcl/src/rcl/context_impl.h b/rcl/src/rcl/context_impl.h index 10c9e82cc..87c6908bf 100644 --- a/rcl/src/rcl/context_impl.h +++ b/rcl/src/rcl/context_impl.h @@ -15,6 +15,7 @@ #ifndef RCL__CONTEXT_IMPL_H_ #define RCL__CONTEXT_IMPL_H_ +#include "rcl_yaml_param_parser/parser_thread_attr.h" #include "rcl/context.h" #include "rcl/error_handling.h" @@ -38,6 +39,8 @@ struct rcl_context_impl_s char ** argv; /// rmw context. rmw_context_t rmw_context; + /// thread attributes. + rcutils_thread_attrs_t thread_attrs; }; RCL_LOCAL diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c index 85a36bf73..6923ce612 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -34,7 +34,9 @@ extern "C" #include "rcl/localhost.h" #include "rcl/logging.h" #include "rcl/security.h" +#include "rcl/thread_attr.h" #include "rcl/validate_enclave_name.h" +#include "rcl_yaml_param_parser/parser_thread_attr.h" #include "./arguments_impl.h" #include "./common.h" @@ -93,6 +95,9 @@ rcl_init( // Zero initialize rmw context first so its validity can by checked in cleanup. context->impl->rmw_context = rmw_get_zero_initialized_context(); + // Zero initialize thread attribute context first so its validity can by checked in cleanup. + context->impl->thread_attrs = rcutils_get_zero_initialized_thread_attrs(); + // Store the allocator. context->impl->allocator = allocator; @@ -256,6 +261,33 @@ rcl_init( "\t%s", discovery_options->static_peers[ii].peer_address); } + ret = rcutils_thread_attrs_init(&(context->impl->thread_attrs), allocator); + if (RCL_RET_OK != ret) { + fail_ret = ret; + goto fail; + } + + if (0 == context->global_arguments.impl->thread_attrs.num_attributes) { + // Get actual thread attribute based on environment variable. + ret = rcl_get_default_thread_attrs_from_value( + &(context->impl->thread_attrs), + allocator); + if (RCL_RET_OK != ret) { + fail_ret = ret; + goto fail; + } + if (0 == context->impl->thread_attrs.num_attributes) { + // Get actual thread attribute file path based on environment variable. + ret = rcl_get_default_thread_attrs_from_file( + &(context->impl->thread_attrs), + allocator); + if (RCL_RET_OK != ret) { + fail_ret = ret; + goto fail; + } + } + } + if (context->global_arguments.impl->enclave) { context->impl->init_options.impl->rmw_init_options.enclave = rcutils_strdup( context->global_arguments.impl->enclave, diff --git a/rcl/src/rcl/thread_attr.c b/rcl/src/rcl/thread_attr.c new file mode 100644 index 000000000..4933046a9 --- /dev/null +++ b/rcl/src/rcl/thread_attr.c @@ -0,0 +1,90 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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/thread_attr.h" +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcl/macros.h" +#include "rcl_yaml_param_parser/parser_thread_attr.h" +#include "rcutils/env.h" +#include "rcutils/strdup.h" +#include "rcutils/thread_attr.h" + +const char * const RCL_THREAD_ATTRS_FILE_ENV_VAR = "ROS_THREAD_ATTRS_FILE"; +const char * const RCL_THREAD_ATTRS_VALUE_ENV_VAR = "ROS_THREAD_ATTRS_VALUE"; + +rcl_ret_t +rcl_get_default_thread_attrs_from_value( + rcutils_thread_attrs_t * thread_attrs, + rcl_allocator_t allocator) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + + RCL_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT); + + const char * ros_thread_attrs_value = NULL; + const char * get_env_error_str = NULL; + + get_env_error_str = rcutils_get_env(RCL_THREAD_ATTRS_VALUE_ENV_VAR, &ros_thread_attrs_value); + if (NULL != get_env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var '" RCUTILS_STRINGIFY(RCL_THREAD_ATTRS_VALUE_ENV_VAR) "': %s\n", + get_env_error_str); + return RCL_RET_ERROR; + } + if (ros_thread_attrs_value && strcmp(ros_thread_attrs_value, "") != 0) { + rcl_ret_t ret = rcl_parse_yaml_thread_attrs_value(ros_thread_attrs_value, thread_attrs); + if (RCUTILS_RET_OK != ret) { + return ret; + } + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_get_default_thread_attrs_from_file( + rcutils_thread_attrs_t * thread_attrs, + rcl_allocator_t allocator) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + + RCL_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT); + + const char * ros_thread_attrs_file = NULL; + const char * get_env_error_str = NULL; + + get_env_error_str = + rcutils_get_env(RCL_THREAD_ATTRS_FILE_ENV_VAR, &ros_thread_attrs_file); + if (NULL != get_env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var '" RCUTILS_STRINGIFY(RCL_THREAD_ATTRS_FILE_ENV_VAR) "': %s\n", + get_env_error_str); + return RCL_RET_ERROR; + } + if (ros_thread_attrs_file && strcmp(ros_thread_attrs_file, "") != 0) { + rcutils_ret_t ret = rcl_parse_yaml_thread_attrs_file(ros_thread_attrs_file, thread_attrs); + if (RCUTILS_RET_OK != ret) { + return ret; + } + } + return RCL_RET_OK; +} diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 1b73ab5e6..ae128aace 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -81,6 +81,7 @@ function(test_target_function) SRCS rcl/test_context.cpp ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ LIBRARIES ${PROJECT_NAME} mimick osrf_testing_tools_cpp::memory_tools AMENT_DEPENDENCIES ${rmw_implementation} ) diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp index 02f8371a7..cbff38e3d 100644 --- a/rcl/test/rcl/test_arguments.cpp +++ b/rcl/test/rcl/test_arguments.cpp @@ -201,6 +201,16 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_known_vs_unkno EXPECT_FALSE(are_known_ros_args({"--ros-args", "stdout-logs"})); EXPECT_FALSE(are_known_ros_args({"--ros-args", "external-lib-logs"})); EXPECT_FALSE(are_known_ros_args({"--ros-args", "external-lib-logs"})); + + // Thread attributes + EXPECT_TRUE( + are_known_ros_args( + {"--ros-args", "--thread-attrs-file", + (test_path / "test_thread_attrs.yaml").string().c_str()})); + EXPECT_TRUE( + are_known_ros_args( + {"--ros-args", "--thread-attrs-value", + "[{priority: 10, scheduling_policy: FIFO, tag: attr-1, core_affinity: [1,2,3]}]"})); } bool @@ -226,7 +236,9 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_inval { "--ros-args", "-p", "foo:=bar", "-r", "__node:=node_name", "--params-file", parameters_filepath.c_str(), "--log-level", "INFO", - "--log-config-file", "file.config" + "--log-config-file", "file.config", + "--thread-attrs-value", + "[{priority: 10, scheduling_policy: IDLE, tag: attr-1, core_affinity: [1]}]" })); // ROS args unknown to rcl are not (necessarily) invalid diff --git a/rcl/test/rcl/test_context.cpp b/rcl/test/rcl/test_context.cpp index 31e4100b3..a5dd9417f 100644 --- a/rcl/test/rcl/test_context.cpp +++ b/rcl/test/rcl/test_context.cpp @@ -24,6 +24,11 @@ #include "../mocking_utils/patch.hpp" +#include "rcutils/thread_attr.h" + +#include "./arguments_impl.h" +#include "./context_impl.h" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) @@ -147,6 +152,49 @@ TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), nominal) { EXPECT_NE(rmw_context_ptr, nullptr) << rcl_get_error_string().str; rcl_reset_error(); + // test rcl_context_get_thread_attrs + rcutils_thread_attrs_t * thread_attrs; + EXPECT_NO_MEMORY_OPERATIONS( + { + thread_attrs = rcl_context_get_thread_attrs(nullptr); + }); + EXPECT_EQ(nullptr, thread_attrs); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_NO_MEMORY_OPERATIONS( + { + EXPECT_EQ(nullptr, rcl_context_get_thread_attrs(&context)); + }); + + { + rcutils_thread_attrs_t * arg_attrs = &context.global_arguments.impl->thread_attrs; + rcutils_thread_attrs_t * ctx_attrs = &context.impl->thread_attrs; + rcutils_ret_t ret; + rcutils_thread_core_affinity_t tmp_affinity = + rcutils_get_zero_initialized_thread_core_affinity(); + + ret = rcutils_thread_attrs_init(ctx_attrs, rcl_get_default_allocator()); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_thread_core_affinity_init(&tmp_affinity, rcl_get_default_allocator()); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_thread_attrs_add_attr( + ctx_attrs, RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, &tmp_affinity, 10, "arg"); + EXPECT_EQ(RCUTILS_RET_OK, ret); + + EXPECT_EQ(ctx_attrs, rcl_context_get_thread_attrs(&context)); + + ret = rcutils_thread_attrs_copy(ctx_attrs, arg_attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + + EXPECT_EQ(arg_attrs, rcl_context_get_thread_attrs(&context)); + + ret = rcutils_thread_attrs_fini(ctx_attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_thread_attrs_fini(arg_attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + } + ret = rcl_init_options_fini(&init_options); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } diff --git a/rcl/test/resources/test_arguments/test_thread_attrs.yaml b/rcl/test/resources/test_arguments/test_thread_attrs.yaml new file mode 100644 index 000000000..b1096dcc2 --- /dev/null +++ b/rcl/test/resources/test_arguments/test_thread_attrs.yaml @@ -0,0 +1,8 @@ +- tag: attr-1 + priority: 10 + scheduling_policy: FIFO + core_affinity: [1] +- tag: attr-2 + priority: 20 + scheduling_policy: RR + core_affinity: [2] diff --git a/rcl_yaml_param_parser/CMakeLists.txt b/rcl_yaml_param_parser/CMakeLists.txt index a5082ad1e..e2cc685fa 100644 --- a/rcl_yaml_param_parser/CMakeLists.txt +++ b/rcl_yaml_param_parser/CMakeLists.txt @@ -28,6 +28,8 @@ add_library( src/node_params.c src/parse.c src/parser.c + src/parse_thread_attr.c + src/parser_thread_attr.c src/yaml_variant.c ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -180,6 +182,31 @@ if(BUILD_TESTING) ) endif() + ament_add_gtest(test_parse_thread_attr + test/test_parse_thread_attr.cpp + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + if(TARGET test_parse_thread_attr) + target_link_libraries(test_parse_thread_attr + ${PROJECT_NAME} + osrf_testing_tools_cpp::memory_tools + rcutils::rcutils + yaml + ) + endif() + + ament_add_gtest(test_parser_thread_attr + test/test_parser_thread_attr.cpp + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + if(TARGET test_parser_thread_attr) + target_link_libraries(test_parser_thread_attr + ${PROJECT_NAME} + osrf_testing_tools_cpp::memory_tools + rcutils::rcutils + ) + endif() + add_performance_test(benchmark_parse_yaml test/benchmark/benchmark_parse_yaml.cpp WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") if(TARGET benchmark_parse_yaml) diff --git a/rcl_yaml_param_parser/include/rcl_yaml_param_parser/parser_thread_attr.h b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/parser_thread_attr.h new file mode 100644 index 000000000..bbaf21ca5 --- /dev/null +++ b/rcl_yaml_param_parser/include/rcl_yaml_param_parser/parser_thread_attr.h @@ -0,0 +1,56 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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_YAML_PARAM_PARSER__PARSER_THREAD_ATTR_H_ +#define RCL_YAML_PARAM_PARSER__PARSER_THREAD_ATTR_H_ + +#include + + +#include "rcl_yaml_param_parser/visibility_control.h" + +#include "rcutils/allocator.h" +#include "rcutils/macros.h" +#include "rcutils/thread_attr.h" +#include "rcutils/types/rcutils_ret.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// \brief Parse the YAML file and populate \p thread_attrs +/// \pre Given \p thread_attrs must be a valid thread attribute struct +/// \param[in] file_path is the path to the YAML file +/// \param[in,out] thread_attrs points to the struct to be populated +/// \return true on success and false on failure +RCL_YAML_PARAM_PARSER_PUBLIC +rcutils_ret_t rcl_parse_yaml_thread_attrs_file( + const char * file_path, + rcutils_thread_attrs_t * thread_attrs); + +/// \brief Parse a thread attribute value as a YAML string, updating thread_attrs accordingly +/// \param[in] yaml_value is the thread attribute value as a YAML string to be parsed +/// \param[in,out] thread_attrs points to the thread attribute struct +/// \return true on success and false on failure +RCL_YAML_PARAM_PARSER_PUBLIC +rcutils_ret_t rcl_parse_yaml_thread_attrs_value( + const char * yaml_value, + rcutils_thread_attrs_t * thread_attrs); + +#ifdef __cplusplus +} +#endif + +#endif // RCL_YAML_PARAM_PARSER__PARSER_THREAD_ATTR_H_ diff --git a/rcl_yaml_param_parser/src/impl/parse_thread_attr.h b/rcl_yaml_param_parser/src/impl/parse_thread_attr.h new file mode 100644 index 000000000..6a5a3485e --- /dev/null +++ b/rcl_yaml_param_parser/src/impl/parse_thread_attr.h @@ -0,0 +1,60 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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 IMPL__PARSE_THREAD_ATTR_H_ +#define IMPL__PARSE_THREAD_ATTR_H_ + +#include + +#include "rcutils/allocator.h" +#include "rcutils/macros.h" +#include "rcutils/thread_attr.h" +#include "rcutils/types/rcutils_ret.h" + +#include "./types.h" +#include "rcl_yaml_param_parser/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t parse_thread_attr_key( + const char * value, + thread_attr_key_type_t * key_type); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_thread_scheduling_policy_t parse_thread_attr_scheduling_policy( + const char * value); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t parse_thread_attr( + yaml_parser_t * parser, + rcutils_thread_attrs_t * attrs); + +RCL_YAML_PARAM_PARSER_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t parse_thread_attr_events( + yaml_parser_t * parser, + rcutils_thread_attrs_t * thread_attrs); + +#ifdef __cplusplus +} +#endif + +#endif // IMPL__PARSE_THREAD_ATTR_H_ diff --git a/rcl_yaml_param_parser/src/impl/types.h b/rcl_yaml_param_parser/src/impl/types.h index 4776dc128..aae3e384b 100644 --- a/rcl_yaml_param_parser/src/impl/types.h +++ b/rcl_yaml_param_parser/src/impl/types.h @@ -63,6 +63,24 @@ typedef struct namespace_tracker_s uint32_t num_parameter_ns; } namespace_tracker_t; +typedef enum thread_attr_key_type_e +{ + THREAD_ATTR_KEY_CORE_AFFINITY = 1, + THREAD_ATTR_KEY_SCHEDULING_POLICY = 2, + THREAD_ATTR_KEY_PRIORITY = 4, + THREAD_ATTR_KEY_TAG = 8 +} thread_attr_key_type_t; + +typedef enum thread_attr_key_bits_e +{ + THREAD_ATTR_KEY_BITS_NONE = 0, + THREAD_ATTR_KEY_BITS_ALL = + THREAD_ATTR_KEY_CORE_AFFINITY | + THREAD_ATTR_KEY_SCHEDULING_POLICY | + THREAD_ATTR_KEY_PRIORITY | + THREAD_ATTR_KEY_TAG +} thread_attr_key_bits_t; + #ifdef __cplusplus } #endif diff --git a/rcl_yaml_param_parser/src/parse_thread_attr.c b/rcl_yaml_param_parser/src/parse_thread_attr.c new file mode 100644 index 000000000..88eca2bb6 --- /dev/null +++ b/rcl_yaml_param_parser/src/parse_thread_attr.c @@ -0,0 +1,319 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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 + +#include + +#include "rcl_yaml_param_parser/parser_thread_attr.h" + +#include "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/format_string.h" +#include "rcutils/strdup.h" +#include "rcutils/thread_attr.h" + +#include "./impl/parse.h" // to use get_value +#include "./impl/parse_thread_attr.h" + +#define PARSE_WITH_CHECK_ERROR() \ + do { \ + int _parse_ret_; \ + _parse_ret_ = yaml_parser_parse(parser, &event); \ + if (0 == _parse_ret_) { \ + RCUTILS_SET_ERROR_MSG("Failed to parse thread attributes"); \ + ret = RCUTILS_RET_ERROR; \ + goto error; \ + } \ + } while (0) + +#define PARSE_WITH_CHECK_EVENT(event_type) \ + do { \ + PARSE_WITH_CHECK_ERROR(); \ + if (event.type != (event_type)) { \ + RCUTILS_SET_ERROR_MSG("Unexpected element in a configuration of thread attributes"); \ + ret = RCUTILS_RET_ERROR; \ + goto error; \ + } \ + } while (0) + +/// +/// Parse the key part of a thread attribute +/// +rcutils_ret_t +parse_thread_attr_key( + const char * str, + thread_attr_key_type_t * key_type) +{ + rcutils_ret_t ret; + if (strcmp(str, "core_affinity") == 0) { + *key_type = THREAD_ATTR_KEY_CORE_AFFINITY; + } else if (strcmp(str, "priority") == 0) { + *key_type = THREAD_ATTR_KEY_PRIORITY; + } else if (strcmp(str, "scheduling_policy") == 0) { + *key_type = THREAD_ATTR_KEY_SCHEDULING_POLICY; + } else if (strcmp(str, "tag") == 0) { + *key_type = THREAD_ATTR_KEY_TAG; + } else if (*str == '\0') { + RCUTILS_SET_ERROR_MSG("empty tag for a thread attribute"); + ret = RCUTILS_RET_ERROR; + goto error; + } else { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("unrecognized key for a thread attribute: %s", str); + ret = RCUTILS_RET_ERROR; + goto error; + } + return RCUTILS_RET_OK; + +error: + return ret; +} + +/// +/// Parse the value of the scheduling policy of a thread attribute +/// +rcutils_thread_scheduling_policy_t parse_thread_attr_scheduling_policy( + const char * value) +{ + rcutils_thread_scheduling_policy_t ret; + if (strcmp(value, "FIFO") == 0) { + ret = RCUTILS_THREAD_SCHEDULING_POLICY_FIFO; + } else if (strcmp(value, "RR") == 0) { + ret = RCUTILS_THREAD_SCHEDULING_POLICY_RR; + } else if (strcmp(value, "SPORADIC") == 0) { + ret = RCUTILS_THREAD_SCHEDULING_POLICY_SPORADIC; + } else if (strcmp(value, "OTHER") == 0) { + ret = RCUTILS_THREAD_SCHEDULING_POLICY_OTHER; + } else if (strcmp(value, "IDLE") == 0) { + ret = RCUTILS_THREAD_SCHEDULING_POLICY_IDLE; + } else if (strcmp(value, "BATCH") == 0) { + ret = RCUTILS_THREAD_SCHEDULING_POLICY_BATCH; + } else if (strcmp(value, "DEADLINE") == 0) { + ret = RCUTILS_THREAD_SCHEDULING_POLICY_DEADLINE; + } else { + ret = RCUTILS_THREAD_SCHEDULING_POLICY_UNKNOWN; + } + return ret; +} + +/// +/// parse a thread attribute YAML value string and process them +/// +rcutils_ret_t parse_thread_attr( + yaml_parser_t * parser, + rcutils_thread_attrs_t * attrs) +{ + rcutils_ret_t ret; + yaml_event_t event; + thread_attr_key_bits_t key_bits = THREAD_ATTR_KEY_BITS_NONE; + rcutils_thread_scheduling_policy_t sched_policy; + rcutils_thread_core_affinity_t core_affinity = + rcutils_get_zero_initialized_thread_core_affinity(); + int priority; + char const * tag = NULL; + rcutils_allocator_t allocator = attrs->allocator; + void * ret_val = NULL; + + while (1) { + PARSE_WITH_CHECK_ERROR(); + + if (YAML_MAPPING_END_EVENT == event.type) { + break; + } + if (YAML_SCALAR_EVENT != event.type) { + RCUTILS_SET_ERROR_MSG("Unexpected element in a configuration of thread attributes"); + ret = RCUTILS_RET_ERROR; + goto error; + } + + const char * str = (char *)event.data.scalar.value; + thread_attr_key_type_t key_type; + ret = parse_thread_attr_key(str, &key_type); + if (RCUTILS_RET_OK != ret) { + goto error; + } + if (key_bits & (thread_attr_key_bits_t)key_type) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING("duplicated key for a thread attribute: %s", str); + ret = RCUTILS_RET_ERROR; + goto error; + } + + PARSE_WITH_CHECK_ERROR(); + + const char * value = (char *)event.data.scalar.value; + yaml_scalar_style_t style = event.data.scalar.style; + const yaml_char_t * yaml_tag = event.data.scalar.tag; + data_types_t val_type; + + switch (key_type) { + case THREAD_ATTR_KEY_CORE_AFFINITY: + if (event.type != YAML_SEQUENCE_START_EVENT) { + ret = RCUTILS_RET_ERROR; + goto error; + } + ret = rcutils_thread_core_affinity_init(&core_affinity, allocator); + if (RCUTILS_RET_OK != ret) { + goto error; + } + while (1) { + PARSE_WITH_CHECK_ERROR(); + if (YAML_SEQUENCE_END_EVENT == event.type) { + break; + } + value = (char *)event.data.scalar.value; + style = event.data.scalar.style; + yaml_tag = event.data.scalar.tag; + ret_val = get_value(value, style, yaml_tag, &val_type, allocator); + if (DATA_TYPE_INT64 != val_type) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Unrecognized value for thread core affinity: %s", value); + ret = RCUTILS_RET_ERROR; + goto error; + } + size_t core_no = ((size_t)*(int64_t *)(ret_val)); + allocator.deallocate(ret_val, allocator.state); + ret_val = NULL; + ret = rcutils_thread_core_affinity_set(&core_affinity, core_no); + if (RCUTILS_RET_OK != ret) { + goto error; + } + } + break; + case THREAD_ATTR_KEY_PRIORITY: + if (event.type != YAML_SCALAR_EVENT) { + goto error; + } + ret_val = get_value(value, style, yaml_tag, &val_type, allocator); + if (DATA_TYPE_INT64 != val_type) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Unrecognized value for thread priority: %s", value); + ret = RCUTILS_RET_ERROR; + goto error; + } + priority = ((int)*(int64_t *)(ret_val)); + break; + case THREAD_ATTR_KEY_SCHEDULING_POLICY: + if (event.type != YAML_SCALAR_EVENT) { + goto error; + } + sched_policy = parse_thread_attr_scheduling_policy(value); + break; + case THREAD_ATTR_KEY_TAG: + if (event.type != YAML_SCALAR_EVENT) { + goto error; + } + if (*value == '\0') { + RCUTILS_SET_ERROR_MSG("Empty thread attribute tag"); + ret = RCUTILS_RET_ERROR; + goto error; + } + tag = rcutils_strdup(value, allocator); + if (NULL == tag) { + ret = RCUTILS_RET_BAD_ALLOC; + goto error; + } + break; + } + + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + ret_val = NULL; + } + + key_bits |= (thread_attr_key_bits_t)key_type; + } + + if (THREAD_ATTR_KEY_BITS_ALL != key_bits) { + RCUTILS_SET_ERROR_MSG("A thread attribute does not have enough parameters"); + ret = RCUTILS_RET_ERROR; + goto error; + } + + ret = rcutils_thread_attrs_add_attr(attrs, sched_policy, &core_affinity, priority, tag); + if (RCUTILS_RET_OK != ret) { + goto error; + } + + allocator.deallocate((char *)tag, allocator.state); + + return RCUTILS_RET_OK; + +error: + if (NULL != tag) { + allocator.deallocate((char *)tag, allocator.state); + } + if (NULL != ret_val) { + allocator.deallocate(ret_val, allocator.state); + } + if (0 < core_affinity.core_count) { + rcutils_ret_t tmp_ret = + rcutils_thread_core_affinity_fini(&core_affinity); + (void)tmp_ret; + } + return ret; +} + +/// +/// Get events from parsing thread attributes YAML value string and process them +/// +rcutils_ret_t parse_thread_attr_events( + yaml_parser_t * parser, + rcutils_thread_attrs_t * thread_attrs) +{ + yaml_event_t event; + rcutils_ret_t ret; + + RCUTILS_CHECK_ARGUMENT_FOR_NULL(parser, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); + + PARSE_WITH_CHECK_EVENT(YAML_STREAM_START_EVENT); + PARSE_WITH_CHECK_EVENT(YAML_DOCUMENT_START_EVENT); + PARSE_WITH_CHECK_EVENT(YAML_SEQUENCE_START_EVENT); + + while (1) { + PARSE_WITH_CHECK_ERROR(); + + if (YAML_SEQUENCE_END_EVENT == event.type) { + break; + } else if (YAML_MAPPING_START_EVENT != event.type) { + RCUTILS_SET_ERROR_MSG("Unexpected element in a configuration of thread attributes"); + ret = RCUTILS_RET_ERROR; + goto error; + } + + ret = parse_thread_attr(parser, thread_attrs); + if (RCUTILS_RET_OK != ret) { + goto error; + } + } + PARSE_WITH_CHECK_EVENT(YAML_DOCUMENT_END_EVENT); + PARSE_WITH_CHECK_EVENT(YAML_STREAM_END_EVENT); + + if (0 == thread_attrs->num_attributes) { + RCUTILS_SET_ERROR_MSG("No thread attributes."); + ret = RCUTILS_RET_ERROR; + goto error; + } + + return RCUTILS_RET_OK; + +error: + if (0 < thread_attrs->capacity_attributes) { + rcutils_ret_t thread_attrs_ret = rcutils_thread_attrs_fini(thread_attrs); + (void)thread_attrs_ret; + // Since an error has already occurred, ignore this result. + } + return ret; +} diff --git a/rcl_yaml_param_parser/src/parser_thread_attr.c b/rcl_yaml_param_parser/src/parser_thread_attr.c new file mode 100644 index 000000000..abddba9b5 --- /dev/null +++ b/rcl_yaml_param_parser/src/parser_thread_attr.c @@ -0,0 +1,106 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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 +#include +#include + +#include + +#include "rcl_yaml_param_parser/parser_thread_attr.h" +#include "rcl_yaml_param_parser/types.h" + +#include "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/thread_attr.h" +#include "rcutils/types/rcutils_ret.h" + +#include "./impl/types.h" +#include "./impl/parse.h" +#include "./impl/parse_thread_attr.h" +#include "./impl/node_params.h" +#include "./impl/yaml_variant.h" + +/// +/// Parse the YAML file and populate thread_attrs +/// +rcutils_ret_t rcl_parse_yaml_thread_attrs_file( + const char * file_path, + rcutils_thread_attrs_t * thread_attrs) +{ + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + file_path, "YAML file path is NULL", return RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); + + if (0 < thread_attrs->num_attributes) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Pass an initialized thread attr structure"); + return RCUTILS_RET_ERROR; + } + + yaml_parser_t parser; + int success = yaml_parser_initialize(&parser); + if (0 == success) { + RCUTILS_SET_ERROR_MSG("Could not initialize the parser"); + return RCUTILS_RET_ERROR; + } + + FILE * yaml_file = fopen(file_path, "r"); + if (NULL == yaml_file) { + yaml_parser_delete(&parser); + RCUTILS_SET_ERROR_MSG("Error opening YAML file"); + return RCUTILS_RET_ERROR; + } + + yaml_parser_set_input_file(&parser, yaml_file); + rcutils_ret_t ret = parse_thread_attr_events(&parser, thread_attrs); + + fclose(yaml_file); + + yaml_parser_delete(&parser); + + return ret; +} + +/// +/// Parse a YAML string and populate thread_attrs +/// +rcutils_ret_t rcl_parse_yaml_thread_attrs_value( + const char * yaml_value, + rcutils_thread_attrs_t * thread_attrs) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(yaml_value, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); + + if (0 < thread_attrs->num_attributes) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Pass an initialized thread attr structure"); + return RCUTILS_RET_ERROR; + } + + yaml_parser_t parser; + int success = yaml_parser_initialize(&parser); + if (0 == success) { + RCUTILS_SET_ERROR_MSG("Could not initialize the parser"); + return RCUTILS_RET_ERROR; + } + + yaml_parser_set_input_string( + &parser, (const unsigned char *)yaml_value, strlen(yaml_value)); + + rcutils_ret_t ret = parse_thread_attr_events(&parser, thread_attrs); + + yaml_parser_delete(&parser); + + return ret; +} diff --git a/rcl_yaml_param_parser/test/test_parse_thread_attr.cpp b/rcl_yaml_param_parser/test/test_parse_thread_attr.cpp new file mode 100644 index 000000000..058c09ffa --- /dev/null +++ b/rcl_yaml_param_parser/test/test_parse_thread_attr.cpp @@ -0,0 +1,174 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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 +#include +#include +#include + +#include "rcutils/thread_attr.h" +#include "rcutils/error_handling.h" + +#include "../src/impl/parse_thread_attr.h" + +struct TestParseThreadAttrs : testing::Test +{ + void SetUp() override + { + rcutils_reset_error(); + + rcutils_ret_t ret; + rcutils_allocator_t allocator; + attrs = rcutils_get_zero_initialized_thread_attrs(); + allocator = rcutils_get_default_allocator(); + ret = rcutils_thread_attrs_init(&attrs, allocator); + ASSERT_EQ(RCUTILS_RET_OK, ret); + + int parser_ret = yaml_parser_initialize(&parser); + ASSERT_NE(0, parser_ret); + } + void TearDown() override + { + yaml_parser_delete(&parser); + rcutils_ret_t ret = rcutils_thread_attrs_fini(&attrs); + ASSERT_EQ(RCUTILS_RET_OK, ret); + } + + void prepare_yaml_parser(char const * yaml_value) + { + yaml_parser_set_input_string(&parser, (const unsigned char *)yaml_value, strlen(yaml_value)); + } + + yaml_parser_t parser; + rcutils_thread_attrs_t attrs; +}; + +struct TestParseThreadAttr : TestParseThreadAttrs +{ + void prepare_yaml_parser(char const * yaml_value) + { + TestParseThreadAttrs::prepare_yaml_parser(yaml_value); + + yaml_event_t event; + int ret; + ret = yaml_parser_parse(&parser, &event); + ASSERT_NE(0, ret); + ret = yaml_parser_parse(&parser, &event); + ASSERT_NE(0, ret); + ret = yaml_parser_parse(&parser, &event); + ASSERT_NE(0, ret); + } +}; + +TEST_F(TestParseThreadAttr, success) { + rcutils_ret_t ret; + yaml_event_t event; + + prepare_yaml_parser( + "{ priority: 10, tag: attr-1, core_affinity: [1], scheduling_policy: FIFO }"); + + ret = parse_thread_attr(&parser, &attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + + EXPECT_EQ(1, attrs.num_attributes); + EXPECT_EQ(10, attrs.attributes[0].priority); + EXPECT_STREQ("attr-1", attrs.attributes[0].tag); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[0].core_affinity, 1)); + EXPECT_EQ(RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, attrs.attributes[0].scheduling_policy); + + int parser_ret; + parser_ret = yaml_parser_parse(&parser, &event); + ASSERT_NE(0, parser_ret); + EXPECT_EQ(YAML_DOCUMENT_END_EVENT, event.type); + parser_ret = yaml_parser_parse(&parser, &event); + ASSERT_NE(0, parser_ret); + EXPECT_EQ(YAML_STREAM_END_EVENT, event.type); +} + +TEST_F(TestParseThreadAttr, unknown_key) { + rcutils_ret_t ret; + + prepare_yaml_parser( + "{ priority: 10, tag: attr-1, core_affinity: [1], unknown_key: FIFO }"); + + ret = parse_thread_attr(&parser, &attrs); + EXPECT_EQ(RCUTILS_RET_ERROR, ret); +} + +TEST_F(TestParseThreadAttr, all_valid_keys_with_unknown_key) { + rcutils_ret_t ret; + + prepare_yaml_parser( + "{ priority: 10, tag: attr-1, core_affinity: [1], " + "scheduling_policy: FIFO, unknown_key: RR }"); + + ret = parse_thread_attr(&parser, &attrs); + EXPECT_EQ(RCUTILS_RET_ERROR, ret); +} + +TEST_F(TestParseThreadAttr, missing_key_value) { + rcutils_ret_t ret; + prepare_yaml_parser( + "{ priority: 10, tag: attr-1 }"); + + ret = parse_thread_attr(&parser, &attrs); + EXPECT_EQ(RCUTILS_RET_ERROR, ret); +} + +TEST_F(TestParseThreadAttrs, success) { + rcutils_ret_t ret; + + std::stringstream ss; + ss << "["; + for (std::size_t i = 0; i < 100; ++i) { + ss << "{ priority: " << i * 10; + ss << ", tag: attr-" << i; + ss << ", core_affinity: [" << i << "]"; + ss << ", scheduling_policy: FIFO },"; + } + ss << "]"; + + std::string buf = ss.str(); + prepare_yaml_parser(buf.c_str()); + + ret = parse_thread_attr_events(&parser, &attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ASSERT_EQ(100, attrs.num_attributes); + + for (std::size_t i = 0; i < 100; ++i) { + EXPECT_EQ(i * 10, attrs.attributes[i].priority); + ss.str(""); + ss << "attr-" << i; + buf = ss.str(); + EXPECT_STREQ(buf.c_str(), attrs.attributes[i].tag); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[i].core_affinity, i)); + EXPECT_EQ(RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, attrs.attributes[i].scheduling_policy); + } +} + +TEST_F(TestParseThreadAttr, affinity_multiple_core) { + rcutils_ret_t ret; + prepare_yaml_parser( + "{ priority: 10, tag: attr-1, core_affinity: [1,2,3], scheduling_policy: FIFO }"); + + ret = parse_thread_attr(&parser, &attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_FALSE(rcutils_thread_core_affinity_is_set(&attrs.attributes[0].core_affinity, 0)); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[0].core_affinity, 1)); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[0].core_affinity, 2)); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[0].core_affinity, 3)); + EXPECT_FALSE(rcutils_thread_core_affinity_is_set(&attrs.attributes[0].core_affinity, 4)); +} diff --git a/rcl_yaml_param_parser/test/test_parser_thread_attr.cpp b/rcl_yaml_param_parser/test/test_parser_thread_attr.cpp new file mode 100644 index 000000000..6d8668a0a --- /dev/null +++ b/rcl_yaml_param_parser/test/test_parser_thread_attr.cpp @@ -0,0 +1,130 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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 +#include +#include + +#include "rcutils/error_handling.h" +#include "rcutils/filesystem.h" +#include "rcutils/thread_attr.h" + +#include "rcl_yaml_param_parser/parser_thread_attr.h" + +struct TestParserThreadAttr : testing::Test +{ + void SetUp() override + { + rcutils_ret_t ret; + path = nullptr; + + rcutils_reset_error(); + attrs = rcutils_get_zero_initialized_thread_attrs(); + alloc = rcutils_get_default_allocator(); + ret = rcutils_thread_attrs_init(&attrs, alloc); + ASSERT_EQ(RCUTILS_RET_OK, ret); + } + + void prepare_thread_attr_file(char const * filename) + { + char buf[1024]; + bool ret = rcutils_get_cwd(buf, sizeof(buf)); + ASSERT_TRUE(ret); + + path = rcutils_join_path(buf, "test/", alloc); + ASSERT_NE(nullptr, path); + path = rcutils_join_path(path, filename, alloc); + ASSERT_NE(nullptr, path); + } + + void TearDown() override + { + rcutils_ret_t ret; + if (path) { + alloc.deallocate(path, alloc.state); + } + ret = rcutils_thread_attrs_fini(&attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + } + rcutils_thread_attrs_t attrs; + rcutils_allocator_t alloc; + char * path; +}; + +static const rcutils_thread_scheduling_policy_t expected_policies[] = { + RCUTILS_THREAD_SCHEDULING_POLICY_UNKNOWN, + RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, + RCUTILS_THREAD_SCHEDULING_POLICY_RR, + RCUTILS_THREAD_SCHEDULING_POLICY_SPORADIC, + RCUTILS_THREAD_SCHEDULING_POLICY_OTHER, + RCUTILS_THREAD_SCHEDULING_POLICY_IDLE, + RCUTILS_THREAD_SCHEDULING_POLICY_BATCH, + RCUTILS_THREAD_SCHEDULING_POLICY_DEADLINE, + RCUTILS_THREAD_SCHEDULING_POLICY_UNKNOWN, + RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, +}; + +TEST_F(TestParserThreadAttr, success_file) { + rcutils_ret_t ret; + + prepare_thread_attr_file("thread_attr_success.yaml"); + + ret = rcl_parse_yaml_thread_attrs_file(path, &attrs); + ASSERT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(10, attrs.num_attributes); + + for (size_t i = 0; i < 10; ++i) { + EXPECT_EQ(attrs.attributes[i].priority, i * 10); + char buf[32]; + snprintf(buf, sizeof(buf), "attr-%lu", i); + EXPECT_STREQ(buf, attrs.attributes[i].tag); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[i].core_affinity, i)); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[i].core_affinity, i + 10)); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[i].core_affinity, i * i)); + EXPECT_EQ(expected_policies[i], attrs.attributes[i].scheduling_policy); + } +} + +TEST_F(TestParserThreadAttr, success_value) { + rcutils_ret_t ret; + + prepare_thread_attr_file("thread_attr_success.yaml"); + + std::ifstream ifs(path); + std::stringstream ss; + ss << ifs.rdbuf(); + + ret = rcl_parse_yaml_thread_attrs_value(ss.str().c_str(), &attrs); + ASSERT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(10, attrs.num_attributes); + + for (size_t i = 0; i < 10; ++i) { + EXPECT_EQ(attrs.attributes[i].priority, i * 10); + char buf[32]; + snprintf(buf, sizeof(buf), "attr-%lu", i); + EXPECT_STREQ(buf, attrs.attributes[i].tag); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[i].core_affinity, i)); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[i].core_affinity, i + 10)); + EXPECT_TRUE(rcutils_thread_core_affinity_is_set(&attrs.attributes[i].core_affinity, i * i)); + EXPECT_EQ(expected_policies[i], attrs.attributes[i].scheduling_policy); + } +} + +TEST_F(TestParserThreadAttr, bad_file_path) { + rcutils_ret_t ret = rcl_parse_yaml_thread_attrs_file("not_exist.yaml", &attrs); + + EXPECT_EQ(RCUTILS_RET_ERROR, ret); +} diff --git a/rcl_yaml_param_parser/test/thread_attr_success.yaml b/rcl_yaml_param_parser/test/thread_attr_success.yaml new file mode 100644 index 000000000..062c98a97 --- /dev/null +++ b/rcl_yaml_param_parser/test/thread_attr_success.yaml @@ -0,0 +1,40 @@ +- priority: 0 + tag: attr-0 + core_affinity: [0,10,0] + scheduling_policy: BADSCHEDPOLICY1 +- priority: 10 + tag: attr-1 + core_affinity: [1,11,1] + scheduling_policy: FIFO +- priority: 20 + tag: attr-2 + core_affinity: [2,12,4] + scheduling_policy: RR +- priority: 30 + tag: attr-3 + core_affinity: [3,13,9] + scheduling_policy: SPORADIC +- priority: 40 + tag: attr-4 + core_affinity: [4,14,16] + scheduling_policy: OTHER +- priority: 50 + tag: attr-5 + core_affinity: [5,15,25] + scheduling_policy: IDLE +- priority: 60 + tag: attr-6 + core_affinity: [6,16,36] + scheduling_policy: BATCH +- priority: 70 + tag: attr-7 + core_affinity: [7,17,49] + scheduling_policy: DEADLINE +- priority: 80 + tag: attr-8 + core_affinity: [8,18,64] + scheduling_policy: BADSCHEDPOLICY2 +- priority: 90 + tag: attr-9 + core_affinity: [9,19,81] + scheduling_policy: FIFO