Allow for deferred responses with ipc (#135)

* allow for deferred responses with ipc

* fix send response

* fix use member service ipc process

* add map to store CallbackInfoVariant

* add send_response to base class, set function ptr to get handle

* move service intra process outside base

* copy response into shared pointer for ipc

* add typename for service template

* remove ref signature

* try without std::ref

* try without ref wrapper

* try emplace

* make pair with variant

* some cleanup

* erase callback info if client invalid

* add post_init_setup for services

* fix extra comma

* add post init setup after lifecycle node services

* add documentation for ServiceIntraProcess template change

* use weak ptr to service in ServiceIntraProcess

* move check for valid service handle to beginning of function

* add comment for callback info map

(cherry picked from commit aa95a48)

Author: bpwilcox

rclcpp/include/rclcpp/create_service.hpp

@@ -75,7 +75,8 @@ create_service(
 
   auto serv = Service<ServiceT>::make_shared(
     node_base,
-    service_name, any_service_callback, service_options, ipc_setting);
+    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;

rclcpp/include/rclcpp/experimental/service_intra_process.hpp

@@ -70,11 +70,12 @@ class ServiceIntraProcess : public ServiceIntraProcessBase
   using ClientIDtoRequest = std::pair<uint64_t, RequestCallbackPair>;
 
   ServiceIntraProcess(
+    std::weak_ptr<rclcpp::Service<ServiceT>> service_handle,
     AnyServiceCallback<ServiceT> callback,
     rclcpp::Context::SharedPtr context,
     const std::string & service_name,
     const rclcpp::QoS & qos_profile)
-  : ServiceIntraProcessBase(context, service_name, qos_profile), any_callback_(callback)
+  : ServiceIntraProcessBase(context, service_name, qos_profile), any_callback_(callback), service_handle_(service_handle)
   {
     // Create the intra-process buffer.
     buffer_ = rclcpp::experimental::create_service_intra_process_buffer<
@@ -107,40 +108,63 @@ class ServiceIntraProcess : public ServiceIntraProcessBase
     return std::static_pointer_cast<void>(data);
   }
 
+  void send_response(uint64_t intra_process_client_id, SharedResponse & response)
+  {
+    std::unique_lock<std::recursive_mutex> lock(reentrant_mutex_);
+
+    auto client_it = clients_.find(intra_process_client_id);
+
+    if (client_it == clients_.end()) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling intra_process_service_send_response for invalid or no "
+        "longer existing client id");
+
+      callback_info_.erase(intra_process_client_id);
+      return;
+    }
+
+    auto client_intra_process_base = client_it->second.lock();
+
+    if (client_intra_process_base) {
+      auto client = std::dynamic_pointer_cast<
+        rclcpp::experimental::ClientIntraProcess<ServiceT>>(
+        client_intra_process_base);
+      CallbackInfoVariant & value = callback_info_[intra_process_client_id];
+      client->store_intra_process_response(
+        std::make_pair(std::move(response), std::move(value)));
+    } else {
+      clients_.erase(client_it);
+    }
+
+    callback_info_.erase(intra_process_client_id);
+  }
+
   void execute(const std::shared_ptr<void> & data)
   {
+    auto serv_handle = service_handle_.lock();
+
+    // Return if the service handle is no longer valid
+    if (!serv_handle) {
+      return;
+    }
+
     auto ptr = std::static_pointer_cast<ClientIDtoRequest>(data);
 
     uint64_t intra_process_client_id = ptr->first;
     SharedRequest & typed_request = ptr->second.first;
     CallbackInfoVariant & value = ptr->second.second;
+    callback_info_.emplace(std::make_pair(intra_process_client_id, std::move(value)));
+
+    // To allow for the user callback to handle deferred responses for IPC in an ambiguous way,
+    // we are overloading the rmw_request_id semantics to provide the intra process client ID.
+    auto req_id = std::make_shared<rmw_request_id_t>();
+    req_id->sequence_number = intra_process_client_id;
 
-    SharedResponse response = any_callback_.dispatch(nullptr, nullptr, std::move(typed_request));
+    SharedResponse response = any_callback_.dispatch(serv_handle, req_id, std::move(typed_request));
 
     if (response) {
-      std::unique_lock<std::recursive_mutex> lock(reentrant_mutex_);
-
-      auto client_it = clients_.find(intra_process_client_id);
-
-      if (client_it == clients_.end()) {
-        RCLCPP_WARN(
-          rclcpp::get_logger("rclcpp"),
-          "Calling intra_process_service_send_response for invalid or no "
-          "longer existing client id");
-        return;
-      }
-
-      auto client_intra_process_base = client_it->second.lock();
-
-      if (client_intra_process_base) {
-        auto client = std::dynamic_pointer_cast<
-          rclcpp::experimental::ClientIntraProcess<ServiceT>>(
-          client_intra_process_base);
-        client->store_intra_process_response(
-          std::make_pair(std::move(response), std::move(value)));
-      } else {
-        clients_.erase(client_it);
-      }
+      send_response(intra_process_client_id, response);
     }
   }
 
@@ -152,6 +176,12 @@ class ServiceIntraProcess : public ServiceIntraProcessBase
   BufferUniquePtr buffer_;
 
   AnyServiceCallback<ServiceT> any_callback_;
+
+  std::weak_ptr<rclcpp::Service<ServiceT>> service_handle_;
+
+  // Store callback variants in a map to support deferred response
+  // access by intra-process client id.
+  std::unordered_map<uint64_t, CallbackInfoVariant> callback_info_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/service.hpp

@@ -292,8 +292,6 @@ class ServiceBase
     uint64_t intra_process_service_id,
     IntraProcessManagerWeakPtr weak_ipm);
 
-  std::shared_ptr<rclcpp::experimental::ServiceIntraProcessBase> service_intra_process_;
-
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rclcpp::Context> context_;
 
@@ -345,14 +343,12 @@ class Service
    * \param[in] service_name Name of the topic to publish to.
    * \param[in] any_callback User defined callback to call when a client request is received.
    * \param[in] service_options options for the subscription.
-   * \param[in] ipc_setting Intra-process communication setting for the service.
    */
   Service(
     std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base,
     const std::string & service_name,
     AnyServiceCallback<ServiceT> any_callback,
-    rcl_service_options_t & service_options,
-    rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault)
+    rcl_service_options_t & service_options)
   : ServiceBase(node_base), any_callback_(any_callback),
     srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
@@ -398,7 +394,14 @@ class Service
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
+    // Setup continues in the post construction method, post_init_setup().
+  }
 
+  /// Called post construction, so that construction may continue after shared_from_this() works.
+  void post_init_setup(
+    std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base,
+    rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault)
+  {
     // Setup intra process if requested.
     if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) {
       create_intra_process_service();
@@ -414,13 +417,11 @@ class Service
    * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] service_handle service handle.
    * \param[in] any_callback User defined callback to call when a client request is received.
-   * \param[in] ipc_setting Intra-process communication setting for the service.
    */
   Service(
     std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base,
     std::shared_ptr<rcl_service_t> service_handle,
-    AnyServiceCallback<ServiceT> any_callback,
-    rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault)
+    AnyServiceCallback<ServiceT> any_callback)
   : ServiceBase(node_base), any_callback_(any_callback),
     srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
@@ -440,11 +441,7 @@ class Service
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
-
-    // Setup intra process if requested.
-    if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) {
-      create_intra_process_service();
-    }
+    // Setup continues in the post construction method, post_init_setup().
   }
 
   /// Default constructor.
@@ -456,13 +453,11 @@ class Service
    * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] service_handle service handle.
    * \param[in] any_callback User defined callback to call when a client request is received.
-   * \param[in] ipc_setting Intra-process communication setting for the service.
    */
   Service(
     std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base,
     rcl_service_t * service_handle,
-    AnyServiceCallback<ServiceT> any_callback,
-    rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault)
+    AnyServiceCallback<ServiceT> any_callback)
   : ServiceBase(node_base), any_callback_(any_callback),
     srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
@@ -484,10 +479,7 @@ class Service
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
-    // Setup intra process if requested.
-    if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) {
-      create_intra_process_service();
-    }
+    // Setup continues in the post construction method, post_init_setup().
   }
 
   Service() = delete;
@@ -553,6 +545,14 @@ class Service
   void
   send_response(rmw_request_id_t & req_id, typename ServiceT::Response & response)
   {
+    if (use_intra_process_)
+    {
+      auto intra_response = std::make_shared<typename ServiceT::Response>(response);
+      // The sequence number here is used as a proxy for the intra-process client ID
+      service_intra_process_->send_response(req_id.sequence_number, intra_response);
+      return;
+    }
+
     rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);
 
     if (ret == RCL_RET_TIMEOUT) {
@@ -591,6 +591,7 @@ class Service
     using ServiceIntraProcessT = rclcpp::experimental::ServiceIntraProcess<ServiceT>;
 
     service_intra_process_ = std::make_shared<ServiceIntraProcessT>(
+      this->shared_from_this(),
       any_callback_,
       context_,
       this->get_service_name(),
@@ -635,6 +636,12 @@ class Service
   AnyServiceCallback<ServiceT> any_callback_;
 
   const rosidl_service_type_support_t * srv_type_support_handle_;
+
+  // In order to mirror the send_response signature with a SharedResponse
+  // of the appropriate ServiceT type, the template class is stored
+  // as opposed to the base class which has no knowledge of ServiceT.
+  std::shared_ptr<rclcpp::experimental::ServiceIntraProcess<ServiceT>> service_intra_process_;
+
 };
 
 }  // namespace rclcpp

rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp

@@ -128,8 +128,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
       srv_change_state_ = std::make_shared<rclcpp::Service<ChangeStateSrv>>(
         node_base_interface_,
         &state_machine_.com_interface.srv_change_state,
-        any_cb,
-        ipc_setting);
+        any_cb);
+      srv_change_state_->post_init_setup(node_base_interface_, ipc_setting);
       node_services_interface_->add_service(
         std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_change_state_),
         nullptr);
@@ -146,6 +146,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
         node_base_interface_,
         &state_machine_.com_interface.srv_get_state,
         any_cb);
+      srv_get_state_->post_init_setup(node_base_interface_, ipc_setting);
       node_services_interface_->add_service(
         std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_state_),
         nullptr);
@@ -162,6 +163,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
         node_base_interface_,
         &state_machine_.com_interface.srv_get_available_states,
         any_cb);
+      srv_get_available_states_->post_init_setup(node_base_interface_, ipc_setting);
       node_services_interface_->add_service(
         std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_states_),
         nullptr);
@@ -179,6 +181,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
         node_base_interface_,
         &state_machine_.com_interface.srv_get_available_transitions,
         any_cb);
+      srv_get_available_transitions_->post_init_setup(node_base_interface_, ipc_setting);
       node_services_interface_->add_service(
         std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_transitions_),
         nullptr);
@@ -196,6 +199,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf
         node_base_interface_,
         &state_machine_.com_interface.srv_get_transition_graph,
         any_cb);
+      srv_get_transition_graph_->post_init_setup(node_base_interface_, ipc_setting);
       node_services_interface_->add_service(
         std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_transition_graph_),
         nullptr);