diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 315ac1457..7be2de650 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -129,7 +129,7 @@ function(test_target_function) set(AMENT_GTEST_ARGS TIMEOUT 120) endif() rcl_add_custom_gtest(test_info_by_topic${target_suffix} - SRCS rcl/test_info_by_topic.cpp + SRCS rcl/test_info_by_topic.cpp rcl/wait_for_entity_helpers.cpp ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} LIBRARIES ${PROJECT_NAME} diff --git a/rcl/test/rcl/test_info_by_topic.cpp b/rcl/test/rcl/test_info_by_topic.cpp index 007693304..e7faadb9a 100644 --- a/rcl/test/rcl/test_info_by_topic.cpp +++ b/rcl/test/rcl/test_info_by_topic.cpp @@ -27,6 +27,7 @@ #include "rmw/topic_endpoint_info_array.h" #include "rmw/error_handling.h" +#include "wait_for_entity_helpers.hpp" #include "test_msgs/msg/strings.h" #include "rosidl_runtime_c/string_functions.h" @@ -357,6 +358,8 @@ TEST_F( &subscription_options); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; const std::string fqdn = std::string("/") + this->topic_name; + // Wait until GraphCache publishers are updated + ASSERT_TRUE(wait_for_graph_publication(&this->node, fqdn.c_str(), 1u, 10, 100)); // Get publishers info by topic rmw_topic_endpoint_info_array_t topic_endpoint_info_array_pub = rmw_get_zero_initialized_topic_endpoint_info_array(); @@ -371,6 +374,9 @@ TEST_F( EXPECT_STREQ(topic_endpoint_info_pub.topic_type, "test_msgs/msg/Strings"); assert_qos_equality(topic_endpoint_info_pub.qos_profile, default_qos_profile, true); + // Wait until GraphCache subcribers are updated + ASSERT_TRUE(wait_for_graph_subscription(&this->node, fqdn.c_str(), 1u, 10, 100)); + // Get subscribers info by topic rmw_topic_endpoint_info_array_t topic_endpoint_info_array_sub = rmw_get_zero_initialized_topic_endpoint_info_array(); ret = rcl_get_subscriptions_info_by_topic( diff --git a/rcl/test/rcl/wait_for_entity_helpers.cpp b/rcl/test/rcl/wait_for_entity_helpers.cpp index 625e1ea4a..bdf2f5d0d 100644 --- a/rcl/test/rcl/wait_for_entity_helpers.cpp +++ b/rcl/test/rcl/wait_for_entity_helpers.cpp @@ -236,3 +236,63 @@ wait_for_subscription_to_be_ready( } return false; } + +bool +wait_for_graph_publication( + const rcl_node_t * node, + const char * topic_name, + size_t count_to_wait, + size_t max_tries, + int64_t period_ms) +{ + if (count_to_wait == 0) { + return true; // Nothing to wait + } + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + size_t count; + rcl_ret_t ret = rcl_count_publishers(node, topic_name, &count); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Error in rcl_count_publishers: %s", rcl_get_error_string().str); + return false; + } + if (count == count_to_wait) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + return false; +} + +bool +wait_for_graph_subscription( + const rcl_node_t * node, + const char * topic_name, + size_t count_to_wait, + size_t max_tries, + int64_t period_ms) +{ + if (count_to_wait == 0) { + return true; // Nothing to wait + } + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + size_t count; + rcl_ret_t ret = rcl_count_subscribers(node, topic_name, &count); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Error in rcl_count_subscribers: %s", rcl_get_error_string().str); + return false; + } + if (count == count_to_wait) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + return false; +} diff --git a/rcl/test/rcl/wait_for_entity_helpers.hpp b/rcl/test/rcl/wait_for_entity_helpers.hpp index 35baa645b..9a31ff10c 100644 --- a/rcl/test/rcl/wait_for_entity_helpers.hpp +++ b/rcl/test/rcl/wait_for_entity_helpers.hpp @@ -63,4 +63,24 @@ wait_for_subscription_to_be_ready( size_t max_tries, int64_t period_ms); +/// Wait for specified number of publication in GraphCache +/// by trying at most `max_tries` times with a `period_ms` period. +bool +wait_for_graph_publication( + const rcl_node_t * node, + const char * topic_name, + size_t count_to_wait, + size_t max_tries, + int64_t period_ms); + +/// Wait for specified number of subcription in GraphCache +/// by trying at most `max_tries` times with a `period_ms` period. +bool +wait_for_graph_subscription( + const rcl_node_t * node, + const char * topic_name, + size_t count_to_wait, + size_t max_tries, + int64_t period_ms); + #endif // RCL__WAIT_FOR_ENTITY_HELPERS_HPP_