Fix test flakes by waiting for pub/sub discovery (#55)

We should ensure that the publisher and subscription are connected before publishing test data,
otherwise we risk the data never arriving.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

test/interactive_markers/test_interactive_marker_client.cpp

@@ -145,6 +145,26 @@ class TestInteractiveMarkerClient : public ::testing::Test
     }
   }
 
+  /// Wait for client and server to discover each other or die trying
+  void waitForDiscovery(
+    std::shared_ptr<MockInteractiveMarkerServer> server,
+    std::chrono::seconds timeout = std::chrono::seconds(3))
+  {
+    const auto start_time = std::chrono::system_clock::now();
+    while (
+      (server->publisher_->get_subscription_count() == 0u ||
+      server->subscription_->get_publisher_count() == 0u) &&
+      (std::chrono::system_clock::now() - start_time) < timeout)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    ASSERT_GT(server->publisher_->get_subscription_count(), 0u);
+    ASSERT_GT(server->subscription_->get_publisher_count(), 0u);
+    // TODO(jacobperron): We should probably also wait for the client to discover the server
+    //                    to avoid flakes. This requires additional interactive marker client API.
+  }
+
   std::string target_frame_id_;
   std::string topic_namespace_;
   std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;
@@ -229,6 +249,7 @@ TEST_F(TestInteractiveMarkerClient, states)
     // Start server
     auto mock_server = std::make_shared<MockInteractiveMarkerServer>(topic_namespace_);
     executor_->add_node(mock_server);
+    waitForDiscovery(mock_server);
     // Repeatedly call the client's update method until the server is detected
     auto update_func = std::bind(
       &interactive_markers::InteractiveMarkerClient::update, client_.get());
@@ -289,6 +310,7 @@ TEST_F(TestInteractiveMarkerClient, update_callback)
   // First, we need to get into the RUNNING state
   auto mock_server = std::make_shared<MockInteractiveMarkerServer>(topic_namespace_);
   executor_->add_node(mock_server);
+  waitForDiscovery(mock_server);
   makeTfDataAvailable(mock_server->markers_);
   auto update_func = std::bind(
     &interactive_markers::InteractiveMarkerClient::update, client_.get());
@@ -344,6 +366,7 @@ TEST_F(TestInteractiveMarkerClient, feedback)
 
   auto mock_server = std::make_shared<MockInteractiveMarkerServer>(topic_namespace_);
   executor_->add_node(mock_server);
+  waitForDiscovery(mock_server);
 
   EXPECT_EQ(mock_server->feedback_received_, 0u);
 

test/interactive_markers/test_interactive_marker_server.cpp

@@ -34,6 +34,7 @@
 #include <chrono>
 #include <memory>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include "interactive_marker_fixtures.hpp"
@@ -141,6 +142,16 @@ class TestInteractiveMarkerServerWithMarkers : public ::testing::Test
 
     // Wait for discovery (or timeout)
     ASSERT_TRUE(mock_client_->client_->wait_for_service(std::chrono::seconds(3)));
+    const auto start_time = std::chrono::system_clock::now();
+    while (
+      mock_client_->publisher_->get_subscription_count() == 0u &&
+      (std::chrono::system_clock::now() - start_time) < std::chrono::seconds(3))
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+    ASSERT_EQ(mock_client_->publisher_->get_subscription_count(), 1u);
+    // TODO(jacobperron): We should probably also wait for the server to discover the client
+    //                    to avoid flakes. This requires additional interactive marker server API.
   }
 
   void TearDown()