diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f474a67192..5f713359a0 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -237,7 +237,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 = @@ -306,7 +306,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) { ROSMessageType ros_msg; diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index d9181bea43..702cbbf7c6 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -133,6 +133,12 @@ class PublisherBase : public std::enable_shared_from_this 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 index 698db2d559..44a125b05d 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -253,6 +253,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 {