Ignore local endpoints (ros2#131)

* Refs #18846: PoC ignore local endpoints: extend RCLCPP API Signed-off-by: JLBuenoLopez-eProsima <joseluisbueno@eprosima.com> * Refs #18846: PoC ignore local endpoints: modify RCLCPP publish logic Signed-off-by: JLBuenoLopez-eProsima <joseluisbueno@eprosima.com> --------- Signed-off-by: JLBuenoLopez-eProsima <joseluisbueno@eprosima.com> Co-authored-by: JLBuenoLopez-eProsima <joseluisbueno@eprosima.com>
mauropasse · Nov 24, 2023 · 8c423ce · 8c423ce
1 parent cf5257c
commit 8c423ce
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 2 deletions.
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
@@ -273,7 +273,7 @@ class Publisher : public PublisherBase
     // It's not possible to do that with an unique_ptr,
     // as do_intra_process_publish takes the ownership of the message.
     bool inter_process_publish_needed =
-      get_subscription_count() > get_intra_process_subscription_count();
+      get_non_local_subscription_count() > 0;
 
     if (inter_process_publish_needed) {
       auto shared_msg =
@@ -351,7 +351,7 @@ class Publisher : public PublisherBase
     }
 
     bool inter_process_publish_needed =
-      get_subscription_count() > get_intra_process_subscription_count();
+      get_non_local_subscription_count() > 0;
 
     if (inter_process_publish_needed) {
       auto ros_msg_ptr = std::make_shared<ROSMessageType>();

diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp
@@ -126,6 +126,12 @@ class PublisherBase : public std::enable_shared_from_this<PublisherBase>
   size_t
   get_subscription_count() const;
 
+  /// Get non local subscription count
+  /** \return The number of non local subscriptions. */
+  RCLCPP_PUBLIC
+  size_t
+  get_non_local_subscription_count() const;
+
   /// Get intraprocess subscription count
   /** \return The number of intraprocess subscriptions. */
   RCLCPP_PUBLIC

diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp
@@ -189,6 +189,31 @@ PublisherBase::get_subscription_count() const
   return inter_process_subscription_count;
 }
 
+size_t
+PublisherBase::get_non_local_subscription_count() const
+{
+  size_t inter_process_non_local_subscription_count = 0;
+
+  rcl_ret_t status = rcl_publisher_get_non_local_subscription_count(
+    publisher_handle_.get(),
+    &inter_process_non_local_subscription_count);
+
+  if (RCL_RET_PUBLISHER_INVALID == status) {
+    rcl_reset_error();  /* next call will reset error message if not context */
+    if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) {
+      rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get());
+      if (nullptr != context && !rcl_context_is_valid(context)) {
+        /* publisher is invalid due to context being shutdown */
+        return 0;
+      }
+    }
+  }
+  if (RCL_RET_OK != status) {
+    rclcpp::exceptions::throw_from_rcl_error(status, "failed to get get non local subscription count");
+  }
+  return inter_process_non_local_subscription_count;
+}
+
 size_t
 PublisherBase::get_intra_process_subscription_count() const
 {