Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

apply actual QoS from rmw to the IPC publisher. #2707

Open
wants to merge 2 commits into
base: rolling
Choose a base branch
from
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
14 changes: 8 additions & 6 deletions rclcpp/include/rclcpp/publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,32 +146,34 @@ class Publisher : public PublisherBase
const rclcpp::QoS & qos,
const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
{
// Topic is unused for now.
(void)qos;
(void)options;

// If needed, setup intra process communication.
if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
auto context = node_base->get_context();
// Get the intra process manager instance for this context.
auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();
// Register the publisher with the intra process manager.
if (qos.history() != rclcpp::HistoryPolicy::KeepLast) {
// Check if the QoS is compatible with intra-process.
auto qos_profile = get_actual_qos();
if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
throw std::invalid_argument(
"intraprocess communication on topic '" + topic +
"' allowed only with keep last history qos policy");
}
if (qos.depth() == 0) {
fujitatomoya marked this conversation as resolved.
Show resolved Hide resolved
if (qos_profile.depth() == 0) {
throw std::invalid_argument(
"intraprocess communication on topic '" + topic +
"' is not allowed with a zero qos history depth value");
}
if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
if (qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
buffer_ = rclcpp::experimental::create_intra_process_buffer<
ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter>(
rclcpp::detail::resolve_intra_process_buffer_type(options_.intra_process_buffer_type),
qos,
qos_profile,
std::make_shared<ROSMessageTypeAllocator>(ros_message_type_allocator_));
}
// Register the publisher with the intra process manager.
uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_);
this->setup_intra_process(
intra_process_publisher_id,
Expand Down
16 changes: 16 additions & 0 deletions rclcpp/test/rclcpp/test_create_subscription.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,3 +92,19 @@ TEST_F(TestCreateSubscription, create_with_statistics) {
ASSERT_NE(nullptr, subscription);
EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name());
}

TEST_F(TestCreateSubscription, create_with_intra_process_com) {
auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
auto options = rclcpp::SubscriptionOptions();
options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;

auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {};
rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
ASSERT_NO_THROW(
{
subscription = rclcpp::create_subscription<test_msgs::msg::Empty>(
node, "topic_name", rclcpp::SystemDefaultsQoS(), callback, options);
});
ASSERT_NE(nullptr, subscription);
EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name());
}
25 changes: 19 additions & 6 deletions rclcpp/test/rclcpp/test_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,21 @@ TEST_F(TestPublisher, various_creation_signatures) {
}
}

/*
Testing publisher with intraprocess enabled and SystemDefaultQoS
*/
TEST_F(TestPublisher, test_publisher_with_system_default_qos) {
fujitatomoya marked this conversation as resolved.
Show resolved Hide resolved
initialize(rclcpp::NodeOptions().use_intra_process_comms(false));
// explicitly enable intra-process comm with publisher option
auto options = rclcpp::PublisherOptions();
options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
using test_msgs::msg::Empty;
ASSERT_NO_THROW(
{
auto publisher = node->create_publisher<Empty>("topic", rclcpp::SystemDefaultsQoS());
});
}

/*
Testing publisher with intraprocess enabled and invalid QoS
*/
Expand Down Expand Up @@ -432,12 +447,10 @@ TEST_F(TestPublisher, intra_process_publish_failures) {
publisher->get_publisher_handle().get(), msg.get()));
}
}
RCLCPP_EXPECT_THROW_EQ(
node->create_publisher<test_msgs::msg::Empty>(
"topic", rclcpp::QoS(0), options),
std::invalid_argument(
"intraprocess communication on topic 'topic' "
"is not allowed with a zero qos history depth value"));
// a zero depth with KEEP_LAST doesn't make sense,
// this will be interpreted as SystemDefaultQoS by rclcpp.
EXPECT_NO_THROW(
node->create_publisher<test_msgs::msg::Empty>("topic", rclcpp::QoS(0), options));
}

TEST_F(TestPublisher, inter_process_publish_failures) {
Expand Down