Add test for possible race in guard condition notification (2)

test_rmw_implementation/test/test_wait_set.cpp

@@ -13,6 +13,8 @@
 // limitations under the License.
 
 #include <gtest/gtest.h>
+#include <algorithm>
+#include <array>
 
 #include "osrf_testing_tools_cpp/scope_exit.hpp"
 
@@ -413,3 +415,122 @@ TEST_F(TestWaitSet, rmw_destroy_wait_set)
   ret = rmw_destroy_wait_set(wait_set);
   EXPECT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str;
 }
+
+/**
+ * This test creates a wait set with multiple guard conditions and triggers them in a separate thread.
+ * Expects that all guard conditions are notified exactly once and that rmw_wait returns RMW_RET_TIMEOUT after all
+ * guard conditions have been triggered.
+ */
+TEST_F(TestWaitSet, rmw_wait_guard_conditions)
+{
+  constexpr size_t number_of_guard_conditions = 100u;
+  typedef rmw_guard_condition_t * guard_condition_ptr_t;
+  std::array<guard_condition_ptr_t, number_of_guard_conditions> guard_condition_ptrs;
+
+  // Create the guard conditions
+  std::array<bool, number_of_guard_conditions> guard_conditions_triggered;
+  guard_conditions_triggered.fill(false);
+
+  for (size_t i = 0; i < number_of_guard_conditions; ++i) {
+    guard_condition_ptrs[i] = rmw_create_guard_condition(&context);
+    ASSERT_NE(nullptr, guard_condition_ptrs[i]) << rcutils_get_error_string().str;
+  }
+
+  // Create a wait set
+  rmw_wait_set_t * wait_set = rmw_create_wait_set(&context, number_of_guard_conditions);
+  ASSERT_NE(nullptr, wait_set) << rcutils_get_error_string().str;
+
+  for(size_t runs = 0; runs < 100; runs++) {
+    guard_conditions_triggered.fill(false);
+
+    // Prepare input arguments for rmw_wait
+    rmw_time_t timeout_argument = {2, 0};  // 2 seconds
+    rmw_guard_conditions_t guard_conditions;
+    INITIALIZE_ARRAY(guard_conditions, guard_condition, number_of_guard_conditions);
+
+    // Prepare the input array for rmw_wait
+    for (size_t j = 0; j < number_of_guard_conditions; ++j) {
+      guard_conditions.guard_conditions[j] = guard_condition_ptrs[j]->data;
+    }
+
+    // Thread that triggers all guard conditions in reverse order after 10ms
+    std::thread trigger_thread([&]() {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        for (size_t i = number_of_guard_conditions; i > 0; --i) {
+          rmw_ret_t ret = rmw_trigger_guard_condition(guard_condition_ptrs[i - 1]);
+          EXPECT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str;
+        }
+      });
+
+    // wait until we receive the first guard condition trigger
+    rmw_ret_t ret = rmw_wait(nullptr, &guard_conditions, nullptr, nullptr, nullptr, wait_set,
+      &timeout_argument);
+    EXPECT_EQ(ret, RMW_RET_OK) << "Failed to receive any guard condition trigger";
+
+    // Join the trigger thread
+    trigger_thread.join();
+
+    // mark triggered guard conditions as ready
+    for (size_t j = 0; j < number_of_guard_conditions; ++j) {
+      if (guard_conditions.guard_conditions[j] != nullptr) {
+        // Check that condition was not already triggered
+        ASSERT_NE(guard_conditions_triggered[j], true);
+        // Mark this condition as triggered
+        guard_conditions_triggered[j] = true;
+      }
+    }
+
+    // set timeout to almost zero there is not point in waiting longer from here on
+    timeout_argument.sec = 0;
+    timeout_argument.nsec = 1;
+
+    if(std::ranges::any_of(guard_conditions_triggered, [](bool triggered) {return !triggered;})) {
+      // Prepare the input array for rmw_wait
+      for (size_t j = 0; j < number_of_guard_conditions; ++j) {
+        guard_conditions.guard_conditions[j] = guard_condition_ptrs[j]->data;
+      }
+
+      // Wait for guard conditions to be triggered
+      rmw_ret_t ret = rmw_wait(nullptr, &guard_conditions, nullptr, nullptr, nullptr, wait_set,
+        &timeout_argument);
+      ASSERT_EQ(ret, RMW_RET_OK) << "Expected guard condition trigger but got timeout";
+
+      // This should collect the outstanding triggers now
+      for (size_t j = 0; j < number_of_guard_conditions; ++j) {
+        if (guard_conditions.guard_conditions[j] != nullptr) {
+          // Check that condition was not already triggered
+          ASSERT_NE(guard_conditions_triggered[j], true);
+          // Mark this condition as triggered
+          guard_conditions_triggered[j] = true;
+        }
+      }
+    }
+
+    // Check that all guard conditions were triggered
+    ASSERT_FALSE(std::ranges::any_of(guard_conditions_triggered, [](bool triggered) {
+        return !triggered;
+    })) << "Not all guard conditions were triggered";
+
+    // Calling rmw_wait shall now return RMW_RET_TIMEOUT since all guard conditions
+    // have been triggered
+    {
+      for (size_t j = 0; j < number_of_guard_conditions; ++j) {
+        guard_conditions.guard_conditions[j] = guard_condition_ptrs[j]->data;
+      }
+      rmw_ret_t ret = rmw_wait(nullptr, &guard_conditions, nullptr, nullptr, nullptr, wait_set,
+        &timeout_argument);
+      ASSERT_EQ(ret, RMW_RET_TIMEOUT) << rcutils_get_error_string().str;
+    }
+  }
+
+  // Clean up: destroy guard conditions and wait set
+  for (size_t i = 0; i < number_of_guard_conditions; ++i) {
+    rmw_ret_t ret = rmw_destroy_guard_condition(guard_condition_ptrs[i]);
+    ASSERT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str;
+  }
+
+  {
+    rmw_ret_t ret = rmw_destroy_wait_set(wait_set);
+    ASSERT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str;
+  }
+}