Design changes that move most transient local publish functionalities out of

intra process manager into intra process manager

Signed-off-by: Jeffery Hsu <[email protected]>

Author: jefferyyjhsu

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -34,6 +34,7 @@
 #include "rclcpp/experimental/action_server_intra_process_base.hpp"
 #include "rclcpp/experimental/client_intra_process.hpp"
 #include "rclcpp/experimental/client_intra_process_base.hpp"
+#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
 #include "rclcpp/experimental/service_intra_process.hpp"
 #include "rclcpp/experimental/service_intra_process_base.hpp"
@@ -120,9 +121,76 @@ class IntraProcessManager
    * \param subscription the SubscriptionIntraProcess to register.
    * \return an unsigned 64-bit integer which is the subscription's unique id.
    */
+  template<
+    typename ROSMessageType,
+    typename Alloc = std::allocator<ROSMessageType>
+  >
   RCLCPP_PUBLIC
   uint64_t
-  add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription);
+  add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription)
+  {
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+    std::unique_lock<std::shared_timed_mutex> lock(mutex_);
+
+    uint64_t sub_id = IntraProcessManager::get_next_unique_id();
+
+    subscriptions_[sub_id] = subscription;
+
+    // adds the subscription id to all the matchable publishers
+    for (auto & pair : publishers_) {
+      auto publisher = pair.second.lock();
+      if (!publisher) {
+        continue;
+      }
+      if (can_communicate(publisher, subscription)) {
+        uint64_t pub_id = pair.first;
+        insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
+        if (publisher->is_durability_transient_local() &&
+          subscription->is_durability_transient_local())
+        {
+          auto publisher_buffer = publisher_buffers_[pub_id].lock();
+          if (!publisher_buffer) {
+            throw std::runtime_error("publisher buffer has unexpectedly gone out of scope");
+          }
+          auto buffer = std::dynamic_pointer_cast<
+            rclcpp::experimental::buffers::IntraProcessBuffer<
+              ROSMessageType,
+              ROSMessageTypeAllocator,
+              ROSMessageTypeDeleter
+            >
+            >(publisher_buffer);
+          if (!buffer) {
+            throw std::runtime_error(
+                    "failed to dynamic cast publisher's IntraProcessBufferBase to "
+                    "IntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+                    "ROSMessageTypeDeleter> which can happen when the publisher and "
+                    "subscription use different allocator types, which is not supported");
+          }
+          if (subscription->use_take_shared_method()) {
+            auto data_vec = buffer->get_all_data_shared();
+            for (auto shared_data : data_vec) {
+              this->template add_shared_msg_to_buffer<
+                ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>(
+                shared_data, sub_id);
+            }
+          } else {
+            auto data_vec = buffer->get_all_data_unique();
+            for (auto & owned_data : data_vec) {
+              auto allocator = ROSMessageTypeAllocator();
+              this->template add_owned_msg_to_buffer<
+                ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>(
+                std::move(owned_data), sub_id, allocator);
+            }
+          }
+        }
+      }
+    }
+
+    return sub_id;
+  }
 
   /// Register an intra-process client with the manager, returns the client unique id.
   /**
@@ -218,7 +286,10 @@ class IntraProcessManager
    */
   RCLCPP_PUBLIC
   uint64_t
-  add_publisher(rclcpp::PublisherBase::SharedPtr publisher);
+  add_publisher(
+    rclcpp::PublisherBase::SharedPtr publisher,
+    rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer =
+    rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr());
 
   /// Unregister a publisher using the publisher's unique id.
   /**
@@ -789,6 +860,9 @@ class IntraProcessManager
   using PublisherMap =
     std::unordered_map<uint64_t, rclcpp::PublisherBase::WeakPtr>;
 
+  using PublisherBufferMap =
+    std::unordered_map<uint64_t, rclcpp::experimental::buffers::IntraProcessBufferBase::WeakPtr>;
+
   using PublisherToSubscriptionIdsMap =
     std::unordered_map<uint64_t, SplittedSubscriptions>;
 
@@ -1023,6 +1097,7 @@ class IntraProcessManager
   ActionClientToServerIdsMap action_clients_to_servers_;
 
   std::unordered_map<size_t, uint64_t> clients_uuid_to_id_;
+  PublisherBufferMap publisher_buffers_;
 
   mutable std::shared_timed_mutex mutex_;
 };

rclcpp/include/rclcpp/publisher.hpp

@@ -112,11 +112,11 @@ class Publisher : public PublisherBase
   [[deprecated("use std::shared_ptr<const PublishedType>")]] =
     std::shared_ptr<const PublishedType>;
 
-  using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
+  using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
     ROSMessageType,
     ROSMessageTypeAllocator,
     ROSMessageTypeDeleter
-    >::UniquePtr;
+    >::SharedPtr;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, AllocatorT>)
 
@@ -211,7 +211,7 @@ class Publisher : public PublisherBase
           qos,
           std::make_shared<ROSMessageTypeAllocator>(ros_message_type_allocator_));
       }
-      uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());
+      uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_);
       this->setup_intra_process(
         intra_process_publisher_id,
         ipm);
@@ -470,53 +470,6 @@ class Publisher : public PublisherBase
     return ros_message_type_allocator_;
   }
 
-  /// Publish shared messages from intra process buffer for late joiner
-  /**
-   * This signature allows the user to give a sub_id from intra process manager.
-   * The function will publish messages currently held in buffer_ to the subscription
-   * as shared pointers.
-   *
-   * \param[in] sud_id subscription id in ipm to publish data to
-   */
-  void do_shared_intra_process_publish_for_late_joiner(const uint64_t sub_id) override
-  {
-    auto ipm = weak_ipm_.lock();
-    if (!buffer_ || !ipm) {
-      throw std::runtime_error(
-              "intra process publish for late joiner called "
-              "after destruction of intra process manager and/or transient_local buffer");
-    }
-    auto data_vec = buffer_->get_all_data_shared();
-    for (auto shared_data : data_vec) {
-      ipm->template add_shared_msg_to_buffer<
-        ROSMessageType, AllocatorT, ROSMessageTypeDeleter, ROSMessageType>(shared_data, sub_id);
-    }
-  }
-
-  /// Publish owned messages from intra process buffer for late joiner
-  /**
-   * This signature allows the user to give a sub_id from intra process manager.
-   * The function will publish messages currently held in buffer_ to the subscription
-   * as unique pointers.
-   *
-   * \param[in] sud_id subscription id in ipm to publish data to
-   */
-  void do_unique_intra_process_publish_for_late_joiner(const uint64_t sub_id) override
-  {
-    auto ipm = weak_ipm_.lock();
-    if (!buffer_ || !ipm) {
-      throw std::runtime_error(
-              "intra process publish for late joiner called "
-              "after destruction of intra process manager and/or transient_local buffer");
-    }
-    auto data_vec = buffer_->get_all_data_unique();
-    for (auto & owned_data : data_vec) {
-      ipm->template add_owned_msg_to_buffer<
-        ROSMessageType, AllocatorT, ROSMessageTypeDeleter, ROSMessageType>(
-        std::move(owned_data), sub_id, ros_message_type_allocator_);
-    }
-  }
-
 protected:
   void
   do_inter_process_publish(const ROSMessageType & msg)
@@ -669,7 +622,7 @@ class Publisher : public PublisherBase
   ROSMessageTypeAllocator ros_message_type_allocator_;
   ROSMessageTypeDeleter ros_message_type_deleter_;
 
-  BufferUniquePtr buffer_;
+  BufferSharedPtr buffer_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/publisher_base.hpp

@@ -319,27 +319,6 @@ class PublisherBase : public std::enable_shared_from_this<PublisherBase>
     event_handlers_[event_type]->clear_on_ready_callback();
   }
 
-
-  /// Publish shared messages from intra process buffer for late joiner
-  /**
-   * This signature allows the user to give a sub_id from intra process manager.
-   * The function will publish messages currently held in buffer_ to the subscription
-   * as shared pointers.
-   *
-   * \param[in] sud_id subscription id in ipm to publish data to
-   */
-  virtual void do_shared_intra_process_publish_for_late_joiner(const uint64_t sub_id);
-
-  /// Publish owned messages from intra process buffer for late joiner
-  /**
-   * This signature allows the user to give a sub_id from intra process manager.
-   * The function will publish messages currently held in buffer_ to the subscription
-   * as unique pointers.
-   *
-   * \param[in] sud_id subscription id in ipm to publish data to
-   */
-  virtual void do_unique_intra_process_publish_for_late_joiner(const uint64_t sub_id);
-
 protected:
   template<typename EventCallbackT>
   void

rclcpp/include/rclcpp/subscription.hpp

@@ -218,7 +218,8 @@ class Subscription : public SubscriptionBase
       // Add it to the intra process manager.
       using rclcpp::experimental::IntraProcessManager;
       auto ipm = context->get_sub_context<IntraProcessManager>();
-      uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process_);
+      uint64_t intra_process_subscription_id = ipm->template add_subscription<
+        ROSMessageType, ROSMessageTypeAllocator>(subscription_intra_process_);
       this->setup_intra_process(intra_process_subscription_id, ipm);
     }
 

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -26,13 +26,24 @@ namespace experimental
 static std::atomic<uint64_t> _next_unique_id {1};
 
 uint64_t
-IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
+IntraProcessManager::add_publisher(
+  rclcpp::PublisherBase::SharedPtr publisher,
+  rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer)
 {
   std::unique_lock<std::shared_timed_mutex> lock(mutex_);
 
   uint64_t pub_id = IntraProcessManager::get_next_unique_id();
 
   publishers_[pub_id] = publisher;
+  if (publisher->is_durability_transient_local()) {
+    if (buffer) {
+      publisher_buffers_[pub_id] = buffer;
+    } else {
+      throw std::runtime_error(
+              "transient_local publisher needs to pass"
+              "a valid publisher buffer ptr when calling add_publisher()");
+    }
+  }
 
   // Initialize the subscriptions storage for this publisher.
   pub_to_subs_[pub_id] = SplittedSubscriptions();
@@ -52,39 +63,6 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
   return pub_id;
 }
 
-uint64_t
-IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription)
-{
-  std::unique_lock<std::shared_timed_mutex> lock(mutex_);
-
-  uint64_t sub_id = IntraProcessManager::get_next_unique_id();
-
-  subscriptions_[sub_id] = subscription;
-
-  // adds the subscription id to all the matchable publishers
-  for (auto & pair : publishers_) {
-    auto publisher = pair.second.lock();
-    if (!publisher) {
-      continue;
-    }
-    if (can_communicate(publisher, subscription)) {
-      uint64_t pub_id = pair.first;
-      insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
-      if (publisher->is_durability_transient_local() &&
-        subscription->is_durability_transient_local())
-      {
-        if (subscription->use_take_shared_method()) {
-          publisher->do_shared_intra_process_publish_for_late_joiner(sub_id);
-        } else {
-          publisher->do_unique_intra_process_publish_for_late_joiner(sub_id);
-        }
-      }
-    }
-  }
-
-  return sub_id;
-}
-
 uint64_t
 IntraProcessManager::add_intra_process_client(ClientIntraProcessBase::SharedPtr client)
 {

rclcpp/src/rclcpp/publisher_base.cpp

@@ -317,11 +317,3 @@ std::vector<rclcpp::NetworkFlowEndpoint> PublisherBase::get_network_flow_endpoin
   return network_flow_endpoint_vector;
 }
 
-void PublisherBase::do_shared_intra_process_publish_for_late_joiner(const uint64_t)
-{
-  throw std::runtime_error("intra process publish for late joiner is not implemented");
-}
-void PublisherBase::do_unique_intra_process_publish_for_late_joiner(const uint64_t)
-{
-  throw std::runtime_error("intra process publish for late joiner is not implemented");
-}