Add ROS2 support for calling server-advertised services (#142)

**Public-Facing Changes**
- Add ROS2 support for calling server-advertised services

**Description**
Based on #136 

- Adds experimental ROS2 support for advertising/unadvertising services
and allowing clients to call them
- Implements the services spec that was added in
foxglove/ws-protocol#344

Implementation details:

The main problem is, that symbols from getting the service type support
are missing from the `rosidl_typesupport_cpp` libraries that are
generated for each msg/srv package (see
ros2/rosidl_typesupport#122). There is an open
pull request (ros2/rosidl_typesupport#114) to
fix that, but so far it hasn't been merged.

I found a working, yet little bit hacky, workaround for this problem
which is described more in detail here:

https://github.com/foxglove/ros-foxglove-bridge/blob/f978c182185e5deda93a6518132b6d3c339dcaeb/ros2_foxglove_bridge/src/generic_client.cpp#L59-L89

All in all, the implementation is working, but I would consider it as
experimental for now. I am not sure if this approach would work on
windows / mac

CMakeLists.txt

@@ -139,6 +139,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2")
       ros2_foxglove_bridge/src/param_utils.cpp
       ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp
       ros2_foxglove_bridge/src/parameter_interface.cpp
+      ros2_foxglove_bridge/src/generic_client.cpp
     )
     target_include_directories(foxglove_bridge_component
       PUBLIC
@@ -203,7 +204,7 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake")
     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)
+    ament_target_dependencies(smoke_test rclcpp rclcpp_components std_msgs std_srvs)
     target_link_libraries(smoke_test foxglove_bridge_base)
   endif()
 endif()

ros2_foxglove_bridge/include/foxglove_bridge/generic_client.hpp

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <future>
+
+#include <rclcpp/client.hpp>
+#include <rclcpp/serialized_message.hpp>
+#include <rcpputils/shared_library.hpp>
+
+namespace foxglove_bridge {
+
+class GenericClient : public rclcpp::ClientBase {
+public:
+  using SharedRequest = std::shared_ptr<rclcpp::SerializedMessage>;
+  using SharedResponse = std::shared_ptr<rclcpp::SerializedMessage>;
+  using Promise = std::promise<SharedResponse>;
+  using PromiseWithRequest = std::promise<std::pair<SharedRequest, SharedResponse>>;
+  using SharedPromise = std::shared_ptr<Promise>;
+  using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
+  using SharedFuture = std::shared_future<SharedResponse>;
+  using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
+  using CallbackType = std::function<void(SharedFuture)>;
+  using CallbackWithRequestType = std::function<void(SharedFutureWithRequest)>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericClient)
+
+  GenericClient(rclcpp::node_interfaces::NodeBaseInterface* node_base,
+                rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+                std::string service_name, std::string service_type,
+                rcl_client_options_t& client_options);
+  virtual ~GenericClient() {}
+
+  std::shared_ptr<void> create_response() override;
+  std::shared_ptr<rmw_request_id_t> create_request_header() override;
+  void handle_response(std::shared_ptr<rmw_request_id_t> request_header,
+                       std::shared_ptr<void> response) override;
+  SharedFuture async_send_request(SharedRequest request);
+  SharedFuture async_send_request(SharedRequest request, CallbackType&& cb);
+
+private:
+  RCLCPP_DISABLE_COPY(GenericClient)
+
+  std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
+  std::mutex pending_requests_mutex_;
+  std::shared_ptr<rcpputils::SharedLibrary> _typeSupportLib;
+  std::shared_ptr<rcpputils::SharedLibrary> _typeIntrospectionLib;
+  const rosidl_service_type_support_t* _serviceTypeSupportHdl;
+  const rosidl_message_type_support_t* _requestTypeSupportHdl;
+  const rosidl_message_type_support_t* _responseTypeSupportHdl;
+  const rosidl_service_type_support_t* _typeIntrospectionHdl;
+};
+
+}  // namespace foxglove_bridge

ros2_foxglove_bridge/include/foxglove_bridge/param_utils.hpp

@@ -16,6 +16,7 @@ constexpr char PARAM_CERTFILE[] = "certfile";
 constexpr char PARAM_KEYFILE[] = "keyfile";
 constexpr char PARAM_MAX_QOS_DEPTH[] = "max_qos_depth";
 constexpr char PARAM_TOPIC_WHITELIST[] = "topic_whitelist";
+constexpr char PARAM_SERVICE_WHITELIST[] = "service_whitelist";
 constexpr char PARAM_PARAMETER_WHITELIST[] = "param_whitelist";
 
 constexpr int64_t DEFAULT_PORT = 8765;

ros2_foxglove_bridge/src/generic_client.cpp

@@ -0,0 +1,209 @@
+#include <future>
+#include <iostream>
+
+#include <rclcpp/client.hpp>
+#include <rclcpp/serialized_message.hpp>
+#include <rclcpp/typesupport_helpers.hpp>
+#include <rosidl_typesupport_introspection_cpp/field_types.hpp>
+#include <rosidl_typesupport_introspection_cpp/service_introspection.hpp>
+
+#include <foxglove_bridge/generic_client.hpp>
+
+namespace {
+
+// Copy of github.com/ros2/rclcpp/blob/33dae5d67/rclcpp/src/rclcpp/typesupport_helpers.cpp#L69-L92
+static std::tuple<std::string, std::string, std::string> extract_type_identifier(
+  const std::string& full_type) {
+  char type_separator = '/';
+  auto sep_position_back = full_type.find_last_of(type_separator);
+  auto sep_position_front = full_type.find_first_of(type_separator);
+  if (sep_position_back == std::string::npos || sep_position_back == 0 ||
+      sep_position_back == full_type.length() - 1) {
+    throw std::runtime_error(
+      "Message type is not of the form package/type and cannot be processed");
+  }
+
+  std::string package_name = full_type.substr(0, sep_position_front);
+  std::string middle_module = "";
+  if (sep_position_back - sep_position_front > 0) {
+    middle_module =
+      full_type.substr(sep_position_front + 1, sep_position_back - sep_position_front - 1);
+  }
+  std::string type_name = full_type.substr(sep_position_back + 1);
+
+  return std::make_tuple(package_name, middle_module, type_name);
+}
+}  // namespace
+
+namespace foxglove_bridge {
+
+constexpr char TYPESUPPORT_INTROSPECTION_LIB_NAME[] = "rosidl_typesupport_introspection_cpp";
+constexpr char TYPESUPPORT_LIB_NAME[] = "rosidl_typesupport_cpp";
+using rosidl_typesupport_introspection_cpp::MessageMembers;
+using rosidl_typesupport_introspection_cpp::ServiceMembers;
+
+std::shared_ptr<void> allocate_message(const MessageMembers* members) {
+  uint8_t* buf = static_cast<uint8_t*>(malloc(members->size_of_));
+  memset(buf, 0, members->size_of_);
+  members->init_function(buf, rosidl_runtime_cpp::MessageInitialization::ALL);
+  return std::shared_ptr<void>(buf);
+}
+
+std::string getTypeIntrospectionSymbolName(const std::string& serviceType) {
+  const auto [pkgName, middleModule, typeName] = extract_type_identifier(serviceType);
+
+  return std::string(TYPESUPPORT_INTROSPECTION_LIB_NAME) + "__get_service_type_support_handle__" +
+         pkgName + "__" + (middleModule.empty() ? "srv" : middleModule) + "__" + typeName;
+}
+
+/**
+ * The default symbol names for getting type support handles for services are missing from the
+ * rosidl_typesupport_cpp shared libraries, see
+ * https://github.com/ros2/rosidl_typesupport/issues/122
+ *
+ * We can however, as a (hacky) workaround, use other symbols defined in the shared library.
+ * With `nm -C -D /opt/ros/humble/lib/libtest_msgs__rosidl_typesupport_cpp.so` we see that there is
+ * `rosidl_service_type_support_t const*
+ * rosidl_typesupport_cpp::get_service_type_support_handle<test_msgs::srv::BasicTypes>()` which
+ * mangled becomes
+ * `_ZN22rosidl_typesupport_cpp31get_service_type_support_handleIN9test_msgs3srv10BasicTypesEEEPK29rosidl_service_type_support_tv`
+ * This is the same for galactic, humble and rolling (tested with gcc / clang)
+ *
+ * This function produces the mangled symbol name for a given service type.
+ *
+ * \param[in] serviceType The service type, e.g. "test_msgs/srv/BasicTypes"
+ * \return Symbol name for getting the service type support handle
+ */
+std::string getServiceTypeSupportHandleSymbolName(const std::string& serviceType) {
+  const auto [pkgName, middleModule, typeName] = extract_type_identifier(serviceType);
+  const auto lengthPrefixedString = [](const std::string& s) {
+    return std::to_string(s.size()) + s;
+  };
+
+  return "_ZN" + lengthPrefixedString(TYPESUPPORT_LIB_NAME) +
+         lengthPrefixedString("get_service_type_support_handle") + "IN" +
+         lengthPrefixedString(pkgName) +
+         lengthPrefixedString(middleModule.empty() ? "srv" : middleModule) +
+         lengthPrefixedString(typeName) + "EEEPK" +
+         lengthPrefixedString("rosidl_service_type_support_t") + "v";
+}
+
+GenericClient::GenericClient(rclcpp::node_interfaces::NodeBaseInterface* nodeBase,
+                             rclcpp::node_interfaces::NodeGraphInterface::SharedPtr nodeGraph,
+                             std::string serviceName, std::string serviceType,
+                             rcl_client_options_t& client_options)
+    : rclcpp::ClientBase(nodeBase, nodeGraph) {
+  const auto [pkgName, middleModule, typeName] = extract_type_identifier(serviceType);
+  const auto requestTypeName = serviceType + "_Request";
+  const auto responseTypeName = serviceType + "_Response";
+
+  _typeSupportLib = rclcpp::get_typesupport_library(serviceType, TYPESUPPORT_LIB_NAME);
+  _typeIntrospectionLib =
+    rclcpp::get_typesupport_library(serviceType, TYPESUPPORT_INTROSPECTION_LIB_NAME);
+  if (!_typeSupportLib || !_typeIntrospectionLib) {
+    throw std::runtime_error("Failed to load shared library for service type " + serviceType);
+  }
+
+  const auto typesupportSymbolName = getServiceTypeSupportHandleSymbolName(serviceType);
+  if (!_typeSupportLib->has_symbol(typesupportSymbolName)) {
+    throw std::runtime_error("Failed to find symbol '" + typesupportSymbolName + "' in " +
+                             _typeSupportLib->get_library_path());
+  }
+
+  const rosidl_service_type_support_t* (*get_ts)() = nullptr;
+  _serviceTypeSupportHdl =
+    (reinterpret_cast<decltype(get_ts)>(_typeSupportLib->get_symbol(typesupportSymbolName)))();
+
+  const auto typeinstrospection_symbol_name = getTypeIntrospectionSymbolName(serviceType);
+
+  // This will throw runtime_error if the symbol was not found.
+  _typeIntrospectionHdl = (reinterpret_cast<decltype(get_ts)>(
+    _typeIntrospectionLib->get_symbol(typeinstrospection_symbol_name)))();
+
+  _requestTypeSupportHdl =
+    rclcpp::get_typesupport_handle(requestTypeName, TYPESUPPORT_LIB_NAME, *_typeSupportLib);
+  _responseTypeSupportHdl =
+    rclcpp::get_typesupport_handle(responseTypeName, TYPESUPPORT_LIB_NAME, *_typeSupportLib);
+
+  rcl_ret_t ret = rcl_client_init(this->get_client_handle().get(), this->get_rcl_node_handle(),
+                                  _serviceTypeSupportHdl, serviceName.c_str(), &client_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+      auto rcl_node_handle = this->get_rcl_node_handle();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      rclcpp::expand_topic_or_service_name(serviceName, rcl_node_get_name(rcl_node_handle),
+                                           rcl_node_get_namespace(rcl_node_handle), true);
+    }
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
+  }
+}
+
+std::shared_ptr<void> GenericClient::create_response() {
+  auto srv_members = static_cast<const ServiceMembers*>(_typeIntrospectionHdl->data);
+  return allocate_message(srv_members->response_members_);
+}
+
+std::shared_ptr<rmw_request_id_t> GenericClient::create_request_header() {
+  return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
+}
+
+void GenericClient::handle_response(std::shared_ptr<rmw_request_id_t> request_header,
+                                    std::shared_ptr<void> response) {
+  std::unique_lock<std::mutex> lock(pending_requests_mutex_);
+  int64_t sequence_number = request_header->sequence_number;
+
+  auto ser_response = std::make_shared<rclcpp::SerializedMessage>();
+  rmw_ret_t r = rmw_serialize(response.get(), _responseTypeSupportHdl,
+                              &ser_response->get_rcl_serialized_message());
+  if (r != RMW_RET_OK) {
+    RCUTILS_LOG_ERROR_NAMED("foxglove_bridge", "Failed to serialize service response. Ignoring...");
+    return;
+  }
+
+  // TODO(esteve) this should throw instead since it is not expected to happen in the first place
+  if (this->pending_requests_.count(sequence_number) == 0) {
+    RCUTILS_LOG_ERROR_NAMED("foxglove_bridge", "Received invalid sequence number. Ignoring...");
+    return;
+  }
+  auto tuple = this->pending_requests_[sequence_number];
+  auto call_promise = std::get<0>(tuple);
+  auto callback = std::get<1>(tuple);
+  auto future = std::get<2>(tuple);
+  this->pending_requests_.erase(sequence_number);
+  // Unlock here to allow the service to be called recursively from one of its callbacks.
+  lock.unlock();
+
+  call_promise->set_value(ser_response);
+  callback(future);
+}
+
+GenericClient::SharedFuture GenericClient::async_send_request(SharedRequest request) {
+  return async_send_request(request, [](SharedFuture) {});
+}
+
+GenericClient::SharedFuture GenericClient::async_send_request(SharedRequest request,
+                                                              CallbackType&& cb) {
+  std::lock_guard<std::mutex> lock(pending_requests_mutex_);
+  int64_t sequence_number;
+
+  auto srv_members = static_cast<const ServiceMembers*>(_typeIntrospectionHdl->data);
+  auto buf = allocate_message(srv_members->request_members_);
+
+  const rmw_serialized_message_t* sm = &request->get_rcl_serialized_message();
+  if (const auto ret = rmw_deserialize(sm, _requestTypeSupportHdl, buf.get()) != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to desirialize request");
+  }
+  rcl_ret_t ret = rcl_send_request(get_client_handle().get(), buf.get(), &sequence_number);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
+  }
+
+  SharedPromise call_promise = std::make_shared<Promise>();
+  SharedFuture f(call_promise->get_future());
+  pending_requests_[sequence_number] =
+    std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
+  return f;
+}
+
+}  // namespace foxglove_bridge

ros2_foxglove_bridge/src/message_definition_cache.cpp

@@ -16,8 +16,9 @@
 
 namespace foxglove {
 
-// Match datatype names (foo_msgs/Bar or foo_msgs/msg/Bar)
-static const std::regex PACKAGE_TYPENAME_REGEX{R"(^([a-zA-Z0-9_]+)/(?:msg/)?([a-zA-Z0-9_]+)$)"};
+// Match datatype names (foo_msgs/Bar or foo_msgs/msg/Bar or foo_msgs/srv/Bar)
+static const std::regex PACKAGE_TYPENAME_REGEX{
+  R"(^([a-zA-Z0-9_]+)/(?:msg/|srv/)?([a-zA-Z0-9_]+)$)"};
 
 // Match field types from .msg definitions ("foo_msgs/Bar" in "foo_msgs/Bar[] bar")
 static const std::regex MSG_FIELD_TYPE_REGEX{R"((?:^|\n)\s*([a-zA-Z0-9_/]+)(?:\[[^\]]*\])?\s+)"};

ros2_foxglove_bridge/src/param_utils.cpp

@@ -80,6 +80,15 @@ void declareParameters(rclcpp::Node* node) {
   node->declare_parameter(PARAM_TOPIC_WHITELIST, std::vector<std::string>({".*"}),
                           topicWhiteListDescription);
 
+  auto serviceWhiteListDescription = rcl_interfaces::msg::ParameterDescriptor{};
+  serviceWhiteListDescription.name = PARAM_SERVICE_WHITELIST;
+  serviceWhiteListDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY;
+  serviceWhiteListDescription.description =
+    "List of regular expressions (ECMAScript) of whitelisted service names.";
+  serviceWhiteListDescription.read_only = true;
+  node->declare_parameter(PARAM_SERVICE_WHITELIST, std::vector<std::string>({".*"}),
+                          serviceWhiteListDescription);
+
   auto paramWhiteListDescription = rcl_interfaces::msg::ParameterDescriptor{};
   paramWhiteListDescription.name = PARAM_PARAMETER_WHITELIST;
   paramWhiteListDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY;