Add ROS1 support for calling server-advertised services

CMakeLists.txt

@@ -106,6 +106,7 @@ if("$ENV{ROS_VERSION}" STREQUAL "1")
     add_library(foxglove_bridge_nodelet
       ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp
       ros1_foxglove_bridge/src/param_utils.cpp
+      ros1_foxglove_bridge/src/service_utils.cpp
     )
     target_include_directories(foxglove_bridge_nodelet
       SYSTEM PRIVATE
@@ -167,7 +168,7 @@ if(ROS_BUILD_TYPE STREQUAL "catkin")
   if (CATKIN_ENABLE_TESTING)
     message(STATUS "Building tests with catkin")
 
-    find_package(catkin REQUIRED COMPONENTS roscpp std_msgs)
+    find_package(catkin REQUIRED COMPONENTS roscpp std_msgs std_srvs)
     if(NOT "$ENV{ROS_DISTRO}" STREQUAL "melodic")
       find_package(GTest REQUIRED)
     endif()
@@ -176,6 +177,9 @@ if(ROS_BUILD_TYPE STREQUAL "catkin")
     catkin_add_gtest(version_test foxglove_bridge_base/tests/version_test.cpp)
     target_link_libraries(version_test foxglove_bridge_base)
 
+    catkin_add_gtest(serialization_test foxglove_bridge_base/tests/serialization_test.cpp)
+    target_link_libraries(serialization_test foxglove_bridge_base)
+
     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>
@@ -195,6 +199,9 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake")
     ament_add_gtest(version_test foxglove_bridge_base/tests/version_test.cpp)
     target_link_libraries(version_test foxglove_bridge_base)
 
+    ament_add_gtest(serialization_test foxglove_bridge_base/tests/serialization_test.cpp)
+    target_link_libraries(serialization_test foxglove_bridge_base)
+
     ament_add_gtest(smoke_test ros2_foxglove_bridge/tests/smoke_test.cpp)
     ament_target_dependencies(smoke_test rclcpp rclcpp_components std_msgs)
     target_link_libraries(smoke_test foxglove_bridge_base)

README.md

@@ -49,6 +49,8 @@ Parameters are provided to configure the behavior of the bridge. These parameter
  * __certfile__: Path to the certificate to use for TLS. Required when __tls__ is set to `true`. Defaults to `""`.
  * __keyfile__: Path to the private key to use for TLS. Required when __tls__ is set to `true`. Defaults to `""`.
  * __topic_whitelist__: List of regular expressions ([ECMAScript grammar](https://en.cppreference.com/w/cpp/regex/ecmascript)) of whitelisted topic names. Defaults to `[".*"]`.
+ * __service_whitelist__: List of regular expressions ([ECMAScript grammar](https://en.cppreference.com/w/cpp/regex/ecmascript)) of whitelisted service names. Defaults to `[".*"]`.
+ * __param_whitelist__: List of regular expressions ([ECMAScript grammar](https://en.cppreference.com/w/cpp/regex/ecmascript)) of whitelisted parameter names. Defaults to `[".*"]`.
  * __send_buffer_limit__: Connection send buffer limit in bytes. Messages will be dropped when a connection's send buffer reaches this limit to avoid a queue of outdated messages building up. Defaults to `10000000` (10 MB).
  * (ROS 1) __max_update_ms__: The maximum number of milliseconds to wait in between polling `roscore` for new topics, services, or parameters. Defaults to `5000`.
  * (ROS 2) __num_threads__: The number of threads to use for the ROS node executor. This controls the number of subscriptions that can be processed in parallel. 0 means one thread per CPU core. Defaults to `0`.

foxglove_bridge_base/include/foxglove_bridge/common.hpp

@@ -11,18 +11,22 @@ constexpr char CAPABILITY_CLIENT_PUBLISH[] = "clientPublish";
 constexpr char CAPABILITY_TIME[] = "time";
 constexpr char CAPABILITY_PARAMETERS[] = "parameters";
 constexpr char CAPABILITY_PARAMETERS_SUBSCRIBE[] = "parametersSubscribe";
+constexpr char CAPABILITY_SERVICES[] = "services";
 
 using ChannelId = uint32_t;
 using ClientChannelId = uint32_t;
 using SubscriptionId = uint32_t;
+using ServiceId = uint32_t;
 
 enum class BinaryOpcode : uint8_t {
   MESSAGE_DATA = 1,
   TIME_DATA = 2,
+  SERVICE_CALL_RESPONSE = 3,
 };
 
 enum class ClientBinaryOpcode : uint8_t {
   MESSAGE_DATA = 1,
+  SERVICE_CALL_REQUEST = 2,
 };
 
 struct ClientAdvertisement {
@@ -33,4 +37,40 @@ struct ClientAdvertisement {
   std::vector<uint8_t> schema;
 };
 
+struct ServiceWithoutId {
+  std::string name;
+  std::string type;
+  std::string requestSchema;
+  std::string responseSchema;
+};
+
+struct Service : ServiceWithoutId {
+  ServiceId id;
+
+  Service() = default;
+  Service(const ServiceWithoutId& s, const ServiceId& id)
+      : ServiceWithoutId(s)
+      , id(id) {}
+};
+
+struct ServiceResponse {
+  ServiceId serviceId;
+  uint32_t callId;
+  std::string encoding;
+  std::vector<uint8_t> data;
+
+  size_t size() const {
+    return 4 + 4 + 4 + encoding.size() + data.size();
+  }
+  void read(const uint8_t* data, size_t size);
+  void write(uint8_t* data) const;
+
+  bool operator==(const ServiceResponse& other) const {
+    return serviceId == other.serviceId && callId == other.callId && encoding == other.encoding &&
+           data == other.data;
+  }
+};
+
+using ServiceRequest = ServiceResponse;
+
 }  // namespace foxglove

foxglove_bridge_base/include/foxglove_bridge/serialization.hpp

@@ -4,6 +4,7 @@
 
 #include <nlohmann/json.hpp>
 
+#include "common.hpp"
 #include "parameter.hpp"
 
 namespace foxglove {
@@ -36,5 +37,7 @@ inline void WriteUint32LE(uint8_t* buf, uint32_t val) {
 
 void to_json(nlohmann::json& j, const Parameter& p);
 void from_json(const nlohmann::json& j, Parameter& p);
+void to_json(nlohmann::json& j, const Service& p);
+void from_json(const nlohmann::json& j, Service& p);
 
 }  // namespace foxglove

foxglove_bridge_base/include/foxglove_bridge/test/test_client.hpp

@@ -17,6 +17,11 @@ std::vector<uint8_t> connectClientAndReceiveMsg(const std::string& uri,
 std::future<std::vector<Parameter>> waitForParameters(std::shared_ptr<ClientInterface> client,
                                                       const std::string& requestId = std::string());
 
+std::future<ServiceResponse> waitForServiceResponse(std::shared_ptr<ClientInterface> client);
+
+std::future<Service> waitForService(std::shared_ptr<ClientInterface> client,
+                                    const std::string& serviceName);
+
 extern template class Client<websocketpp::config::asio_client>;
 
 }  // namespace foxglove

foxglove_bridge_base/include/foxglove_bridge/websocket_client.hpp

@@ -43,6 +43,7 @@ class ClientInterface {
   virtual void advertise(const std::vector<ClientAdvertisement>& channels) = 0;
   virtual void unadvertise(const std::vector<ClientChannelId>& channelIds) = 0;
   virtual void publish(ClientChannelId channelId, const uint8_t* buffer, size_t size) = 0;
+  virtual void sendServiceRequest(const ServiceRequest& request) = 0;
   virtual void getParameters(const std::vector<std::string>& parameterNames,
                              const std::optional<std::string>& requestId) = 0;
   virtual void setParameters(const std::vector<Parameter>& parameters,
@@ -183,6 +184,13 @@ class Client : public ClientInterface {
     sendBinary(payload.data(), payload.size());
   }
 
+  void sendServiceRequest(const ServiceRequest& request) override {
+    std::vector<uint8_t> payload(1 + request.size());
+    payload[0] = uint8_t(ClientBinaryOpcode::SERVICE_CALL_REQUEST);
+    request.write(payload.data() + 1);
+    sendBinary(payload.data(), payload.size());
+  }
+
   void getParameters(const std::vector<std::string>& parameterNames,
                      const std::optional<std::string>& requestId = std::nullopt) override {
     nlohmann::json jsonPayload{{"op", "getParameters"}, {"parameterNames", parameterNames}};