From 99f1d8d1249db9b5b576b326718c58ded59d6caf Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 16 Dec 2024 21:51:50 -0800 Subject: [PATCH] apply actual QoS from rmw to the IPC publisher. (backport #2707) (#2711) * apply actual QoS from rmw to the IPC publisher. (#2707) * apply actual QoS from rmw to the IPC publisher. Signed-off-by: Tomoya Fujita * address uncrustify warning. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita (cherry picked from commit 016cfeac99e4b67f58abdf247e57f05b85c09ec4) # Conflicts: # rclcpp/include/rclcpp/publisher.hpp * resolve conflicts for backport humble. Signed-off-by: Tomoya Fujita * address uncrustify failure. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita Co-authored-by: Tomoya Fujita --- rclcpp/include/rclcpp/publisher.hpp | 12 +++++---- .../test/rclcpp/test_create_subscription.cpp | 16 ++++++++++++ rclcpp/test/rclcpp/test_publisher.cpp | 25 ++++++++++++++----- 3 files changed, 42 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 111d965183..2e4aa479c0 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -177,7 +177,7 @@ class Publisher : public PublisherBase const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options) { - // Topic is unused for now. + (void)qos; (void)options; // If needed, setup intra process communication. @@ -185,21 +185,23 @@ class Publisher : public PublisherBase auto context = node_base->get_context(); // Get the intra process manager instance for this context. auto ipm = context->get_sub_context(); - // 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) { + 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::Volatile) { + if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) { throw std::invalid_argument( "intraprocess communication allowed only with volatile durability"); } + // Register the publisher with the intra process manager. uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this()); this->setup_intra_process( intra_process_publisher_id, diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index fd947485e2..a5e14fdb89 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -93,3 +93,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("my_node", "/ns"); + auto options = rclcpp::SubscriptionOptions(); + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + rclcpp::Subscription::SharedPtr subscription; + ASSERT_NO_THROW( + { + subscription = rclcpp::create_subscription( + node, "topic_name", rclcpp::SystemDefaultsQoS(), callback, options); + }); + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 424615a1f2..31a518f8aa 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -176,6 +176,21 @@ TEST_F(TestPublisher, various_creation_signatures) { } } +/* + Testing publisher with intraprocess enabled and SystemDefaultQoS + */ +TEST_F(TestPublisher, test_publisher_with_system_default_qos) { + 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("topic", rclcpp::SystemDefaultsQoS()); + }); +} + /* Testing publisher with intraprocess enabled and invalid QoS */ @@ -418,12 +433,10 @@ TEST_F(TestPublisher, intra_process_publish_failures) { publisher->publish(std::move(loaned_msg)), std::runtime_error("loaned message is not valid")); } - RCLCPP_EXPECT_THROW_EQ( - node->create_publisher( - "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("topic", rclcpp::QoS(0), options)); } TEST_F(TestPublisher, inter_process_publish_failures) {