diff --git a/ros2_foxglove_bridge/tests/smoke_test.cpp b/ros2_foxglove_bridge/tests/smoke_test.cpp index 4480f78..86e6651 100644 --- a/ros2_foxglove_bridge/tests/smoke_test.cpp +++ b/ros2_foxglove_bridge/tests/smoke_test.cpp @@ -166,6 +166,7 @@ TEST(SmokeTest, testSubscription) { auto node = rclcpp::Node::make_shared("tester"); rclcpp::QoS qos = rclcpp::QoS{rclcpp::KeepLast(1lu)}; + qos.reliable(); qos.transient_local(); auto pub = node->create_publisher(topic_name, qos); pub->publish(rosMsg); @@ -202,6 +203,7 @@ TEST(SmokeTest, testSubscriptionParallel) { auto node = rclcpp::Node::make_shared("tester"); rclcpp::QoS qos = rclcpp::QoS{rclcpp::KeepLast(1lu)}; + qos.reliable(); qos.transient_local(); auto pub = node->create_publisher(topic_name, qos); pub->publish(rosMsg); @@ -258,12 +260,17 @@ TEST(SmokeTest, testPublishing) { executor.add_node(node); // Set up the client, advertise and publish the binary message - foxglove::Client wsClient; - ASSERT_EQ(std::future_status::ready, wsClient.connect(URI).wait_for(DEFAULT_TIMEOUT)); - wsClient.advertise({advertisement}); - std::this_thread::sleep_for(ONE_SECOND); - wsClient.publish(advertisement.channelId, HELLO_WORLD_BINARY, sizeof(HELLO_WORLD_BINARY)); - wsClient.unadvertise({advertisement.channelId}); + auto client = std::make_shared>(); + ASSERT_EQ(std::future_status::ready, client->connect(URI).wait_for(ONE_SECOND)); + client->advertise({advertisement}); + + // Wait until the advertisement got advertised as channel by the server + auto channelFuture = foxglove::waitForChannel(client, advertisement.topic); + ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(ONE_SECOND)); + + // Publish the message and unadvertise again + client->publish(advertisement.channelId, HELLO_WORLD_BINARY, sizeof(HELLO_WORLD_BINARY)); + client->unadvertise({advertisement.channelId}); // Ensure that we have received the correct message via our ROS subscriber const auto ret = executor.spin_until_future_complete(msgFuture, ONE_SECOND); @@ -290,12 +297,17 @@ TEST_F(ExistingPublisherTest, testPublishingWithExistingPublisher) { executor.add_node(node); // Set up the client, advertise and publish the binary message - foxglove::Client wsClient; - ASSERT_EQ(std::future_status::ready, wsClient.connect(URI).wait_for(DEFAULT_TIMEOUT)); - wsClient.advertise({advertisement}); - std::this_thread::sleep_for(ONE_SECOND); - wsClient.publish(advertisement.channelId, HELLO_WORLD_BINARY, sizeof(HELLO_WORLD_BINARY)); - wsClient.unadvertise({advertisement.channelId}); + auto client = std::make_shared>(); + ASSERT_EQ(std::future_status::ready, client->connect(URI).wait_for(ONE_SECOND)); + client->advertise({advertisement}); + + // Wait until the advertisement got advertised as channel by the server + auto channelFuture = foxglove::waitForChannel(client, advertisement.topic); + ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(ONE_SECOND)); + + // Publish the message and unadvertise again + client->publish(advertisement.channelId, HELLO_WORLD_BINARY, sizeof(HELLO_WORLD_BINARY)); + client->unadvertise({advertisement.channelId}); // Ensure that we have received the correct message via our ROS subscriber const auto ret = executor.spin_until_future_complete(msgFuture, ONE_SECOND);