Re-add "Improve trigger test for graph guard condition (ros2#811)"

Updated codes based on commit a6cacef.

Signed-off-by: Barry Xu <barry.xu@sony.com>

rcl/test/rcl/test_graph.cpp

@@ -1223,75 +1223,176 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio
     9);  // number of retries
 }
 
-/* Test the graph guard condition notices topic changes.
+/* Test the graph guard condition notices below changes.
+ * publisher create/destroy, subscription create/destroy
+ * service create/destroy, client create/destroy
+ * Other node added/removed
  *
  * Note: this test could be impacted by other communications on the same ROS Domain.
  */
-TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_condition_topics) {
+TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_condition_trigger_check) {
+#define CHECK_GUARD_CONDITION_CHANGE(EXPECTED_RESULT, TIMEOUT)   do { \
+    ret = rcl_wait_set_clear(&wait_set); \
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \
+    ret = rcl_wait_set_add_guard_condition(&wait_set, graph_guard_condition, NULL); \
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \
+    ret = rcl_wait(&wait_set, TIMEOUT.count()); \
+    ASSERT_EQ(EXPECTED_RESULT, ret) << rcl_get_error_string().str; \
+} while (0)
+
   rcl_ret_t ret;
-  // Create a thread to sleep for a time, then create a publisher, sleep more, then a subscriber,
-  // sleep more, destroy the subscriber, sleep more, and then destroy the publisher.
-  std::promise<bool> topic_changes_promise;
-  std::thread topic_thread(
-    [this, &topic_changes_promise]() {
-      // sleep
-      std::this_thread::sleep_for(std::chrono::milliseconds(200));
-      // create the publisher
-      rcl_publisher_t pub = rcl_get_zero_initialized_publisher();
-      rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options();
-      rcl_ret_t ret = rcl_publisher_init(
-        &pub, this->node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes),
-        "/chatter_test_graph_guard_condition_topics", &pub_ops);
-      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-      // sleep
-      std::this_thread::sleep_for(std::chrono::milliseconds(200));
-      // create the subscription
-      rcl_subscription_t sub = rcl_get_zero_initialized_subscription();
-      rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options();
-      ret = rcl_subscription_init(
-        &sub, this->node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes),
-        "/chatter_test_graph_guard_condition_topics", &sub_ops);
-      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-      // sleep
-      std::this_thread::sleep_for(std::chrono::milliseconds(200));
-      // destroy the subscription
-      ret = rcl_subscription_fini(&sub, this->node_ptr);
-      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-      // sleep
-      std::this_thread::sleep_for(std::chrono::milliseconds(200));
-      // destroy the publication
-      ret = rcl_publisher_fini(&pub, this->node_ptr);
-      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-      // notify that the thread is done
-      topic_changes_promise.set_value(true);
-    });
-  // Wait for the graph state to change, expecting it to do so at least 4 times,
-  // once for each change in the topics thread.
+  std::chrono::nanoseconds timeout_1s = std::chrono::seconds(1);
+  std::chrono::nanoseconds timeout_3s = std::chrono::seconds(3);
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  ret = rcl_wait_set_init(
+    &wait_set, 0, 1, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str;
+  });
+
   const rcl_guard_condition_t * graph_guard_condition =
-    rcl_node_get_graph_guard_condition(this->node_ptr);
-  ASSERT_NE(nullptr, graph_guard_condition) << rcl_get_error_string().str;
-  std::shared_future<bool> future = topic_changes_promise.get_future();
-  size_t graph_changes_count = 0;
-  // while the topic thread is not done, wait and count the graph changes
-  while (future.wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
-    ret = rcl_wait_set_clear(this->wait_set_ptr);
+    rcl_node_get_graph_guard_condition(node_ptr);
+
+  // Wait for no graph change condition
+  int idx = 0;
+  for (; idx < 100; idx++) {
+    ret = rcl_wait_set_clear(&wait_set);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition, NULL);
+    ret = rcl_wait_set_add_guard_condition(&wait_set, graph_guard_condition, NULL);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(400);
-    RCUTILS_LOG_INFO_NAMED(
-      ROS_PACKAGE_NAME,
-      "waiting up to '%s' nanoseconds for graph changes",
-      std::to_string(time_to_sleep.count()).c_str());
-    ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count());
-    if (ret == RCL_RET_TIMEOUT) {
-      continue;
+    ret = rcl_wait(&wait_set, timeout_3s.count());
+    if (RCL_RET_TIMEOUT == ret) {
+      break;
+    } else {
+      RCUTILS_LOG_INFO_NAMED(
+        ROS_PACKAGE_NAME,
+        "waiting for no graph change condition ...");
     }
-    graph_changes_count++;
   }
-  topic_thread.join();
-  // expect at least 4 changes
-  ASSERT_GE(graph_changes_count, 4ul);
+  ASSERT_NE(idx, 100);
+
+  // Graph change since creating the publisher
+  rcl_publisher_t pub = rcl_get_zero_initialized_publisher();
+  rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options();
+  ret = rcl_publisher_init(
+    &pub, node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes),
+    "/chatter_test_graph_guard_condition_topics", &pub_ops);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Graph change since destroying the publisher
+  ret = rcl_publisher_fini(&pub, node_ptr);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Graph change since creating the subscription
+  rcl_subscription_t sub = rcl_get_zero_initialized_subscription();
+  rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options();
+  ret = rcl_subscription_init(
+    &sub, node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes),
+    "/chatter_test_graph_guard_condition_topics", &sub_ops);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Graph change since destroying the subscription
+  ret = rcl_subscription_fini(&sub, node_ptr);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Graph change since creating service
+  rcl_service_t service = rcl_get_zero_initialized_service();
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  ret = rcl_service_init(
+    &service,
+    node_ptr,
+    ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes),
+    "test_graph_guard_condition_service",
+    &service_options);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Graph change since destroy service
+  ret = rcl_service_fini(&service, node_ptr);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Graph change since creating client
+  rcl_client_t client = rcl_get_zero_initialized_client();
+  rcl_client_options_t client_options = rcl_client_get_default_options();
+  ret = rcl_client_init(
+    &client,
+    node_ptr,
+    ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes),
+    "test_graph_guard_condition_service",
+    &client_options);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Graph change since destroying client
+  ret = rcl_client_fini(&client, node_ptr);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Graph change since adding new node
+  rcl_node_t node_new = rcl_get_zero_initialized_node();
+  rcl_node_options_t node_options = rcl_node_get_default_options();
+  ret = rcl_node_init(&node_new, "test_graph2", "", context_ptr, &node_options);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_3s);
+  }
+
+  // Graph change since destroying new node
+  ret = rcl_node_fini(&node_new);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s);
+  }
+
+  // Should not get graph change if no change
+  {
+    SCOPED_TRACE("Check guard condition change failed !");
+    CHECK_GUARD_CONDITION_CHANGE(RCL_RET_TIMEOUT, timeout_1s);
+  }
 }
 
 /* Test the rcl_service_server_is_available function.