Support rosbridge advertised services

.github/workflows/ci.yml

@@ -0,0 +1,67 @@
+name: CI
+
+on:
+  pull_request:
+  push:
+    branches:
+      - main
+
+jobs:
+  ros-humble:
+    name: ROS Humble build and tests
+    runs-on: ubuntu-22.04
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+        with:
+          path: ros_ws/src/rws
+
+      - name: Install ROS Humble and dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y curl git gnupg software-properties-common
+          sudo add-apt-repository -y universe
+          sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
+            -o /usr/share/keyrings/ros-archive-keyring.gpg
+          echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo "$UBUNTU_CODENAME") main" | \
+            sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+          sudo apt-get update
+          sudo apt-get install -y --no-install-recommends \
+            python3-colcon-common-extensions \
+            ros-humble-ament-cmake \
+            ros-humble-ament-cmake-gmock \
+            ros-humble-ament-cmake-gtest \
+            ros-humble-ament-index-cpp \
+            ros-humble-ament-lint-auto \
+            ros-humble-ament-lint-common \
+            ros-humble-action-msgs \
+            ros-humble-rcl-interfaces \
+            ros-humble-rclcpp \
+            ros-humble-rclcpp-action \
+            ros-humble-ros-base \
+            ros-humble-rosidl-typesupport-cpp \
+            ros-humble-rosidl-typesupport-introspection-cpp \
+            ros-humble-std-msgs \
+            ros-humble-std-srvs \
+            ros-humble-test-msgs \
+            ros-humble-unique-identifier-msgs
+
+      - name: Build
+        working-directory: ros_ws
+        run: |
+          source /opt/ros/humble/setup.bash
+          colcon build \
+            --packages-select rws \
+            --event-handlers console_direct+ \
+            --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
+
+      - name: Test
+        working-directory: ros_ws
+        run: |
+          source /opt/ros/humble/setup.bash
+          colcon test \
+            --packages-select rws \
+            --event-handlers console_direct+ \
+            --return-code-on-test-failure
+          colcon test-result --verbose

CMakeLists.txt

@@ -53,11 +53,11 @@ fetchcontent_getproperties(websocketpp)
 if(NOT websocketpp_POPULATED)
   fetchcontent_populate(websocketpp)
   add_subdirectory(${websocketpp_SOURCE_DIR} ${websocketpp_BINARY_DIR} EXCLUDE_FROM_ALL)
-  
+
   # Patch hybi13.hpp to skip outgoing UTF-8 validation for TEXT frames
   # This improves performance when we know our JSON output is valid UTF-8
   file(READ "${websocketpp_SOURCE_DIR}/websocketpp/processors/hybi13.hpp" HYBI13_CONTENT)
-  string(REPLACE 
+  string(REPLACE
     "// validate payload utf8\n        if (op == frame::opcode::TEXT && !utf8_validator::validate(i)) {\n            return make_error_code(error::invalid_payload);\n        }"
     "// UTF-8 validation disabled for outgoing TEXT frames (we generate valid UTF-8)\n        // if (op == frame::opcode::TEXT && !utf8_validator::validate(i)) {\n        //     return make_error_code(error::invalid_payload);\n        // }"
     HYBI13_CONTENT "${HYBI13_CONTENT}")
@@ -105,6 +105,8 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   find_package(rcl_interfaces REQUIRED)
+  find_package(std_msgs REQUIRED)
+  find_package(std_srvs REQUIRED)
 
   # the following lines skip linters
   set(ament_cmake_cppcheck_FOUND TRUE)
@@ -164,6 +166,8 @@ if(BUILD_TESTING)
     src/generic_action_client.cpp
   )
   ament_target_dependencies(client_handler_test ${DEPENDENCIES})
+  ament_target_dependencies(client_handler_test std_msgs)
+  ament_target_dependencies(client_handler_test std_srvs)
   target_include_directories(client_handler_test PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>

README.md

@@ -51,10 +51,10 @@ rws_server_node = Node(
 | subscribe | + | + | Subscribe to topic |
 | unsubscribe | + | + | Unsubscribe from topic |
 | call_service | + | + | Call service (with timeout support) |
-| advertise_service | - | + | Advertise external service |
-| unadvertise_service | - | + | Stop advertising external service |
+| advertise_service | + | + | Advertise external service |
+| unadvertise_service | + | + | Stop advertising external service |
 | service_request | - | + | Request to external service |
-| service_response | - | + | Response from external service |
+| service_response | + | + | Response from external service |
 | send_action_goal | + | + | Send action goal (with feedback) |
 | cancel_action_goal | + | + | Cancel action goal |
 | advertise_action | - | + | Advertise action server |

include/rws/client_handler.hpp

@@ -15,6 +15,9 @@
 #ifndef RWS__NODE_HPP_
 #define RWS__NODE_HPP_
 
+#include <atomic>
+#include <mutex>
+
 #include <rapidjson/document.h>
 #include <rapidjson/writer.h>
 #include <rapidjson/stringbuffer.h>
@@ -58,7 +61,19 @@ class ClientHandler
   std::map<std::string, std::function<void(std::shared_ptr<const rclcpp::SerializedMessage>)>>
     publisher_cb_;
   std::map<std::string, std::shared_ptr<rws::GenericClient>> clients_;
+  std::map<std::string, std::function<void()>> advertised_services_;
+  std::map<std::string, std::shared_ptr<rws::GenericService>> service_servers_;
+  std::map<std::string, std::string> advertised_service_type_;
   std::map<std::string, std::shared_ptr<rws::GenericActionClient>> action_clients_;
+  struct PendingAdvertisedServiceRequest
+  {
+    std::string service;
+    std::string service_type;
+    std::shared_ptr<rmw_request_id_t> request_header;
+  };
+  std::mutex advertised_service_mutex_;
+  std::map<std::string, PendingAdvertisedServiceRequest> pending_advertised_service_requests_;
+  std::atomic<uint64_t> advertised_service_sequence_{0};
 
   rclcpp::Logger get_logger()
   {
@@ -77,6 +92,9 @@ class ClientHandler
   bool unadvertise_topic_rapid(const rapidjson::Document & msg, rapidjson::StringBuffer & buf, RapidWriter & w);
   bool publish_to_topic_rapid(const rapidjson::Document & msg, rapidjson::StringBuffer & buf, RapidWriter & w);
   bool call_service_rapid(const rapidjson::Document & msg, rapidjson::StringBuffer & buf, RapidWriter & w);
+  bool advertise_service_rapid(const rapidjson::Document & msg, rapidjson::StringBuffer & buf, RapidWriter & w);
+  bool unadvertise_service_rapid(const rapidjson::Document & msg, rapidjson::StringBuffer & buf, RapidWriter & w);
+  bool service_response_rapid(const rapidjson::Document & msg, rapidjson::StringBuffer & buf, RapidWriter & w);
   bool send_action_goal_rapid(const rapidjson::Document & msg, rapidjson::StringBuffer & buf, RapidWriter & w);
   bool cancel_action_goal_rapid(const rapidjson::Document & msg, rapidjson::StringBuffer & buf, RapidWriter & w);
 
@@ -87,4 +105,4 @@ class ClientHandler
 
 }  // namespace rws
 
-#endif  // RWS__NODE_HPP_
+#endif  // RWS__NODE_HPP_

include/rws/generic_service.hpp

@@ -0,0 +1,173 @@
+// Copyright 2026 Innate
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+
+#ifndef RWS__GENERIC_SERVICE_HPP_
+#define RWS__GENERIC_SERVICE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rcl/service.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+#include "rmw/rmw.h"
+#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp"
+#include "rws/typesupport_helpers.hpp"
+
+namespace rws
+{
+
+using rosidl_typesupport_introspection_cpp::ServiceMembers;
+
+class GenericService : public rclcpp::ServiceBase
+{
+public:
+  using SharedRequest = std::shared_ptr<rclcpp::SerializedMessage>;
+  using SharedResponse = std::shared_ptr<rclcpp::SerializedMessage>;
+  using CallbackType = std::function<void(std::shared_ptr<rmw_request_id_t>, SharedRequest)>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericService)
+
+  GenericService(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const std::string & service_name,
+    const std::string & service_type,
+    rcl_service_options_t & service_options,
+    CallbackType callback)
+  : rclcpp::ServiceBase(node_base->get_shared_rcl_node_handle()),
+    service_type_(service_type),
+    callback_(callback)
+  {
+    srv_ts_lib_ = rclcpp::get_typesupport_library(service_type, rws::ts_identifier_srv);
+    srv_ts_hdl_ =
+      rws::get_service_typesupport_handle(service_type, rws::ts_identifier_srv, *srv_ts_lib_);
+
+    srv_intro_lib_ = rclcpp::get_typesupport_library(service_type, rws::ts_identifier);
+    srv_intro_hdl_ =
+      rws::get_service_typesupport_handle(service_type, rws::ts_identifier, *srv_intro_lib_);
+    auto srv_members = static_cast<const ServiceMembers *>(srv_intro_hdl_->data);
+
+    auto request_type = get_type_from_message_members(srv_members->request_members_);
+    req_ts_srv_lib_ = rclcpp::get_typesupport_library(request_type, rws::ts_identifier_srv);
+    req_ts_srv_hdl_ =
+      rclcpp::get_typesupport_handle(request_type, rws::ts_identifier_srv, *req_ts_srv_lib_);
+
+    auto response_type = get_type_from_message_members(srv_members->response_members_);
+    res_ts_srv_lib_ = rclcpp::get_typesupport_library(response_type, rws::ts_identifier_srv);
+    res_ts_srv_hdl_ =
+      rclcpp::get_typesupport_handle(response_type, rws::ts_identifier_srv, *res_ts_srv_lib_);
+
+    service_handle_ = std::shared_ptr<rcl_service_t>(
+      new rcl_service_t, [handle = node_handle_](rcl_service_t * service)
+      {
+        if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+            "Error in destruction of rcl service handle: %s",
+            rcl_get_error_string().str);
+          rcl_reset_error();
+        }
+        delete service;
+      });
+    *service_handle_.get() = rcl_get_zero_initialized_service();
+
+    rcl_ret_t ret = rcl_service_init(
+      service_handle_.get(),
+      node_handle_.get(),
+      srv_ts_hdl_,
+      service_name.c_str(),
+      &service_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+        rcl_reset_error();
+        rclcpp::expand_topic_or_service_name(
+          service_name,
+          rcl_node_get_name(node_handle_.get()),
+          rcl_node_get_namespace(node_handle_.get()),
+          true);
+      }
+
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create generic service");
+    }
+  }
+
+  std::shared_ptr<void> create_request() override
+  {
+    auto srv_members = static_cast<const ServiceMembers *>(srv_intro_hdl_->data);
+    return allocate_message(srv_members->request_members_);
+  }
+
+  std::shared_ptr<rmw_request_id_t> create_request_header() override
+  {
+    return std::make_shared<rmw_request_id_t>();
+  }
+
+  void handle_request(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> request) override
+  {
+    auto serialized_request = std::make_shared<rclcpp::SerializedMessage>();
+    rmw_ret_t ret =
+      rmw_serialize(request.get(), req_ts_srv_hdl_, &serialized_request->get_rcl_serialized_message());
+    if (ret != RMW_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rws", "Failed to serialize advertised service request: %s",
+        rcutils_get_error_string().str);
+      rcutils_reset_error();
+      return;
+    }
+
+    callback_(request_header, serialized_request);
+  }
+
+  void send_serialized_response(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    SharedResponse serialized_response)
+  {
+    auto srv_members = static_cast<const ServiceMembers *>(srv_intro_hdl_->data);
+    auto response = allocate_message(srv_members->response_members_);
+    const rmw_serialized_message_t * sm = &serialized_response->get_rcl_serialized_message();
+    rmw_ret_t rmw_ret = rmw_deserialize(sm, res_ts_srv_hdl_, response.get());
+    if (rmw_ret != RMW_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(rmw_ret, "failed to deserialize service response");
+    }
+
+    rcl_ret_t ret = rcl_send_response(get_service_handle().get(), request_header.get(), response.get());
+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        node_logger_.get_child("rclcpp"),
+        "failed to send response to %s (timeout): %s",
+        this->get_service_name(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send service response");
+    }
+  }
+
+  const std::string & service_type() const { return service_type_; }
+
+private:
+  RCLCPP_DISABLE_COPY(GenericService)
+
+  std::string service_type_;
+  CallbackType callback_;
+  std::shared_ptr<rcpputils::SharedLibrary> srv_ts_lib_;
+  const rosidl_service_type_support_t * srv_ts_hdl_;
+  std::shared_ptr<rcpputils::SharedLibrary> srv_intro_lib_;
+  const rosidl_service_type_support_t * srv_intro_hdl_;
+  std::shared_ptr<rcpputils::SharedLibrary> req_ts_srv_lib_;
+  const rosidl_message_type_support_t * req_ts_srv_hdl_;
+  std::shared_ptr<rcpputils::SharedLibrary> res_ts_srv_lib_;
+  const rosidl_message_type_support_t * res_ts_srv_hdl_;
+};
+
+}  // namespace rws
+
+#endif  // RWS__GENERIC_SERVICE_HPP_

include/rws/node_interface.hpp

@@ -4,6 +4,7 @@
 #include "rclcpp/generic_publisher.hpp"
 #include "rclcpp/generic_subscription.hpp"
 #include "rws/generic_client.hpp"
+#include "rws/generic_service.hpp"
 #include "rws/generic_action_client.hpp"
 
 namespace rws
@@ -56,6 +57,12 @@ class NodeInterface
     const std::string & service_name, const std::string & service_type,
     const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group) = 0;
 
+  /// Create a generic service server with a given type.
+  virtual GenericService::SharedPtr create_generic_service(
+    const std::string & service_name, const std::string & service_type,
+    const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group,
+    GenericService::CallbackType callback) = 0;
+
   /// Create a generic action client with a given type.
   virtual GenericActionClient::SharedPtr create_generic_action_client(
     const std::string & action_name, const std::string & action_type,

include/rws/node_interface_impl.hpp

@@ -6,6 +6,7 @@
 #include "rws/node_interface.hpp"
 #include "rws/typesupport_helpers.hpp"
 #include "rws/generic_action_client.hpp"
+#include "rws/generic_service.hpp"
 
 namespace rws
 {
@@ -52,6 +53,22 @@ class NodeInterfaceImpl : public rws::NodeInterface<>
     return cli;
   }
 
+  GenericService::SharedPtr create_generic_service(
+    const std::string & service_name, const std::string & service_type,
+    const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group,
+    GenericService::CallbackType callback)
+  {
+    rcl_service_options_t options = rcl_service_get_default_options();
+    options.qos = qos_profile;
+
+    auto srv = GenericService::make_shared(
+      node_->get_node_base_interface(), service_name, service_type, options, callback);
+
+    auto srv_base_ptr = std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv);
+    node_->get_node_services_interface()->add_service(srv_base_ptr, group);
+    return srv;
+  }
+
   GenericActionClient::SharedPtr create_generic_action_client(
     const std::string & action_name, const std::string & action_type,
     const rcl_action_client_options_t & options = rcl_action_client_get_default_options())
@@ -120,4 +137,4 @@ class NodeInterfaceImpl : public rws::NodeInterface<>
 
 }  // namespace rws
 
-#endif  // RWS__NODE_INTERFACE_IMPL_HPP_
+#endif  // RWS__NODE_INTERFACE_IMPL_HPP_