ROS1 smoke test (#72)

**Public-Facing Changes**
None


**Description**
- Fixes a small bug in the ws client and adds an utility function
- Adds a smoke test for the ROS1 implementation

Partial fix for #20

CMakeLists.txt

@@ -137,13 +137,20 @@ if(ROS_BUILD_TYPE STREQUAL "catkin")
   if (CATKIN_ENABLE_TESTING)
     message(STATUS "Building tests with catkin")
 
-    find_package(catkin REQUIRED COMPONENTS rostest)
+    find_package(catkin REQUIRED COMPONENTS roscpp std_msgs)
     find_package(GTest REQUIRED)
+    find_package(rostest REQUIRED)
 
     catkin_add_gtest(version_test foxglove_bridge_base/tests/version_test.cpp)
     target_link_libraries(version_test foxglove_bridge_base GTest::GTest GTest::Main)
 
-    add_rostest(ros1_foxglove_bridge/tests/smoke.test)
+    add_rostest_gtest(smoke_test ros1_foxglove_bridge/tests/smoke.test ros1_foxglove_bridge/tests/smoke_test.cpp)
+    target_include_directories(smoke_test SYSTEM PRIVATE
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/foxglove_bridge_base/include>
+      ${catkin_INCLUDE_DIRS}
+      $<INSTALL_INTERFACE:include>
+    )
+    target_link_libraries(smoke_test foxglove_bridge_base ${catkin_LIBRARIES})
   endif()
 elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake")
   if(BUILD_TESTING)

foxglove_bridge_base/include/foxglove_bridge/websocket_client.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <functional>
+#include <future>
 #include <shared_mutex>
 #include <utility>
 #include <vector>
@@ -22,9 +23,9 @@ using OpCode = websocketpp::frame::opcode::value;
 class ClientInterface {
 public:
   virtual void connect(
-    const std::string& uri,
-    std::function<void(websocketpp::connection_hdl)> onOpenHandler = nullptr,
+    const std::string& uri, std::function<void(websocketpp::connection_hdl)> onOpenHandler,
     std::function<void(websocketpp::connection_hdl)> onCloseHandler = nullptr) = 0;
+  virtual std::future<void> connect(const std::string& uri) = 0;
   virtual void close() = 0;
 
   virtual void subscribe(
@@ -59,12 +60,13 @@ class Client : public ClientInterface {
   }
 
   virtual ~Client() {
+    close();
     _endpoint.stop_perpetual();
     _thread->join();
   }
 
   void connect(const std::string& uri,
-               std::function<void(websocketpp::connection_hdl)> onOpenHandler = nullptr,
+               std::function<void(websocketpp::connection_hdl)> onOpenHandler,
                std::function<void(websocketpp::connection_hdl)> onCloseHandler = nullptr) override {
     std::unique_lock<std::shared_mutex> lock(_mutex);
 
@@ -86,6 +88,17 @@ class Client : public ClientInterface {
     _endpoint.connect(_con);
   }
 
+  std::future<void> connect(const std::string& uri) override {
+    auto promise = std::make_shared<std::promise<void>>();
+    auto future = promise->get_future();
+
+    connect(uri, [p = std::move(promise)](websocketpp::connection_hdl) mutable {
+      p->set_value();
+    });
+
+    return future;
+  }
+
   void close() override {
     std::unique_lock<std::shared_mutex> lock(_mutex);
     if (!_con) {
@@ -121,7 +134,7 @@ class Client : public ClientInterface {
 
   void subscribe(const std::vector<std::pair<SubscriptionId, ChannelId>>& subscriptions) override {
     nlohmann::json subscriptionsJson;
-    for (const auto [channelId, subId] : subscriptions) {
+    for (const auto [subId, channelId] : subscriptions) {
       subscriptionsJson.push_back({{"id", subId}, {"channelId", channelId}});
     }
 

package.xml

@@ -26,8 +26,8 @@
     <!-- Test dependencies -->
     <build_depend condition="$ROS_VERSION == 1">rostest</build_depend>
     <test_depend condition="$ROS_VERSION == 1">rosunit</test_depend>
-    <test_depend condition="$ROS_VERSION == 1">geometry_msgs</test_depend>
     <test_depend condition="$ROS_VERSION == 2">ament_cmake_gtest</test_depend>
+    <test_depend>std_msgs</test_depend>
 
     <!-- Common dependencies -->
     <build_depend>asio</build_depend>

ros1_foxglove_bridge/tests/smoke.test

@@ -1,5 +1,7 @@
 <launch>
-  <node name="foxglove_bridge" pkg="foxglove_bridge" type="foxglove_bridge">
-    <param name="port" value="0" />
+  <node name="foxglove_bridge" pkg="foxglove_bridge" type="foxglove_bridge" output="screen">
+    <param name="port" value="9876" />
   </node>
+
+  <test test-name="smoke_test" pkg="foxglove_bridge" type="smoke_test" />
 </launch>

ros1_foxglove_bridge/tests/smoke_test.cpp

@@ -0,0 +1,123 @@
+#include <chrono>
+#include <future>
+#include <thread>
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <websocketpp/config/asio_client.hpp>
+
+#include <foxglove_bridge/websocket_client.hpp>
+
+constexpr char URI[] = "ws://localhost:9876";
+
+// Binary representation of std_msgs/String for "hello world"
+constexpr uint8_t HELLO_WORLD_BINARY[] = {11,  0,  0,   0,   104, 101, 108, 108,
+                                          111, 32, 119, 111, 114, 108, 100};
+
+TEST(SmokeTest, testConnection) {
+  foxglove::Client<websocketpp::config::asio_client> wsClient;
+  EXPECT_EQ(std::future_status::ready, wsClient.connect(URI).wait_for(std::chrono::seconds(5)));
+}
+
+TEST(SmokeTest, testSubscription) {
+  // Publish a string message on a latched ros topic
+  const std::string topic_name = "/pub_topic";
+  ros::NodeHandle nh;
+  auto pub = nh.advertise<std_msgs::String>(topic_name, 10, true);
+
+  std_msgs::String rosMsg;
+  rosMsg.data = "hello world";
+  pub.publish(rosMsg);
+
+  const auto clientCount = 3;
+  for (auto i = 0; i < clientCount; ++i) {
+    (void)i;
+
+    // Set up text message handler to resolve the promise when the string topic is advertised
+    foxglove::Client<websocketpp::config::asio_client> wsClient;
+    std::promise<nlohmann::json> channelPromise;
+    auto channelFuture = channelPromise.get_future();
+    wsClient.setTextMessageHandler([&topic_name, &channelPromise](const std::string& payload) {
+      const auto msg = nlohmann::json::parse(payload);
+      const auto& op = msg.at("op").get<std::string>();
+      if (op == "advertise") {
+        for (const auto& channel : msg.at("channels")) {
+          if (topic_name == channel.at("topic")) {
+            channelPromise.set_value(channel);
+          }
+        }
+      }
+    });
+
+    // Connect the client and wait for the channel future
+    ASSERT_EQ(std::future_status::ready, wsClient.connect(URI).wait_for(std::chrono::seconds(5)));
+    ASSERT_EQ(std::future_status::ready, channelFuture.wait_for(std::chrono::seconds(5)));
+
+    // Set up binary message handler to resolve when a binary message has been received
+    std::promise<std::pair<const uint8_t*, size_t>> msgPromise;
+    auto msgFuture = msgPromise.get_future();
+    wsClient.setBinaryMessageHandler([&msgPromise](const uint8_t* data, size_t dataLength) {
+      const size_t offset = 1 + 4 + 8;
+      msgPromise.set_value({data + offset, dataLength - offset});
+    });
+
+    // Subscribe to the channel that corresponds to the string topic
+    const auto channelId = channelFuture.get().at("id").get<foxglove::ChannelId>();
+    wsClient.subscribe({{1, channelId}});
+
+    // Wait until we have received a binary message and test that it is the right one
+    ASSERT_EQ(std::future_status::ready, msgFuture.wait_for(std::chrono::seconds(5)));
+    const auto& [data, dataLength] = msgFuture.get();
+    ASSERT_EQ(sizeof(HELLO_WORLD_BINARY), dataLength);
+    EXPECT_TRUE(std::memcmp(HELLO_WORLD_BINARY, data, dataLength));
+  }
+}
+
+TEST(SmokeTest, testPublishing) {
+  foxglove::Client<websocketpp::config::asio_client> wsClient;
+
+  foxglove::ClientAdvertisement advertisement;
+  advertisement.channelId = 1;
+  advertisement.topic = "/foo";
+  advertisement.encoding = "ros1";
+  advertisement.schemaName = "std_msgs/String";
+
+  // Set up a ROS node with a subscriber
+  ros::NodeHandle nh;
+  std::promise<std::string> msgPromise;
+  auto msgFuture = msgPromise.get_future();
+  auto subscriber = nh.subscribe<std_msgs::String>(
+    advertisement.topic, 10, [&msgPromise](const std_msgs::String::ConstPtr& msg) {
+      msgPromise.set_value(msg->data);
+    });
+
+  // Set up the client, advertise and publish the binary message
+  ASSERT_EQ(std::future_status::ready, wsClient.connect(URI).wait_for(std::chrono::seconds(5)));
+  wsClient.advertise({advertisement});
+  std::this_thread::sleep_for(std::chrono::seconds(1));
+  wsClient.publish(advertisement.channelId, HELLO_WORLD_BINARY, sizeof(HELLO_WORLD_BINARY));
+  wsClient.unadvertise({advertisement.channelId});
+
+  // Ensure that we have received the correct message via our ROS subscriber
+  const auto msgResult = msgFuture.wait_for(std::chrono::seconds(1));
+  ASSERT_EQ(std::future_status::ready, msgResult);
+  EXPECT_EQ("hello world", msgFuture.get());
+}
+
+// Run all the tests that were declared with TEST()
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "tester");
+  ros::NodeHandle nh;
+
+  // Give the server some time to start
+  std::this_thread::sleep_for(std::chrono::seconds(2));
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+  const auto testResult = RUN_ALL_TESTS();
+  spinner.stop();
+
+  return testResult;
+}