Support intra-process communication: Clients & Services

Signed-off-by: Mauro Passerino <[email protected]>

Author: Mauro Passerino

rclcpp/CMakeLists.txt

@@ -38,6 +38,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/any_executable.cpp
   src/rclcpp/callback_group.cpp
   src/rclcpp/client.cpp
+  src/rclcpp/client_intra_process_base.cpp
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
@@ -97,6 +98,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/serialization.cpp
   src/rclcpp/serialized_message.cpp
   src/rclcpp/service.cpp
+  src/rclcpp/service_intra_process_base.cpp
   src/rclcpp/signal_handler.cpp
   src/rclcpp/subscription_base.cpp
   src/rclcpp/subscription_intra_process_base.cpp

rclcpp/include/rclcpp/client.hpp

@@ -36,7 +36,10 @@
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/experimental/client_intra_process.hpp"
+#include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
@@ -45,6 +48,8 @@
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcutils/logging_macros.h"
+
 #include "rmw/error_handling.h"
 #include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
@@ -251,6 +256,25 @@ class ClientBase
   rclcpp::QoS
   get_response_subscription_actual_qos() const;
 
+  using IntraProcessManagerWeakPtr =
+    std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
+
+  /// Implementation detail.
+  RCLCPP_PUBLIC
+  void
+  setup_intra_process(
+    uint64_t intra_process_client_id,
+    IntraProcessManagerWeakPtr weak_ipm);
+
+  /// Return the waitable for intra-process
+  /**
+   * \return the waitable sharedpointer for intra-process, or nullptr if intra-process is not setup.
+   * \throws std::runtime_error if the intra process manager is destroyed
+   */
+  RCLCPP_PUBLIC
+  rclcpp::Waitable::SharedPtr
+  get_intra_process_waitable() const;
+
   /// Set a callback to be called when each new response is received.
   /**
    * The callback receives a size_t which is the number of responses received
@@ -363,6 +387,10 @@ class ClientBase
 
   std::atomic<bool> in_use_by_wait_set_{false};
 
+  bool use_intra_process_{false};
+  IntraProcessManagerWeakPtr weak_ipm_;
+  uint64_t intra_process_client_id_;
+
   std::recursive_mutex callback_mutex_;
   std::function<void(size_t)> on_new_response_callback_{nullptr};
 };
@@ -460,12 +488,14 @@ class Client : public ClientBase
    * \param[in] node_graph The node graph interface of the corresponding node.
    * \param[in] service_name Name of the topic to publish to.
    * \param[in] client_options options for the subscription.
+   * \param[in] ipc_setting Intra-process communication setting for the client.
    */
   Client(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & service_name,
-    rcl_client_options_t & client_options)
+    rcl_client_options_t & client_options,
+    rclcpp::IntraProcessSetting ipc_setting)
   : ClientBase(node_base, node_graph)
   {
     using rosidl_typesupport_cpp::get_service_type_support_handle;
@@ -490,6 +520,37 @@ class Client : public ClientBase
       }
       rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
     }
+
+    // Setup intra process if requested.
+    if (ipc_setting == IntraProcessSetting::Enable) {
+      // Check if the QoS is compatible with intra-process.
+      auto qos_profile = get_response_subscription_actual_qos();
+
+      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
+        throw std::invalid_argument(
+                "intraprocess communication allowed only with keep last history qos policy");
+      }
+      if (qos_profile.depth() == 0) {
+        throw std::invalid_argument(
+                "intraprocess communication is not allowed with 0 depth qos policy");
+      }
+      if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
+        throw std::invalid_argument(
+                "intraprocess communication allowed only with volatile durability");
+      }
+
+      // Create a ClientIntraProcess which will be given to the intra-process manager.
+      client_intra_process_ = std::make_shared<ClientIntraProcessT>(
+        context_,
+        this->get_service_name(),
+        qos_profile);
+
+      // Add it to the intra process manager.
+      using rclcpp::experimental::IntraProcessManager;
+      auto ipm = context_->get_sub_context<IntraProcessManager>();
+      uint64_t intra_process_client_id = ipm->add_intra_process_client(client_intra_process_);
+      this->setup_intra_process(intra_process_client_id, ipm);
+    }
   }
 
   virtual ~Client()
@@ -610,7 +671,7 @@ class Client : public ClientBase
     Promise promise;
     auto future = promise.get_future();
     auto req_id = async_send_request_impl(
-      *request,
+      std::move(request),
       std::move(promise));
     return FutureAndRequestId(std::move(future), req_id);
   }
@@ -645,7 +706,7 @@ class Client : public ClientBase
     Promise promise;
     auto shared_future = promise.get_future().share();
     auto req_id = async_send_request_impl(
-      *request,
+      std::move(request),
       std::make_tuple(
         CallbackType{std::forward<CallbackT>(cb)},
         shared_future,
@@ -676,7 +737,7 @@ class Client : public ClientBase
     PromiseWithRequest promise;
     auto shared_future = promise.get_future().share();
     auto req_id = async_send_request_impl(
-      *request,
+      request,
       std::make_tuple(
         CallbackWithRequestType{std::forward<CallbackT>(cb)},
         request,
@@ -789,10 +850,31 @@ class Client : public ClientBase
     CallbackWithRequestTypeValueVariant>;
 
   int64_t
-  async_send_request_impl(const Request & request, CallbackInfoVariant value)
+  async_send_request_impl(SharedRequest request, CallbackInfoVariant value)
   {
+    if (use_intra_process_) {
+      auto ipm = weak_ipm_.lock();
+      if (!ipm) {
+        throw std::runtime_error(
+                "intra process send called after destruction of intra process manager");
+      }
+      bool intra_process_server_available = ipm->service_is_available(intra_process_client_id_);
+
+      // Check if there's an intra-process server available matching this client.
+      // If there's not, we fall back into inter-process communication, since
+      // the server might be available in another process or was configured to not use IPC.
+      if (intra_process_server_available) {
+        // Send intra-process request
+        ipm->send_intra_process_client_request<ServiceT>(
+          intra_process_client_id_,
+          std::make_pair(std::move(request), std::move(value)));
+        return ipc_sequence_number_++;
+      }
+    }
+
+    // Send inter-process request
     int64_t sequence_number;
-    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
+    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
@@ -830,6 +912,11 @@ class Client : public ClientBase
       CallbackInfoVariant>>
   pending_requests_;
   std::mutex pending_requests_mutex_;
+
+private:
+  using ClientIntraProcessT = rclcpp::experimental::ClientIntraProcess<ServiceT>;
+  std::shared_ptr<ClientIntraProcessT> client_intra_process_;
+  std::atomic_uint ipc_sequence_number_{1};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/create_client.hpp

@@ -35,7 +35,8 @@ create_client(
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
   const std::string & service_name,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group,
+  rclcpp::IntraProcessSetting ipc_setting)
 {
   rcl_client_options_t options = rcl_client_get_default_options();
   options.qos = qos_profile;
@@ -44,7 +45,8 @@ create_client(
     node_base.get(),
     node_graph,
     service_name,
-    options);
+    options,
+    ipc_setting);
 
   auto cli_base_ptr = std::dynamic_pointer_cast<rclcpp::ClientBase>(cli);
   node_services->add_client(cli_base_ptr, group);

rclcpp/include/rclcpp/create_service.hpp

@@ -37,7 +37,8 @@ create_service(
   const std::string & service_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group,
+  rclcpp::IntraProcessSetting ipc_setting)
 {
   rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
   any_service_callback.set(std::forward<CallbackT>(callback));
@@ -46,8 +47,8 @@ create_service(
   service_options.qos = qos_profile;
 
   auto serv = Service<ServiceT>::make_shared(
-    node_base->get_shared_rcl_node_handle(),
-    service_name, any_service_callback, service_options);
+    node_base,
+    service_name, any_service_callback, service_options, ipc_setting);
   auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
   node_services->add_service(serv_base_ptr, group);
   return serv;

rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp

@@ -234,6 +234,54 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer<MessageT, Alloc, Messa
   }
 };
 
+template<typename BufferT>
+class ServiceIntraProcessBuffer : public IntraProcessBufferBase
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(ServiceIntraProcessBuffer)
+
+  virtual ~ServiceIntraProcessBuffer() {}
+
+  explicit
+  ServiceIntraProcessBuffer(
+    std::unique_ptr<BufferImplementationBase<BufferT>> buffer_impl)
+  {
+    buffer_ = std::move(buffer_impl);
+  }
+
+  // Not to be used in this class. Todo: review base class to avoid this.
+  bool use_take_shared_method() const override
+  {
+    throw std::runtime_error(
+            "use_take_shared_method not intended to be used in this class");
+    return false;
+  }
+
+  bool has_data() const override
+  {
+    return buffer_->has_data();
+  }
+
+  void clear() override
+  {
+    buffer_->clear();
+  }
+
+  void add(BufferT && msg)
+  {
+    buffer_->enqueue(std::move(msg));
+  }
+
+  BufferT
+  consume()
+  {
+    return buffer_->dequeue();
+  }
+
+private:
+  std::unique_ptr<BufferImplementationBase<BufferT>> buffer_;
+};
+
 }  // namespace buffers
 }  // namespace experimental
 }  // namespace rclcpp