Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions rclcpp/include/rclcpp/publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down Expand Up @@ -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;
Expand Down
6 changes: 6 additions & 0 deletions rclcpp/include/rclcpp/publisher_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,12 @@ class PublisherBase : public std::enable_shared_from_this<PublisherBase>
size_t
get_subscription_count() const;

/// Get non local subscription count
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this means that Not in the same context for the ROS 2 user (precisely for DDS-RTPS, GUID prefix is not the same), that would be nicer to add the description explains?

/** \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
Expand Down
27 changes: 27 additions & 0 deletions rclcpp/src/rclcpp/publisher_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -253,6 +253,33 @@ 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");
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
"failed to get get non local subscription count");
"failed to get non local subscription count");

}
return inter_process_non_local_subscription_count;
}

size_t
PublisherBase::get_intra_process_subscription_count() const
{
Expand Down