Modified to receive multiple core affinity parameters according to th…

…e update of REP-2017 below. ros-infrastructure/rep#385 Signed-off-by: Shoji Morita <[email protected]>
esol-community · Oct 26, 2023 · 78ee281 · 78ee281
1 parent 043b11b
commit 78ee281
Show file tree

Hide file tree

Showing 7 changed files with 102 additions and 46 deletions.
diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp
@@ -210,7 +210,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_known_vs_unkno
   EXPECT_TRUE(
     are_known_ros_args(
       {"--ros-args", "--thread-attrs-value",
-        "[{priority: 10, scheduling_policy: FIFO, name: thread-1, core_affinity: 1}]"}));
+        "[{priority: 10, scheduling_policy: FIFO, name: thread-1, core_affinity: [1,2,3]}]"}));
 }
 
 bool
@@ -238,7 +238,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_inval
     "--params-file", parameters_filepath.c_str(), "--log-level", "INFO",
     "--log-config-file", "file.config",
     "--thread-attrs-value",
-    "[{priority: 10, scheduling_policy: IDLE, name: thread-1, core_affinity: 1}]"
+    "[{priority: 10, scheduling_policy: IDLE, name: thread-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
@@ -171,11 +171,15 @@ TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), nominal) {
     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, 1, 10, "arg");
+      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));

diff --git a/rcl/test/resources/test_arguments/test_thread_attrs.yaml b/rcl/test/resources/test_arguments/test_thread_attrs.yaml
@@ -1,8 +1,8 @@
 - name: thread-1
   priority: 10
   scheduling_policy: FIFO
-  core_affinity: 1
+  core_affinity: [1]
 - name: thread-2
   priority: 20
   scheduling_policy: RR
-  core_affinity: 2
+  core_affinity: [2]
diff --git a/rcl_yaml_param_parser/src/parse_thread_attr.c b/rcl_yaml_param_parser/src/parse_thread_attr.c
@@ -119,10 +119,12 @@ rcutils_ret_t parse_thread_attr(
   yaml_event_t event;
   thread_attr_key_bits_t key_bits = THREAD_ATTR_KEY_BITS_NONE;
   rcutils_thread_scheduling_policy_t sched_policy;
-  int core_affinity;
+  rcutils_thread_core_affinity_t core_affinity =
+    rcutils_get_zero_initialized_thread_core_affinity();
   int priority;
   char const * name = NULL;
   rcutils_allocator_t allocator = attrs->allocator;
+  void * ret_val = NULL;
 
   while (1) {
     PARSE_WITH_CHECK_ERROR();
@@ -148,26 +150,51 @@ rcutils_ret_t parse_thread_attr(
       goto error;
     }
 
-    PARSE_WITH_CHECK_EVENT(YAML_SCALAR_EVENT);
+    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 * tag = event.data.scalar.tag;
     data_types_t val_type;
-    void * ret_val = NULL;
 
     switch (key_type) {
       case THREAD_ATTR_KEY_CORE_AFFINITY:
-        ret_val = get_value(value, style, 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);
+        if (event.type != YAML_SEQUENCE_START_EVENT) {
           ret = RCUTILS_RET_ERROR;
           goto error;
         }
-        core_affinity = ((int)*(int64_t *)(ret_val));
+        ret = rcutils_thread_core_affinity_init(&core_affinity, 0, 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;
+          tag = event.data.scalar.tag;
+          ret_val = get_value(value, style, 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, tag, &val_type, allocator);
         if (DATA_TYPE_INT64 != val_type) {
           RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
@@ -178,9 +205,15 @@ rcutils_ret_t parse_thread_attr(
         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_NAME:
+        if (event.type != YAML_SCALAR_EVENT) {
+          goto error;
+        }
         if (*value == '\0') {
           RCUTILS_SET_ERROR_MSG("Empty thread name");
           ret = RCUTILS_RET_ERROR;
@@ -196,6 +229,7 @@ rcutils_ret_t parse_thread_attr(
 
     if (NULL != ret_val) {
       allocator.deallocate(ret_val, allocator.state);
+      ret_val = NULL;
     }
 
     key_bits |= (thread_attr_key_bits_t)key_type;
@@ -207,7 +241,7 @@ rcutils_ret_t parse_thread_attr(
     goto error;
   }
 
-  ret = rcutils_thread_attrs_add_attr(attrs, sched_policy, core_affinity, priority, name);
+  ret = rcutils_thread_attrs_add_attr(attrs, sched_policy, &core_affinity, priority, name);
   if (RCUTILS_RET_OK != ret) {
     goto error;
   }
@@ -220,6 +254,14 @@ rcutils_ret_t parse_thread_attr(
   if (NULL != name) {
     allocator.deallocate((char *)name, 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;
 }
 

diff --git a/rcl_yaml_param_parser/test/test_parse_thread_attr.cpp b/rcl_yaml_param_parser/test/test_parse_thread_attr.cpp
@@ -35,7 +35,7 @@ struct TestParseThreadAttrs : testing::Test
     attrs = rcutils_get_zero_initialized_thread_attrs();
     allocator = rcutils_get_default_allocator();
     ret = rcutils_thread_attrs_init(&attrs, allocator);
-    ASSERT_EQ(ret, RCUTILS_RET_OK);
+    ASSERT_EQ(RCUTILS_RET_OK, ret);
 
     int parser_ret = yaml_parser_initialize(&parser);
     ASSERT_NE(0, parser_ret);
@@ -78,15 +78,15 @@ TEST_F(TestParseThreadAttr, success) {
   yaml_event_t event;
 
   prepare_yaml_parser(
-    "{ priority: 10, name: thread-1, core_affinity: 1, scheduling_policy: FIFO }");
+    "{ priority: 10, name: thread-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("thread-1", attrs.attributes[0].name);
-  EXPECT_EQ(1, attrs.attributes[0].core_affinity);
+  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;
@@ -102,7 +102,7 @@ TEST_F(TestParseThreadAttr, unknown_key) {
   rcutils_ret_t ret;
 
   prepare_yaml_parser(
-    "{ priority: 10, name: thread-1, core_affinity: 1, unknown_key: FIFO }");
+    "{ priority: 10, name: thread-1, core_affinity: [1], unknown_key: FIFO }");
 
   ret = parse_thread_attr(&parser, &attrs);
   EXPECT_EQ(RCUTILS_RET_ERROR, ret);
@@ -112,7 +112,8 @@ TEST_F(TestParseThreadAttr, all_valid_keys_with_unknown_key) {
   rcutils_ret_t ret;
 
   prepare_yaml_parser(
-    "{ priority: 10, name: thread-1, core_affinity: 1, scheduling_policy: FIFO, unknown_key: RR }");
+    "{ priority: 10, name: thread-1, core_affinity: [1], "
+    "scheduling_policy: FIFO, unknown_key: RR }");
 
   ret = parse_thread_attr(&parser, &attrs);
   EXPECT_EQ(RCUTILS_RET_ERROR, ret);
@@ -127,15 +128,6 @@ TEST_F(TestParseThreadAttr, missing_key_value) {
   EXPECT_EQ(RCUTILS_RET_ERROR, ret);
 }
 
-TEST_F(TestParseThreadAttr, not_acceptable_mapping) {
-  rcutils_ret_t ret;
-  prepare_yaml_parser(
-    "{ priority: 10, name: thread-1, core_affinity: [1,2,3], scheduling_policy: FIFO }");
-
-  ret = parse_thread_attr(&parser, &attrs);
-  EXPECT_EQ(RCUTILS_RET_ERROR, ret);
-}
-
 TEST_F(TestParseThreadAttrs, success) {
   rcutils_ret_t ret;
 
@@ -144,7 +136,7 @@ TEST_F(TestParseThreadAttrs, success) {
   for (std::size_t i = 0; i < 100; ++i) {
     ss << "{ priority: " << i * 10;
     ss << ", name: thread-" << i;
-    ss << ", core_affinity: " << i;
+    ss << ", core_affinity: [" << i << "]";
     ss << ", scheduling_policy: FIFO },";
   }
   ss << "]";
@@ -162,7 +154,21 @@ TEST_F(TestParseThreadAttrs, success) {
     ss << "thread-" << i;
     buf = ss.str();
     EXPECT_STREQ(buf.c_str(), attrs.attributes[i].name);
-    EXPECT_EQ(i, attrs.attributes[i].core_affinity);
+    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, name: thread-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
@@ -86,12 +86,14 @@ TEST_F(TestParserThreadAttr, success_file) {
   ASSERT_EQ(RCUTILS_RET_OK, ret);
   EXPECT_EQ(10, attrs.num_attributes);
 
-  for (int i = 0; i < 10; ++i) {
+  for (size_t i = 0; i < 10; ++i) {
     EXPECT_EQ(attrs.attributes[i].priority, i * 10);
     char buf[32];
-    snprintf(buf, sizeof(buf), "thread-%d", i);
+    snprintf(buf, sizeof(buf), "thread-%lu", i);
     EXPECT_STREQ(buf, attrs.attributes[i].name);
-    EXPECT_EQ(i, attrs.attributes[i].core_affinity);
+    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);
   }
 }
@@ -109,12 +111,14 @@ TEST_F(TestParserThreadAttr, success_value) {
   ASSERT_EQ(RCUTILS_RET_OK, ret);
   EXPECT_EQ(10, attrs.num_attributes);
 
-  for (int i = 0; i < 10; ++i) {
+  for (size_t i = 0; i < 10; ++i) {
     EXPECT_EQ(attrs.attributes[i].priority, i * 10);
     char buf[32];
-    snprintf(buf, sizeof(buf), "thread-%d", i);
+    snprintf(buf, sizeof(buf), "thread-%lu", i);
     EXPECT_STREQ(buf, attrs.attributes[i].name);
-    EXPECT_EQ(i, attrs.attributes[i].core_affinity);
+    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);
   }
 }

diff --git a/rcl_yaml_param_parser/test/thread_attr_success.yaml b/rcl_yaml_param_parser/test/thread_attr_success.yaml
@@ -1,40 +1,40 @@
 - priority: 0
   name: thread-0
-  core_affinity: 0
+  core_affinity: [0,10,0]
   scheduling_policy: BADSCHEDPOLICY1
 - priority: 10
   name: thread-1
-  core_affinity: 1
+  core_affinity: [1,11,1]
   scheduling_policy: FIFO
 - priority: 20
   name: thread-2
-  core_affinity: 2
+  core_affinity: [2,12,4]
   scheduling_policy: RR
 - priority: 30
   name: thread-3
-  core_affinity: 3
+  core_affinity: [3,13,9]
   scheduling_policy: SPORADIC
 - priority: 40
   name: thread-4
-  core_affinity: 4
+  core_affinity: [4,14,16]
   scheduling_policy: OTHER
 - priority: 50
   name: thread-5
-  core_affinity: 5
+  core_affinity: [5,15,25]
   scheduling_policy: IDLE
 - priority: 60
   name: thread-6
-  core_affinity: 6
+  core_affinity: [6,16,36]
   scheduling_policy: BATCH
 - priority: 70
   name: thread-7
-  core_affinity: 7
+  core_affinity: [7,17,49]
   scheduling_policy: DEADLINE
 - priority: 80
   name: thread-8
-  core_affinity: 8
+  core_affinity: [8,18,64]
   scheduling_policy: BADSCHEDPOLICY2
 - priority: 90
   name: thread-9
-  core_affinity: 9
+  core_affinity: [9,19,81]
   scheduling_policy: FIFO