diff --git a/test_rmw_implementation/test/test_wait_set.cpp b/test_rmw_implementation/test/test_wait_set.cpp index 6c4bef4c..fd962c90 100644 --- a/test_rmw_implementation/test/test_wait_set.cpp +++ b/test_rmw_implementation/test/test_wait_set.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #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_ptrs; + + // Create the guard conditions + std::array 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; + } +}