irobot-ros
diff --git a/‎rclcpp/CMakeLists.txt‎
Lines changed: 10 additions & 0 deletions b/‎rclcpp/CMakeLists.txt‎
Lines changed: 10 additions & 0 deletions
diff --git a/‎rclcpp/include/rclcpp/client.hpp‎
Lines changed: 135 additions & 6 deletions b/‎rclcpp/include/rclcpp/client.hpp‎
Lines changed: 135 additions & 6 deletions
diff --git a/‎rclcpp/include/rclcpp/create_client.hpp‎
Lines changed: 8 additions & 4 deletions b/‎rclcpp/include/rclcpp/create_client.hpp‎
Lines changed: 8 additions & 4 deletions
diff --git a/‎rclcpp/include/rclcpp/create_service.hpp‎
Lines changed: 7 additions & 4 deletions b/‎rclcpp/include/rclcpp/create_service.hpp‎
Lines changed: 7 additions & 4 deletions
diff --git a/‎rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp‎
Lines changed: 3 additions & 3 deletions b/‎rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp‎
Lines changed: 3 additions & 3 deletions
@@ -39,9 +39,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 set(${PROJECT_NAME}_SRCS
+  src/rclcpp/action_client_intra_process_base.cpp
+  src/rclcpp/action_server_intra_process_base.cpp
   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
@@ -116,6 +119,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
@@ -285,8 +289,14 @@ ament_export_dependencies(
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  file(GLOB_RECURSE AMENT_LINT_AUTO_FILE_EXCLUDE
+    include/rclcpp/experimental/executors/events_executor/concurrent_queue/*.h)
   ament_lint_auto_find_test_dependencies()
 
+  message(AUTHOR_WARNING
+  "Ament lint auto tests are disabled on the following: "
+  ${AMENT_LINT_AUTO_FILE_EXCLUDE}
+)
   add_subdirectory(test)
 endif()
 
 
@@ -37,9 +37,13 @@
 
 #include "rclcpp/clock.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
+#include "rclcpp/detail/resolve_use_intra_process.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"
@@ -48,6 +52,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"
@@ -277,6 +283,15 @@ class ClientBase
   rclcpp::QoS
   get_response_subscription_actual_qos() const;
 
+  /// 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();
+
   /// 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
@@ -381,6 +396,19 @@ class ClientBase
   void
   set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data);
 
+  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);
+
+  std::shared_ptr<rclcpp::experimental::ClientIntraProcessBase> client_intra_process_;
+  std::atomic_uint ipc_sequence_number_{1};
+
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rclcpp::Context> context_;
@@ -396,6 +424,11 @@ class ClientBase
   std::shared_ptr<rcl_client_t> client_handle_;
 
   std::atomic<bool> in_use_by_wait_set_{false};
+
+  std::recursive_mutex ipc_mutex_;
+  bool use_intra_process_{false};
+  IntraProcessManagerWeakPtr weak_ipm_;
+  uint64_t intra_process_client_id_;
 };
 
 template<typename ServiceT>
@@ -491,12 +524,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 = rclcpp::IntraProcessSetting::NodeDefault)
   : ClientBase(node_base, node_graph),
     srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
@@ -519,10 +554,27 @@ class Client : public ClientBase
       }
       rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
     }
+
+    // Setup intra process if requested.
+    if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) {
+      create_intra_process_client();
+    }
   }
 
   virtual ~Client()
   {
+    if (!use_intra_process_) {
+      return;
+    }
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      // TODO(ivanpauno): should this raise an error?
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Intra process manager died before than a client.");
+      return;
+    }
+    ipm->remove_client(intra_process_client_id_);
   }
 
   /// Take the next response for this client.
@@ -639,7 +691,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);
   }
@@ -674,7 +726,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,
@@ -705,7 +757,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,
@@ -839,11 +891,35 @@ class Client : public ClientBase
     CallbackWithRequestTypeValueVariant>;
 
   int64_t
-  async_send_request_impl(const Request & request, CallbackInfoVariant value)
+  async_send_request_impl(SharedRequest request, CallbackInfoVariant value)
   {
+    {
+      std::lock_guard<std::recursive_mutex> lock(ipc_mutex_);
+      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->template 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;
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-    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");
     }
@@ -869,6 +945,59 @@ class Client : public ClientBase
     return value;
   }
 
+  void
+  create_intra_process_client()
+  {
+    // 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.
+    using ClientIntraProcessT = rclcpp::experimental::ClientIntraProcess<ServiceT>;
+
+    // Expand the given service name.
+    char * remapped_service_name = NULL;
+    rcl_allocator_t allocator = rcl_get_default_allocator();
+
+    rcl_ret_t ret = rcl_node_resolve_name(
+      this->get_rcl_node_handle(),
+      this->get_service_name(),
+      allocator,
+      true,
+      false,
+      &remapped_service_name);
+
+    if (RCL_RET_OK != ret) {
+      allocator.deallocate(remapped_service_name, allocator.state);
+      rclcpp::exceptions::throw_from_rcl_error(ret, "client failed to resolve service name");
+    }
+
+    client_intra_process_ = std::make_shared<ClientIntraProcessT>(
+      context_,
+      remapped_service_name,
+      qos_profile);
+
+    allocator.deallocate(remapped_service_name, allocator.state);
+
+    // 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);
+  }
+
   RCLCPP_DISABLE_COPY(Client)
 
   std::unordered_map<
 
@@ -46,13 +46,15 @@ create_client(
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
   const std::string & service_name,
   const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
-  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  rclcpp::CallbackGroup::SharedPtr group = nullptr,
+  rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault)
 {
   return create_client<ServiceT>(
     node_base, node_graph, node_services,
     service_name,
     qos.get_rmw_qos_profile(),
-    group);
+    group,
+    ipc_setting);
 }
 
 /// Create a service client with a given type.
@@ -65,7 +67,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 = rclcpp::IntraProcessSetting::NodeDefault)
 {
   rcl_client_options_t options = rcl_client_get_default_options();
   options.qos = qos_profile;
@@ -74,7 +77,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);
 
@@ -46,11 +46,12 @@ create_service(
   const std::string & service_name,
   CallbackT && callback,
   const rclcpp::QoS & qos,
-  rclcpp::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group,
+  rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault)
 {
   return create_service<ServiceT, CallbackT>(
     node_base, node_services, service_name,
-    std::forward<CallbackT>(callback), qos.get_rmw_qos_profile(), group);
+    std::forward<CallbackT>(callback), qos.get_rmw_qos_profile(), group, ipc_setting);
 }
 
 /// Create a service with a given type.
@@ -63,7 +64,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::IntraProcessSetting::NodeDefault)
 {
   rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
   any_service_callback.set(std::forward<CallbackT>(callback));
@@ -72,8 +74,9 @@ create_service(
   service_options.qos = qos_profile;
 
   auto serv = Service<ServiceT>::make_shared(
-    node_base->get_shared_rcl_node_handle(),
+    node_base,
     service_name, any_service_callback, service_options);
+  serv->post_init_setup(node_base, ipc_setting);
   auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
   node_services->add_service(serv_base_ptr, group);
   return serv;
 
@@ -26,12 +26,12 @@ namespace detail
 {
 
 /// Return whether or not intra process is enabled, resolving "NodeDefault" if needed.
-template<typename OptionsT, typename NodeBaseT>
+template<typename NodeBaseT>
 bool
-resolve_use_intra_process(const OptionsT & options, const NodeBaseT & node_base)
+resolve_use_intra_process(IntraProcessSetting ipc_setting, const NodeBaseT & node_base)
 {
   bool use_intra_process;
-  switch (options.use_intra_process_comm) {
+  switch (ipc_setting) {
     case IntraProcessSetting::Enable:
       use_intra_process = true;
       break;