diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 8b7d8f973c..634d4837a5 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -33,13 +33,14 @@ using namespace std::chrono_literals; class TestQosEvent : public ::testing::Test { protected: - static void SetUpTestCase() + void SetUp() { + // We initialize and shutdown the context (and hence also the rmw_context), + // for each test case to reset the ROS graph for each test case. rclcpp::init(0, nullptr); - } - void SetUp() - { + rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + node = std::make_shared("test_qos_event", "/ns"); message_callback = [node = node.get()](test_msgs::msg::Empty::ConstSharedPtr /*msg*/) { @@ -50,13 +51,10 @@ class TestQosEvent : public ::testing::Test void TearDown() { node.reset(); - } - - static void TearDownTestCase() - { rclcpp::shutdown(); } + std::string rmw_implementation_str; static constexpr char topic_name[] = "test_topic"; rclcpp::Node::SharedPtr node; std::function message_callback; @@ -75,28 +73,29 @@ TEST_F(TestQosEvent, test_publisher_constructor) auto publisher = node->create_publisher( topic_name, 10, options); - // options arg with one of the callbacks - options.event_callbacks.deadline_callback = - [node = node.get()](rclcpp::QOSDeadlineOfferedInfo & event) { - RCLCPP_INFO( - node->get_logger(), - "Offered deadline missed - total %d (delta %d)", - event.total_count, event.total_count_change); - }; - publisher = node->create_publisher( - topic_name, 10, options); - - // options arg with two of the callbacks - options.event_callbacks.liveliness_callback = - [node = node.get()](rclcpp::QOSLivelinessLostInfo & event) { - RCLCPP_INFO( - node->get_logger(), - "Liveliness lost - total %d (delta %d)", - event.total_count, event.total_count_change); - }; - publisher = node->create_publisher( - topic_name, 10, options); - + if (rmw_implementation_str != "rmw_zenoh_cpp") { + // options arg with one of the callbacks + options.event_callbacks.deadline_callback = + [node = node.get()](rclcpp::QOSDeadlineOfferedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Offered deadline missed - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + publisher = node->create_publisher( + topic_name, 10, options); + + // options arg with two of the callbacks + options.event_callbacks.liveliness_callback = + [node = node.get()](rclcpp::QOSLivelinessLostInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Liveliness lost - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + publisher = node->create_publisher( + topic_name, 10, options); + } // options arg with three of the callbacks options.event_callbacks.incompatible_qos_callback = [node = node.get()](rclcpp::QOSOfferedIncompatibleQoSInfo & event) { @@ -114,35 +113,38 @@ TEST_F(TestQosEvent, test_publisher_constructor) */ TEST_F(TestQosEvent, test_subscription_constructor) { + // While rmw_zenoh does not support Deadline/LivelinessChanged events, + // it does support IncompatibleQoS rclcpp::SubscriptionOptions options; // options arg with no callbacks auto subscription = node->create_subscription( topic_name, 10, message_callback, options); - // options arg with one of the callbacks - options.event_callbacks.deadline_callback = - [node = node.get()](rclcpp::QOSDeadlineRequestedInfo & event) { - RCLCPP_INFO( - node->get_logger(), - "Requested deadline missed - total %d (delta %d)", - event.total_count, event.total_count_change); - }; - subscription = node->create_subscription( - topic_name, 10, message_callback, options); - - // options arg with two of the callbacks - options.event_callbacks.liveliness_callback = - [node = node.get()](rclcpp::QOSLivelinessChangedInfo & event) { - RCLCPP_INFO( - node->get_logger(), - "Liveliness changed - alive %d (delta %d), not alive %d (delta %d)", - event.alive_count, event.alive_count_change, - event.not_alive_count, event.not_alive_count_change); - }; - subscription = node->create_subscription( - topic_name, 10, message_callback, options); - + if (rmw_implementation_str != "rmw_zenoh_cpp") { + // options arg with one of the callbacks + options.event_callbacks.deadline_callback = + [node = node.get()](rclcpp::QOSDeadlineRequestedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Requested deadline missed - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + + // options arg with two of the callbacks + options.event_callbacks.liveliness_callback = + [node = node.get()](rclcpp::QOSLivelinessChangedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Liveliness changed - alive %d (delta %d), not alive %d (delta %d)", + event.alive_count, event.alive_count_change, + event.not_alive_count, event.not_alive_count_change); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + } // options arg with three of the callbacks options.event_callbacks.incompatible_qos_callback = [node = node.get()](rclcpp::QOSRequestedIncompatibleQoSInfo & event) { @@ -209,14 +211,19 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) const auto timeout = std::chrono::seconds(10); ex.spin_until_future_complete(log_msgs_future, timeout); - EXPECT_EQ( - "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " - "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", - pub_log_msg); - EXPECT_EQ( - "New publisher discovered on topic '/ns/test_topic', offering incompatible QoS. " - "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", - sub_log_msg); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_EQ(rclcpp::QoSCompatibility::Ok, + qos_check_compatible(qos_profile_publisher, qos_profile_subscription).compatibility); + } else { + EXPECT_EQ( + "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + pub_log_msg); + EXPECT_EQ( + "New publisher discovered on topic '/ns/test_topic', offering incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + sub_log_msg); + } rcutils_logging_set_output_handler(original_output_handler); } @@ -228,7 +235,8 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { // This callback requires some type of parameter, but it could be anything auto callback = [](int) {}; - const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + const rcl_publisher_event_type_t event_type = rmw_implementation_str == "rmw_zenoh_cpp" ? + RCL_PUBLISHER_MATCHED : RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; { // Logs error and returns @@ -265,13 +273,16 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { } TEST_F(TestQosEvent, execute) { + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } auto publisher = node->create_publisher(topic_name, 10); auto rcl_handle = publisher->get_publisher_handle(); bool handler_callback_executed = false; // This callback requires some type of parameter, but it could be anything auto callback = [&handler_callback_executed](int) {handler_callback_executed = true;}; - rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; rclcpp::EventHandler handler( callback, rcl_publisher_event_init, rcl_handle, event_type); @@ -297,8 +308,9 @@ TEST_F(TestQosEvent, add_to_wait_set) { // This callback requires some type of parameter, but it could be anything auto callback = [](int) {}; - rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; - rclcpp::EventHandler handler( + const rcl_publisher_event_type_t event_type = rmw_implementation_str == "rmw_zenoh_cpp" ? + RCL_PUBLISHER_MATCHED : RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + rclcpp::EventHandler handler( callback, rcl_publisher_event_init, rcl_handle, event_type); EXPECT_EQ(1u, handler.get_number_of_ready_events()); @@ -319,6 +331,10 @@ TEST_F(TestQosEvent, add_to_wait_set) { TEST_F(TestQosEvent, test_on_new_event_callback) { + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1)); auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2)); @@ -364,18 +380,19 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) auto sub = node->create_subscription(topic_name, 10, message_callback); auto dummy_cb = [](size_t count_events) {(void)count_events;}; - EXPECT_NO_THROW( - pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); + if (rmw_implementation_str != "rmw_zenoh_cpp") { + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); - EXPECT_NO_THROW( - pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); - EXPECT_NO_THROW( - pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_LIVELINESS_LOST)); + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_LIVELINESS_LOST)); - EXPECT_NO_THROW( + EXPECT_NO_THROW( pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_LIVELINESS_LOST)); - + } EXPECT_NO_THROW( pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); @@ -388,18 +405,19 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) EXPECT_NO_THROW( pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_MATCHED)); - EXPECT_NO_THROW( - sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); - EXPECT_NO_THROW( - sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); - EXPECT_NO_THROW( - sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_LIVELINESS_CHANGED)); - - EXPECT_NO_THROW( - sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_LIVELINESS_CHANGED)); + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_LIVELINESS_CHANGED)); + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_LIVELINESS_CHANGED)); + } EXPECT_NO_THROW( sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); @@ -412,24 +430,26 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) EXPECT_NO_THROW( sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_MATCHED)); - std::function invalid_cb; + if (rmw_implementation_str != "rmw_zenoh_cpp") { + std::function invalid_cb; - rclcpp::SubscriptionOptions sub_options; - sub_options.event_callbacks.deadline_callback = [](auto) {}; - sub = node->create_subscription( - topic_name, 10, message_callback, sub_options); + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.deadline_callback = [](auto) {}; + sub = node->create_subscription( + topic_name, 10, message_callback, sub_options); - EXPECT_THROW( - sub->set_on_new_qos_event_callback(invalid_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED), - std::invalid_argument); + EXPECT_THROW( + sub->set_on_new_qos_event_callback(invalid_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED), + std::invalid_argument); - rclcpp::PublisherOptions pub_options; - pub_options.event_callbacks.deadline_callback = [](auto) {}; - pub = node->create_publisher(topic_name, 10, pub_options); + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.deadline_callback = [](auto) {}; + pub = node->create_publisher(topic_name, 10, pub_options); - EXPECT_THROW( - pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), - std::invalid_argument); + EXPECT_THROW( + pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), + std::invalid_argument); + } } TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback)