feat: Thread configuration prototype

This is a prototype implementation of RCL for discussion about the thread configuration feature to receive and apply a set of scheduling parameters for the threads controlled by the ROS 2 executor. Our basic idea is as below. 1. Implement a new class rclcpp::thread and modify rclcpp to use it. This class has the same function set as the std::thread but also additional features to control its thread attributions. 2. Modify the rcl layer to receive a set of scheduling parameters. The parameters are described in YAML format and passed via command line parameters, environment variables, or files. 3. the rclcpp reads the parameters from rcl and applies them to each thread in the thread pool. There have been some discussions about this pull request, as below. [ROS Discourse] https://discourse.ros.org/t/adding-thread-attributes-configuration-in-ros-2-framework/30701 [ROS 2 Real-Time Working Group] ros-realtime/ros-realtime.github.io#18 Signed-off-by: Shoji Morita <[email protected]>
esol-community · Jun 6, 2023 · 73718ca · 73718ca
1 parent 1cb1209
commit 73718ca
Show file tree

Hide file tree

Showing 19 changed files with 1,095 additions and 0 deletions.
diff --git 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
@@ -83,6 +83,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 logging 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 +453,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,
+  rcl_thread_attrs_t ** thread_attrs);
+
 #ifdef __cplusplus
 }
 #endif

diff --git a/rcl/include/rcl/context.h b/rcl/include/rcl/context.h
@@ -314,6 +314,23 @@ RCL_WARN_UNUSED
 rmw_context_t *
 rcl_context_get_rmw_context(rcl_context_t * context);
 
+/// Returns the thread attribute context.
+/**
+ * \param[in] context from which the thread attribute should be retrieved.
+ * \param[out] thread_attrs output variable where the thread attribute will be returned.
+ * \return #RCL_RET_INVALID_ARGUMENT if `context` is invalid (see rcl_context_is_valid()), or
+ * \return #RCL_RET_INVALID_ARGUMENT if `context->impl` is `NULL`, or
+ * \return #RCL_RET_INVALID_ARGUMENT if `*thread_attrs` is not `NULL`, or
+ * \return #RCL_RET_INVALID_ARGUMENT if `context->impl->thread_context.thread_attrs` is `NULL`, or
+ * \return #RCL_RET_OK if the thread attribute was correctly retrieved.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_context_get_thread_attrs(
+  const rcl_context_t * context,
+  rcl_thread_attrs_t ** thread_attrs);
+
 #ifdef __cplusplus
 }
 #endif

diff --git a/rcl/include/rcl/thread_attr.h 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 <stddef.h>
+
+#include "rcl/allocator.h"
+#include "rcl/macros.h"
+#include "rcl/types.h"
+#include "rcl/visibility_control.h"
+#include "rcl_yaml_param_parser/types.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(
+  rcl_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(
+  rcl_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
@@ -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
@@ -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"
@@ -286,6 +287,12 @@ rcl_parse_arguments(
     goto fail;
   }
 
+  args_impl->thread_attrs = rcl_get_zero_initialized_thread_attrs();
+  ret = rcl_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 +566,73 @@ 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 attribute rule : %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 attribute rule: '%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 attribute rule 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))
+          {
+            ++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 attr 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);
@@ -988,6 +1062,14 @@ rcl_arguments_fini(
       args->impl->external_log_config_file = NULL;
     }
 
+    rcl_ret_t thread_ret = rcl_thread_attrs_fini(&args->impl->thread_attrs);
+    if (thread_ret != RCL_RET_OK) {
+      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 +2149,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 = rcl_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,
+  rcl_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
@@ -51,6 +51,9 @@ struct rcl_arguments_impl_s
   /// Length of remap_rules.
   int num_remap_rules;
 
+  /// thread attribute.
+  rcl_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
@@ -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);
 }
 
+rcl_ret_t
+rcl_context_get_thread_attrs(
+  const rcl_context_t * context,
+  rcl_thread_attrs_t ** thread_attrs)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(context->impl, RCL_RET_INVALID_ARGUMENT);
+
+  if (0 < context->impl->thread_attrs.num_attributes) {
+    *thread_attrs = &context->impl->thread_attrs;
+    return RCL_RET_OK;
+  } else {
+    return RCL_RET_ERROR;
+  }
+}
+
 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 =
+      rcl_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
@@ -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.
+  rcl_thread_attrs_t thread_attrs;
 };
 
 RCL_LOCAL