diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt
index 261f8bd829..1a31a383e6 100644
--- a/rcl/test/CMakeLists.txt
+++ b/rcl/test/CMakeLists.txt
@@ -191,7 +191,7 @@ function(test_target_function)
   )
 
   rcl_add_custom_gtest(test_service${target_suffix}
-    SRCS rcl/test_service.cpp
+    SRCS rcl/test_service.cpp rcl/wait_for_entity_helpers.cpp
     ENV ${rmw_implementation_env_var}
     APPEND_LIBRARY_DIRS ${extra_lib_dirs}
     LIBRARIES ${PROJECT_NAME}
@@ -199,7 +199,7 @@ function(test_target_function)
   )
 
   rcl_add_custom_gtest(test_subscription${target_suffix}
-    SRCS rcl/test_subscription.cpp
+    SRCS rcl/test_subscription.cpp rcl/wait_for_entity_helpers.cpp
     ENV ${rmw_implementation_env_var}
     APPEND_LIBRARY_DIRS ${extra_lib_dirs}
     LIBRARIES ${PROJECT_NAME}
@@ -259,13 +259,13 @@ function(test_target_function)
   # Launch tests
 
   rcl_add_custom_executable(service_fixture${target_suffix}
-    SRCS rcl/service_fixture.cpp
+    SRCS rcl/service_fixture.cpp rcl/wait_for_entity_helpers.cpp
     LIBRARIES ${PROJECT_NAME}
     AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs"
   )
 
   rcl_add_custom_executable(client_fixture${target_suffix}
-    SRCS rcl/client_fixture.cpp
+    SRCS rcl/client_fixture.cpp rcl/wait_for_entity_helpers.cpp
     LIBRARIES ${PROJECT_NAME}
     AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs"
   )
diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp
index c9cdbd2aa1..c6d035a9c5 100644
--- a/rcl/test/rcl/client_fixture.cpp
+++ b/rcl/test/rcl/client_fixture.cpp
@@ -12,11 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <chrono>
-#include <stdexcept>
-#include <string>
-#include <thread>
-
 #include "rcutils/logging_macros.h"
 
 #include "rcl/client.h"
@@ -26,87 +21,7 @@
 
 #include "osrf_testing_tools_cpp/scope_exit.hpp"
 #include "rcl/error_handling.h"
-#include "rcl/graph.h"
-
-bool
-wait_for_server_to_be_available(
-  rcl_node_t * node,
-  rcl_client_t * client,
-  size_t max_tries,
-  int64_t period_ms)
-{
-  size_t iteration = 0;
-  do {
-    ++iteration;
-    bool is_ready;
-    rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready);
-    if (ret != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        ROS_PACKAGE_NAME,
-        "Error in rcl_service_server_is_available: %s",
-        rcl_get_error_string().str);
-      return false;
-    }
-    if (is_ready) {
-      return true;
-    }
-    std::this_thread::sleep_for(std::chrono::milliseconds(period_ms));
-  } while (iteration < max_tries);
-  return false;
-}
-
-bool
-wait_for_client_to_be_ready(
-  rcl_client_t * client,
-  rcl_context_t * context,
-  size_t max_tries,
-  int64_t period_ms)
-{
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_ret_t ret =
-    rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, 0, context, rcl_get_default_allocator());
-  if (ret != RCL_RET_OK) {
-    RCUTILS_LOG_ERROR_NAMED(
-      ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str);
-    return false;
-  }
-  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
-  {
-    if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str);
-      throw std::runtime_error("error while waiting for client");
-    }
-  });
-  size_t iteration = 0;
-  do {
-    ++iteration;
-    if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str);
-      return false;
-    }
-    if (rcl_wait_set_add_client(&wait_set, client, NULL) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        ROS_PACKAGE_NAME, "Error in wait_set_add_client: %s", rcl_get_error_string().str);
-      return false;
-    }
-    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
-    if (ret == RCL_RET_TIMEOUT) {
-      continue;
-    }
-    if (ret != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str);
-      return false;
-    }
-    for (size_t i = 0; i < wait_set.size_of_clients; ++i) {
-      if (wait_set.clients[i] && wait_set.clients[i] == client) {
-        return true;
-      }
-    }
-  } while (iteration < max_tries);
-  return false;
-}
+#include "wait_for_entity_helpers.hpp"
 
 int main(int argc, char ** argv)
 {
diff --git a/rcl/test/rcl/service_fixture.cpp b/rcl/test/rcl/service_fixture.cpp
index 3bed7a14a1..0dc7fdcd8d 100644
--- a/rcl/test/rcl/service_fixture.cpp
+++ b/rcl/test/rcl/service_fixture.cpp
@@ -13,8 +13,6 @@
 // limitations under the License.
 
 #include <chrono>
-#include <stdexcept>
-#include <string>
 #include <thread>
 
 #include "rcutils/logging_macros.h"
@@ -27,59 +25,8 @@
 
 #include "osrf_testing_tools_cpp/scope_exit.hpp"
 #include "rcl/error_handling.h"
+#include "wait_for_entity_helpers.hpp"
 
-bool
-wait_for_service_to_be_ready(
-  rcl_service_t * service,
-  rcl_context_t * context,
-  size_t max_tries,
-  int64_t period_ms)
-{
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_ret_t ret =
-    rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, context, rcl_get_default_allocator());
-  if (ret != RCL_RET_OK) {
-    RCUTILS_LOG_ERROR_NAMED(
-      ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str);
-    return false;
-  }
-  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
-  {
-    if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str);
-      throw std::runtime_error("error waiting for service to be ready");
-    }
-  });
-  size_t iteration = 0;
-  do {
-    ++iteration;
-    if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str);
-      return false;
-    }
-    if (rcl_wait_set_add_service(&wait_set, service, NULL) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        ROS_PACKAGE_NAME, "Error in wait_set_add_service: %s", rcl_get_error_string().str);
-      return false;
-    }
-    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
-    if (ret == RCL_RET_TIMEOUT) {
-      continue;
-    }
-    if (ret != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str);
-      return false;
-    }
-    for (size_t i = 0; i < wait_set.size_of_services; ++i) {
-      if (wait_set.services[i] && wait_set.services[i] == service) {
-        return true;
-      }
-    }
-  } while (iteration < max_tries);
-  return false;
-}
 
 int main(int argc, char ** argv)
 {
diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp
index 7a2db37c6f..ead37e909e 100644
--- a/rcl/test/rcl/test_service.cpp
+++ b/rcl/test/rcl/test_service.cpp
@@ -14,18 +14,14 @@
 
 #include <gtest/gtest.h>
 
-#include <chrono>
-#include <string>
-#include <thread>
-
 #include "rcl/service.h"
-
 #include "rcl/rcl.h"
 
 #include "test_msgs/srv/basic_types.h"
 
 #include "osrf_testing_tools_cpp/scope_exit.hpp"
 #include "rcl/error_handling.h"
+#include "wait_for_entity_helpers.hpp"
 
 #ifdef RMW_IMPLEMENTATION
 # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
@@ -76,45 +72,6 @@ class CLASSNAME (TestServiceFixture, RMW_IMPLEMENTATION) : public ::testing::Tes
   }
 };
 
-void
-wait_for_service_to_be_ready(
-  rcl_service_t * service,
-  rcl_context_t * context,
-  size_t max_tries,
-  int64_t period_ms,
-  bool & success)
-{
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_ret_t ret =
-    rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, context, rcl_get_default_allocator());
-  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
-  {
-    rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
-    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-  });
-  size_t iteration = 0;
-  do {
-    ++iteration;
-    ret = rcl_wait_set_clear(&wait_set);
-    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    ret = rcl_wait_set_add_service(&wait_set, service, NULL);
-    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
-    if (ret == RCL_RET_TIMEOUT) {
-      continue;
-    }
-    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    for (size_t i = 0; i < wait_set.size_of_services; ++i) {
-      if (wait_set.services[i] && wait_set.services[i] == service) {
-        success = true;
-        return;
-      }
-    }
-  } while (iteration < max_tries);
-  success = false;
-}
-
 /* Basic nominal test of a service.
  */
 TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) {
@@ -165,10 +122,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal)
     EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   });
 
-  // TODO(wjwwood): add logic to wait for the connection to be established
-  //                use count_services busy wait mechanism
-  //                until then we will sleep for a short period of time
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000));
 
   // Initialize a request.
   test_msgs__srv__BasicTypes_Request client_request;
@@ -190,9 +144,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal)
   test_msgs__srv__BasicTypes_Request__fini(&client_request);
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
-  bool success;
-  wait_for_service_to_be_ready(&service, context_ptr, 10, 100, success);
-  ASSERT_TRUE(success);
+  ASSERT_TRUE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100));
 
   // This scope simulates the service responding in a different context so that we can
   // test take_request/send_response in a single-threaded, deterministic execution.
@@ -234,7 +186,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal)
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
     test_msgs__srv__BasicTypes_Request__fini(&service_request);
   }
-  wait_for_service_to_be_ready(&service, context_ptr, 10, 100, success);
+  ASSERT_FALSE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100));
 
   // Initialize the response owned by the client and take the response.
   test_msgs__srv__BasicTypes_Response client_response;
diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp
index 0cf6e42c0a..910775a1ca 100644
--- a/rcl/test/rcl/test_subscription.cpp
+++ b/rcl/test/rcl/test_subscription.cpp
@@ -19,14 +19,15 @@
 #include <thread>
 
 #include "rcl/subscription.h"
-
 #include "rcl/rcl.h"
+
 #include "test_msgs/msg/basic_types.h"
 #include "test_msgs/msg/strings.h"
 #include "rosidl_runtime_c/string_functions.h"
 
 #include "osrf_testing_tools_cpp/scope_exit.hpp"
 #include "rcl/error_handling.h"
+#include "wait_for_entity_helpers.hpp"
 
 #ifdef RMW_IMPLEMENTATION
 # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
@@ -77,45 +78,6 @@ class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing
   }
 };
 
-void
-wait_for_subscription_to_be_ready(
-  rcl_subscription_t * subscription,
-  rcl_context_t * context,
-  size_t max_tries,
-  int64_t period_ms,
-  bool & success)
-{
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_ret_t ret =
-    rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator());
-  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
-  {
-    rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
-    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-  });
-  size_t iteration = 0;
-  do {
-    ++iteration;
-    ret = rcl_wait_set_clear(&wait_set);
-    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    ret = rcl_wait_set_add_subscription(&wait_set, subscription, NULL);
-    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
-    if (ret == RCL_RET_TIMEOUT) {
-      continue;
-    }
-    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
-    for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) {
-      if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) {
-        success = true;
-        return;
-      }
-    }
-  } while (iteration < max_tries);
-  success = false;
-}
-
 /* Test subscription init, fini and is_valid functions
  */
 TEST_F(
@@ -175,10 +137,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
   });
   rcl_reset_error();
 
-  // TODO(wjwwood): add logic to wait for the connection to be established
-  //                probably using the count_subscriptions busy wait mechanism
-  //                until then we will sleep for a short period of time
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
 #ifdef RMW_TIMESTAMPS_SUPPORTED
   rcl_time_point_value_t pre_publish_time;
   EXPECT_EQ(
@@ -193,9 +152,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     test_msgs__msg__BasicTypes__fini(&msg);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
-  bool success;
-  wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
-  ASSERT_TRUE(success);
+  ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100));
   {
     test_msgs__msg__BasicTypes msg;
     test_msgs__msg__BasicTypes__init(&msg);
@@ -250,10 +207,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr);
     EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   });
-  // TODO(wjwwood): add logic to wait for the connection to be established
-  //                probably using the count_subscriptions busy wait mechanism
-  //                until then we will sleep for a short period of time
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
   const char * test_string = "testing";
   {
     test_msgs__msg__Strings msg;
@@ -263,9 +217,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     test_msgs__msg__Strings__fini(&msg);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
-  bool success;
-  wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
-  ASSERT_TRUE(success);
+  ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100));
   {
     test_msgs__msg__Strings msg;
     test_msgs__msg__Strings__init(&msg);
@@ -307,10 +259,7 @@ TEST_F(
     rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr);
     EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   });
-  // TODO(wjwwood): add logic to wait for the connection to be established
-  //                probably using the count_subscriptions busy wait mechanism
-  //                until then we will sleep for a short period of time
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
   const char * test_string = "testing";
   {
     test_msgs__msg__Strings msg;
@@ -322,9 +271,7 @@ TEST_F(
     test_msgs__msg__Strings__fini(&msg);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
-  bool success;
-  wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
-  ASSERT_TRUE(success);
+  ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100));
   auto allocator = rcutils_get_default_allocator();
   {
     size_t size = 1;
@@ -473,14 +420,12 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
   });
   rcl_reset_error();
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
   {
     ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
-  bool success;
-  wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
-  ASSERT_TRUE(success);
+  ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100));
   {
     rcl_serialized_message_t serialized_msg_rcv = rmw_get_zero_initialized_serialized_message();
     initial_capacity_ser = 0u;
@@ -524,10 +469,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr);
     EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   });
-  // TODO(wjwwood): add logic to wait for the connection to be established
-  //                probably using the count_subscriptions busy wait mechanism
-  //                until then we will sleep for a short period of time
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
   const char * test_string = "testing";
   {
     test_msgs__msg__Strings msg;
@@ -537,9 +479,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     test_msgs__msg__Strings__fini(&msg);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
-  bool success;
-  wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
-  ASSERT_TRUE(success);
+  ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100));
   {
     test_msgs__msg__Strings msg;
     test_msgs__msg__Strings * msg_loaned;
diff --git a/rcl/test/rcl/wait_for_entity_helpers.cpp b/rcl/test/rcl/wait_for_entity_helpers.cpp
new file mode 100644
index 0000000000..625e1ea4a2
--- /dev/null
+++ b/rcl/test/rcl/wait_for_entity_helpers.cpp
@@ -0,0 +1,238 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "wait_for_entity_helpers.hpp"
+
+#include <chrono>
+#include <stdexcept>
+#include <thread>
+
+#include "rcutils/logging_macros.h"
+
+#include "osrf_testing_tools_cpp/scope_exit.hpp"
+#include "rcl/error_handling.h"
+#include "rcl/graph.h"
+
+bool
+wait_for_server_to_be_available(
+  rcl_node_t * node,
+  rcl_client_t * client,
+  size_t max_tries,
+  int64_t period_ms)
+{
+  size_t iteration = 0;
+  while (iteration < max_tries) {
+    ++iteration;
+    bool is_ready;
+    rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready);
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME,
+        "Error in rcl_service_server_is_available: %s",
+        rcl_get_error_string().str);
+      return false;
+    }
+    if (is_ready) {
+      return true;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(period_ms));
+  }
+  return false;
+}
+
+bool
+wait_for_client_to_be_ready(
+  rcl_client_t * client,
+  rcl_context_t * context,
+  size_t max_tries,
+  int64_t period_ms)
+{
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_ret_t ret =
+    rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, 0, context, rcl_get_default_allocator());
+  if (ret != RCL_RET_OK) {
+    RCUTILS_LOG_ERROR_NAMED(
+      ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str);
+    return false;
+  }
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str);
+      throw std::runtime_error("error while waiting for client");
+    }
+  });
+  size_t iteration = 0;
+  while (iteration < max_tries) {
+    ++iteration;
+    if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str);
+      return false;
+    }
+    if (rcl_wait_set_add_client(&wait_set, client, NULL) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in wait_set_add_client: %s", rcl_get_error_string().str);
+      return false;
+    }
+    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
+    if (ret == RCL_RET_TIMEOUT) {
+      continue;
+    }
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str);
+      return false;
+    }
+    for (size_t i = 0; i < wait_set.size_of_clients; ++i) {
+      if (wait_set.clients[i] && wait_set.clients[i] == client) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+bool
+wait_for_service_to_be_ready(
+  rcl_service_t * service,
+  rcl_context_t * context,
+  size_t max_tries,
+  int64_t period_ms)
+{
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_ret_t ret =
+    rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, context, rcl_get_default_allocator());
+  if (ret != RCL_RET_OK) {
+    RCUTILS_LOG_ERROR_NAMED(
+      ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str);
+    return false;
+  }
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str);
+      throw std::runtime_error("error waiting for service to be ready");
+    }
+  });
+  size_t iteration = 0;
+  while (iteration < max_tries) {
+    ++iteration;
+    if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str);
+      return false;
+    }
+    if (rcl_wait_set_add_service(&wait_set, service, NULL) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in wait_set_add_service: %s", rcl_get_error_string().str);
+      return false;
+    }
+    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
+    if (ret == RCL_RET_TIMEOUT) {
+      continue;
+    }
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str);
+      return false;
+    }
+    for (size_t i = 0; i < wait_set.size_of_services; ++i) {
+      if (wait_set.services[i] && wait_set.services[i] == service) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+bool
+wait_for_established_subscription(
+  const rcl_publisher_t * publisher,
+  size_t max_tries,
+  int64_t period_ms)
+{
+  size_t iteration = 0;
+  rcl_ret_t ret = RCL_RET_OK;
+  size_t subscription_count = 0;
+  while (iteration < max_tries) {
+    ++iteration;
+    ret = rcl_publisher_get_subscription_count(publisher, &subscription_count);
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME,
+        "Error in rcl_publisher_get_subscription_count: %s",
+        rcl_get_error_string().str);
+      return false;
+    }
+    if (subscription_count > 0) {
+      return true;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(period_ms));
+  }
+  return false;
+}
+
+bool
+wait_for_subscription_to_be_ready(
+  rcl_subscription_t * subscription,
+  rcl_context_t * context,
+  size_t max_tries,
+  int64_t period_ms)
+{
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_ret_t ret =
+    rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator());
+  if (ret != RCL_RET_OK) {
+    RCUTILS_LOG_ERROR_NAMED(
+      ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str);
+    return false;
+  }
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+  {
+    if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str);
+      throw std::runtime_error("error waiting for service to be ready");
+    }
+  });
+  size_t iteration = 0;
+  while (iteration < max_tries) {
+    ++iteration;
+    if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str);
+      return false;
+    }
+    if (rcl_wait_set_add_subscription(&wait_set, subscription, NULL) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        ROS_PACKAGE_NAME, "Error in rcl_wait_set_add_subscription: %s", rcl_get_error_string().str);
+      return false;
+    }
+    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
+    if (ret == RCL_RET_TIMEOUT) {
+      continue;
+    }
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str);
+      return false;
+    }
+    for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) {
+      if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
diff --git a/rcl/test/rcl/wait_for_entity_helpers.hpp b/rcl/test/rcl/wait_for_entity_helpers.hpp
new file mode 100644
index 0000000000..35baa645b7
--- /dev/null
+++ b/rcl/test/rcl/wait_for_entity_helpers.hpp
@@ -0,0 +1,66 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCL__WAIT_FOR_ENTITY_HELPERS_HPP_
+#define RCL__WAIT_FOR_ENTITY_HELPERS_HPP_
+
+#include "rcl/client.h"
+#include "rcl/service.h"
+#include "rcl/rcl.h"
+
+/// Wait for a server to be available for `client`, by trying at most `max_tries` times
+/// with a `period_ms` period.
+bool
+wait_for_server_to_be_available(
+  rcl_node_t * node,
+  rcl_client_t * client,
+  size_t max_tries,
+  int64_t period_ms);
+
+/// Wait for `client` to be ready, i.e. a response is available to be handled.
+/// It's tried at most `max_tries` times with a period of `period_ms`.
+bool
+wait_for_client_to_be_ready(
+  rcl_client_t * client,
+  rcl_context_t * context,
+  size_t max_tries,
+  int64_t period_ms);
+
+/// Wait for service to be ready, i.e. a request is available to be handled.
+/// It's tried at most `max_tries` times with a period of `period_ms`.
+bool
+wait_for_service_to_be_ready(
+  rcl_service_t * service,
+  rcl_context_t * context,
+  size_t max_tries,
+  int64_t period_ms);
+
+/// Wait for a publisher to get one or more established subscriptions
+/// by trying at most `max_tries` times with a `period_ms` period.
+bool
+wait_for_established_subscription(
+  const rcl_publisher_t * publisher,
+  size_t max_tries,
+  int64_t period_ms);
+
+/// Wait a subscription to be ready, i.e. a message is ready to be handled,
+/// by trying at least `max_tries` times with a `period_ms` period.
+bool
+wait_for_subscription_to_be_ready(
+  rcl_subscription_t * subscription,
+  rcl_context_t * context,
+  size_t max_tries,
+  int64_t period_ms);
+
+#endif  // RCL__WAIT_FOR_ENTITY_HELPERS_HPP_