From 8bf120b321fb7fe8aac7160159f8ee26b4cf95c7 Mon Sep 17 00:00:00 2001
From: Tomoya Fujita <Tomoya.Fujita@sony.com>
Date: Fri, 17 Sep 2021 09:21:05 -0700
Subject: [PATCH] test should check specified number of entities. (#935)

* test should check specified number of entities.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

* rename function name to be very explicit about the point of the check.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

* remove check_graph_state().

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
---
 rcl/src/rcl/graph.c         |  2 +-
 rcl/test/rcl/test_graph.cpp | 49 ++++++++++++++++++++-----------------
 2 files changed, 28 insertions(+), 23 deletions(-)

diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c
index 6095cc3d5..5ba0a0a67 100644
--- a/rcl/src/rcl/graph.c
+++ b/rcl/src/rcl/graph.c
@@ -481,7 +481,7 @@ _rcl_wait_for_entities(
   rcl_ret_t ret = RCL_RET_OK;
   *success = false;
 
-  // We can avoid waiting if there are already the expected number of publishers
+  // We can avoid waiting if there are already the expected number of entities
   size_t count = 0u;
   ret = count_entities_func(node, topic_name, &count);
   if (ret != RCL_RET_OK) {
diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp
index dd21856de..3410325da 100644
--- a/rcl/test/rcl/test_graph.cpp
+++ b/rcl/test/rcl/test_graph.cpp
@@ -797,17 +797,17 @@ TEST_F(
 }
 
 void
-check_graph_state(
+check_entity_count(
   const rcl_node_t * node_ptr,
   std::string & topic_name,
   size_t expected_publisher_count,
   size_t expected_subscriber_count,
   bool expected_in_tnat,
-  const std::chrono::nanoseconds & timeout)
+  std::chrono::seconds timeout)
 {
   RCUTILS_LOG_DEBUG_NAMED(
     ROS_PACKAGE_NAME,
-    "Expecting %zu publishers, %zu subscribers, and that the topic is%s in the graph.",
+    "Expecting number of %zu publishers, %zu subscribers, and that the topic is%s in the graph.",
     expected_publisher_count,
     expected_subscriber_count,
     expected_in_tnat ? "" : " not"
@@ -816,25 +816,30 @@ check_graph_state(
   rcl_names_and_types_t tnat {};
   rcl_ret_t ret;
   rcl_allocator_t allocator = rcl_get_default_allocator();
-
-  // Wait for expected number of publishers
-  bool success = false;
-  ret = rcl_wait_for_publishers(
-    node_ptr, &allocator, topic_name.c_str(), expected_publisher_count, timeout.count(), &success);
-  ASSERT_EQ(ret, RCL_RET_OK);
-  EXPECT_TRUE(success);
-  // Wait for expected number of subscribers
-  success = false;
-  ret = rcl_wait_for_subscribers(
-    node_ptr, &allocator, topic_name.c_str(), expected_subscriber_count, timeout.count(), &success);
-  ASSERT_EQ(ret, RCL_RET_OK);
-  EXPECT_TRUE(success);
+  size_t pub_count, sub_count;
+
+  // Check number of entities until timeout expires.
+  auto start_time = std::chrono::system_clock::now();
+  do {
+    ret = rcl_count_publishers(node_ptr, topic_name.c_str(), &pub_count);
+    ASSERT_EQ(ret, RCL_RET_OK);
+    ret = rcl_count_subscribers(node_ptr, topic_name.c_str(), &sub_count);
+    ASSERT_EQ(ret, RCL_RET_OK);
+    if ((expected_publisher_count == pub_count) &&
+      (expected_subscriber_count == sub_count))
+    {
+      break;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  } while (std::chrono::system_clock::now() - start_time < timeout);
+  EXPECT_EQ(expected_publisher_count, pub_count);
+  EXPECT_EQ(expected_subscriber_count, sub_count);
 
   tnat = rcl_get_zero_initialized_names_and_types();
   ret = rcl_get_topic_names_and_types(node_ptr, &allocator, false, &tnat);
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   is_in_tnat = false;
-  for (size_t i = 0; RCL_RET_OK == ret && i < tnat.names.size; ++i) {
+  for (size_t i = 0; i < tnat.names.size; ++i) {
     if (topic_name == std::string(tnat.names.data[i])) {
       ASSERT_FALSE(is_in_tnat) << "duplicates in the tnat";  // Found it more than once!
       is_in_tnat = true;
@@ -1195,7 +1200,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Using topic name: %s", topic_name.c_str());
   rcl_ret_t ret;
   // First assert the "topic_name" is not in use.
-  check_graph_state(
+  check_entity_count(
     this->node_ptr,
     topic_name,
     0,    // expected publishers on topic
@@ -1210,7 +1215,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   rcl_reset_error();
   // Check the graph.
-  check_graph_state(
+  check_entity_count(
     this->node_ptr,
     topic_name,
     1,  // expected publishers on topic
@@ -1224,7 +1229,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   rcl_reset_error();
   // Check the graph again.
-  check_graph_state(
+  check_entity_count(
     this->node_ptr,
     topic_name,
     1,  // expected publishers on topic
@@ -1236,7 +1241,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   rcl_reset_error();
   // Check the graph again.
-  check_graph_state(
+  check_entity_count(
     this->node_ptr,
     topic_name,
     0,  // expected publishers on topic
@@ -1248,7 +1253,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   rcl_reset_error();
   // Check the graph again.
-  check_graph_state(
+  check_entity_count(
     this->node_ptr,
     topic_name,
     0,  // expected publishers on topic