From 2e3a390c591dd584f6445c76ce3674382537be72 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 12 Nov 2019 08:40:19 -0800 Subject: [PATCH 1/8] Added support for on_requested_incompatible_qos and on_offered_incompatible_qos event callbacks for publisher and subscriptions. Signed-off-by: Jaison Titus --- rcl/include/rcl/event.h | 6 ++++-- rcl/src/rcl/event.c | 6 ++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/rcl/include/rcl/event.h b/rcl/include/rcl/event.h index 2d048acf5..c6b3d07a6 100644 --- a/rcl/include/rcl/event.h +++ b/rcl/include/rcl/event.h @@ -30,13 +30,15 @@ extern "C" typedef enum rcl_publisher_event_type_t { RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, - RCL_PUBLISHER_LIVELINESS_LOST + RCL_PUBLISHER_LIVELINESS_LOST, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS } rcl_publisher_event_type_t; typedef enum rcl_subscription_event_type_t { RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, - RCL_SUBSCRIPTION_LIVELINESS_CHANGED + RCL_SUBSCRIPTION_LIVELINESS_CHANGED, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS } rcl_subscription_event_type_t; /// rmw struct. diff --git a/rcl/src/rcl/event.c b/rcl/src/rcl/event.c index ce1eb326f..834c3a16a 100644 --- a/rcl/src/rcl/event.c +++ b/rcl/src/rcl/event.c @@ -76,6 +76,9 @@ rcl_publisher_event_init( case RCL_PUBLISHER_LIVELINESS_LOST: rmw_event_type = RMW_EVENT_LIVELINESS_LOST; break; + case RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS: + rmw_event_type = RMW_EVENT_OFFERED_INCOMPATIBLE_QOS; + break; default: RCL_SET_ERROR_MSG("Event type for publisher not supported"); return RCL_RET_INVALID_ARGUMENT; @@ -116,6 +119,9 @@ rcl_subscription_event_init( case RCL_SUBSCRIPTION_LIVELINESS_CHANGED: rmw_event_type = RMW_EVENT_LIVELINESS_CHANGED; break; + case RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS: + rmw_event_type = RMW_EVENT_REQUESTED_INCOMPATIBLE_QOS; + break; default: RCL_SET_ERROR_MSG("Event type for subscription not supported"); return RCL_RET_INVALID_ARGUMENT; From f9bc47aba153980b1e9a945b970b8c0dd1df8670 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Thu, 21 Nov 2019 09:34:48 -0800 Subject: [PATCH 2/8] Refactored test_events to take in custom qos profiles. Signed-off-by: Jaison Titus --- rcl/test/rcl/test_events.cpp | 60 ++++++++++++++++++------------------ 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index 2371a68cb..15015e0e7 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -73,22 +73,23 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + + default_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + default_qos_profile.depth = 0; + default_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + default_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT; + default_qos_profile.lifespan = {0, 0}; + default_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0}; + default_qos_profile.liveliness_lease_duration = {LIVELINESS_LEASE_DURATION_IN_S.count(), 0}; + default_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; } - rcl_ret_t setup_publisher( - const rmw_time_t & deadline, - const rmw_time_t & lifespan, - const rmw_time_t & liveliness_lease_duration, - const rmw_qos_liveliness_policy_t liveliness_policy) + rcl_ret_t setup_publisher(const rmw_qos_profile_t qos_profile) { // init publisher publisher = rcl_get_zero_initialized_publisher(); rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - publisher_options.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - publisher_options.qos.deadline = deadline; - publisher_options.qos.lifespan = lifespan; - publisher_options.qos.liveliness = liveliness_policy; - publisher_options.qos.liveliness_lease_duration = liveliness_lease_duration; + publisher_options.qos = qos_profile; return rcl_publisher_init( &publisher, this->node_ptr, @@ -97,21 +98,12 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test &publisher_options); } - rcl_ret_t setup_subscriber( - const rmw_time_t & deadline, - const rmw_time_t & lifespan, - const rmw_time_t & liveliness_lease_duration, - const rmw_qos_liveliness_policy_t liveliness_policy) + rcl_ret_t setup_subscriber(const rmw_qos_profile_t qos_profile) { // init publisher subscription = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - subscription_options.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - subscription_options.qos.deadline = deadline; - subscription_options.qos.lifespan = lifespan; - subscription_options.qos.liveliness = liveliness_policy; - subscription_options.qos.liveliness_lease_duration = liveliness_lease_duration; - + subscription_options.qos = qos_profile; return rcl_subscription_init( &subscription, this->node_ptr, @@ -122,17 +114,13 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test void setup_publisher_and_subscriber( const rcl_publisher_event_type_t & pub_event_type, - const rcl_subscription_event_type_t & sub_event_type) + const rmw_qos_profile_t pub_qos_profile, + const rcl_subscription_event_type_t & sub_event_type, + const rmw_qos_profile_t sub_qos_profile) { rcl_ret_t ret; - - rmw_time_t lifespan {0, 0}; - rmw_time_t deadline {DEADLINE_PERIOD_IN_S.count(), 0}; - rmw_time_t lease_duration {LIVELINESS_LEASE_DURATION_IN_S.count(), 0}; - rmw_qos_liveliness_policy_t liveliness_policy = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - // init publisher - ret = setup_publisher(deadline, lifespan, lease_duration, liveliness_policy); + ret = setup_publisher(pub_qos_profile); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; // init publisher events @@ -141,7 +129,7 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; // init subscription - ret = setup_subscriber(deadline, lifespan, lease_duration, liveliness_policy); + ret = setup_subscriber(sub_qos_profile); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; // init subscription event @@ -169,6 +157,17 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test ASSERT_TRUE(subscribe_success) << "Publisher/Subscription discovery timed out"; } + void setup_publisher_and_subscriber( + const rcl_publisher_event_type_t & pub_event_type, + const rcl_subscription_event_type_t & sub_event_type) + { + setup_publisher_and_subscriber( + pub_event_type, + default_qos_profile, + sub_event_type, + default_qos_profile); + } + void tear_down_publisher_subscriber() { rcl_ret_t ret; @@ -213,6 +212,7 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test bool is_opensplice; const char * topic = "rcl_test_publisher_subscription_events"; const rosidl_message_type_support_t * ts; + rmw_qos_profile_t default_qos_profile; }; rcl_ret_t From c728e76782461005dc9f97acc5f470b6139ad593 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Thu, 21 Nov 2019 12:56:52 -0800 Subject: [PATCH 3/8] Added tests for incompatible_qos events. Signed-off-by: Jaison Titus --- rcl/test/rcl/test_events.cpp | 152 +++++++++++++++++++++++++++++++---- 1 file changed, 138 insertions(+), 14 deletions(-) diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index 15015e0e7..e10c453e1 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -136,7 +136,19 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test subscription_event = rcl_get_zero_initialized_event(); ret = rcl_subscription_event_init(&subscription_event, &subscription, sub_event_type); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + void setup_publisher_and_subscriber_and_assert_discovery( + const rcl_publisher_event_type_t & pub_event_type, + const rcl_subscription_event_type_t & sub_event_type) + { + setup_publisher_and_subscriber( + pub_event_type, + default_qos_profile, + sub_event_type, + default_qos_profile); + + rcl_ret_t ret; // wait for discovery, time out after 10s static const size_t max_iterations = 1000; static const auto wait_period = 10ms; @@ -157,17 +169,6 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test ASSERT_TRUE(subscribe_success) << "Publisher/Subscription discovery timed out"; } - void setup_publisher_and_subscriber( - const rcl_publisher_event_type_t & pub_event_type, - const rcl_subscription_event_type_t & sub_event_type) - { - setup_publisher_and_subscriber( - pub_event_type, - default_qos_profile, - sub_event_type, - default_qos_profile); - } - void tear_down_publisher_subscriber() { rcl_ret_t ret; @@ -347,7 +348,7 @@ conditional_wait_for_msgs_and_events( */ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_missed) { - setup_publisher_and_subscriber( + setup_publisher_and_subscriber_and_assert_discovery( RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); rcl_ret_t ret; @@ -413,7 +414,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_ */ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_missed) { - setup_publisher_and_subscriber( + setup_publisher_and_subscriber_and_assert_discovery( RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); rcl_ret_t ret; @@ -487,7 +488,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_mis */ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_kill_pub) { - setup_publisher_and_subscriber( + setup_publisher_and_subscriber_and_assert_discovery( RCL_PUBLISHER_LIVELINESS_LOST, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); rcl_ret_t ret; @@ -565,3 +566,126 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k // clean up tear_down_publisher_subscriber(); } + +/* + * Basic test of publisher and subscriber incompatible qos callback events. + */ +TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible_qos) +{ + // a vector of tuples that holds the expected qos_policy_id, publisher qos profile, + // subscription qos profile and the error message, in that order. + std::vector> input; + // durability + rmw_qos_profile_t pub_qos_profile = default_qos_profile; + rmw_qos_profile_t sub_qos_profile = default_qos_profile; + pub_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + sub_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + input.push_back(std::make_tuple( + RMW_QOS_POLICY_ID_DURABILITY, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos durability")); + + // deadline + pub_qos_profile = default_qos_profile; + pub_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; + sub_qos_profile = default_qos_profile; + sub_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0}; + input.push_back(std::make_tuple( + RMW_QOS_POLICY_ID_DEADLINE, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos deadline")); + + // liveliness + pub_qos_profile = default_qos_profile; + pub_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; + sub_qos_profile = default_qos_profile; + sub_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + input.push_back(std::make_tuple( + RMW_QOS_POLICY_ID_LIVELINESS, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos liveliness policy")); + + // liveliness lease duration + pub_qos_profile = default_qos_profile; + pub_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; + sub_qos_profile = default_qos_profile; + sub_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count(), 0}; + input.push_back(std::make_tuple( + RMW_QOS_POLICY_ID_LIVELINESS, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos liveliness_lease_duration")); + + // reliability + pub_qos_profile = default_qos_profile; + pub_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + sub_qos_profile = default_qos_profile; + sub_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + input.push_back(std::make_tuple( + RMW_QOS_POLICY_ID_RELIABILITY, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos reliability")); + + rcl_ret_t ret; + + for (const auto & element: input) { + const auto & qos_policy_id = std::get<0>(element); + const auto & publisher_qos_profile = std::get<1>(element); + const auto & subscription_qos_profile = std::get<2>(element); + const auto & error_msg = std::get<3>(element); + setup_publisher_and_subscriber( + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, + publisher_qos_profile, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, + subscription_qos_profile); + + // try to received event for 10s + static const size_t max_iterations = 1000; + static const auto wait_period = 10ms; + bool pub_event_success = false; + bool sub_event_success = false; + for (size_t i = 0; i < max_iterations; ++i) { + rmw_requested_incompatible_qos_status_t requested_incompatible_qos_status; + ret = rcl_take_event(&subscription_event, &requested_incompatible_qos_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const auto & pub_total_count = requested_incompatible_qos_status.total_count; + const auto & pub_total_count_change = requested_incompatible_qos_status.total_count_change; + const auto & pub_last_policy_id = requested_incompatible_qos_status.last_policy_id; + if (pub_total_count != 0 && pub_total_count_change != 0 && pub_last_policy_id != 0) { + pub_event_success = true; + EXPECT_EQ(pub_total_count, 1) << error_msg; + EXPECT_EQ(pub_total_count_change, 1) << error_msg; + EXPECT_EQ(pub_last_policy_id, qos_policy_id) << error_msg; + } + + rmw_offered_incompatible_qos_status_t offered_incompatible_qos_status; + ret = rcl_take_event(&publisher_event, &offered_incompatible_qos_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const auto & sub_total_count = offered_incompatible_qos_status.total_count; + const auto & sub_total_count_change = offered_incompatible_qos_status.total_count_change; + const auto & sub_last_policy_id = offered_incompatible_qos_status.last_policy_id; + if (sub_total_count != 0 && sub_total_count_change != 0 && sub_last_policy_id != 0) { + sub_event_success = true; + EXPECT_EQ(sub_total_count, 1) << error_msg; + EXPECT_EQ(sub_total_count_change, 1) << error_msg; + EXPECT_EQ(sub_last_policy_id, qos_policy_id) << error_msg; + } + + if (pub_event_success && sub_event_success) { + break; + } + std::this_thread::sleep_for(wait_period); + } + EXPECT_TRUE(pub_event_success) << "Publisher incompatible qos event timed out for: " + + error_msg; + EXPECT_TRUE(sub_event_success) << "Subscription incompatible qos event timed out for: " + + error_msg; + // clean up + tear_down_publisher_subscriber(); + } +} From 62f3551b0e6369f6f65bfd988ef224fb8ae69a12 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Thu, 21 Nov 2019 13:17:51 -0800 Subject: [PATCH 4/8] - Fixed code styling issues - Skips the test for all implementations except opensplice Signed-off-by: Jaison Titus --- rcl/test/rcl/test_events.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index e10c453e1..54479e4ab 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include "rcl/rcl.h" #include "rcl/subscription.h" @@ -569,9 +571,13 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k /* * Basic test of publisher and subscriber incompatible qos callback events. + * Only implemented in opensplice at the moment. */ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible_qos) { + if (!is_opensplice) { + GTEST_SKIP(); + } // a vector of tuples that holds the expected qos_policy_id, publisher qos profile, // subscription qos profile and the error message, in that order. std::vector(element); const auto & publisher_qos_profile = std::get<1>(element); const auto & subscription_qos_profile = std::get<2>(element); From 2cd4e5066549844e14a49b54eb8616c9064ccfc0 Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Thu, 21 Nov 2019 13:49:24 -0800 Subject: [PATCH 5/8] Fixed typo in comment. Signed-off-by: Jaison Titus --- rcl/test/rcl/test_events.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index 54479e4ab..ee4c16e33 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -650,7 +650,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, subscription_qos_profile); - // try to received event for 10s + // try to receive event, retry for 10s static const size_t max_iterations = 1000; static const auto wait_period = 10ms; bool pub_event_success = false; From 1c7127e937d5fd9c4a576d977225f8edbb770dd1 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Tue, 3 Mar 2020 16:28:26 -0800 Subject: [PATCH 6/8] fix compilation issues Signed-off-by: Miaofei --- rcl/include/rcl/event.h | 4 ++-- rcl/src/rcl/event.c | 4 ++-- rcl/test/rcl/test_events.cpp | 35 ++++++++++++++++++----------------- 3 files changed, 22 insertions(+), 21 deletions(-) diff --git a/rcl/include/rcl/event.h b/rcl/include/rcl/event.h index c6b3d07a6..6daebe1ec 100644 --- a/rcl/include/rcl/event.h +++ b/rcl/include/rcl/event.h @@ -31,14 +31,14 @@ typedef enum rcl_publisher_event_type_t { RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, RCL_PUBLISHER_LIVELINESS_LOST, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, } rcl_publisher_event_type_t; typedef enum rcl_subscription_event_type_t { RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, RCL_SUBSCRIPTION_LIVELINESS_CHANGED, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, } rcl_subscription_event_type_t; /// rmw struct. diff --git a/rcl/src/rcl/event.c b/rcl/src/rcl/event.c index 834c3a16a..81ccf7c50 100644 --- a/rcl/src/rcl/event.c +++ b/rcl/src/rcl/event.c @@ -77,7 +77,7 @@ rcl_publisher_event_init( rmw_event_type = RMW_EVENT_LIVELINESS_LOST; break; case RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS: - rmw_event_type = RMW_EVENT_OFFERED_INCOMPATIBLE_QOS; + rmw_event_type = RMW_EVENT_OFFERED_QOS_INCOMPATIBLE; break; default: RCL_SET_ERROR_MSG("Event type for publisher not supported"); @@ -120,7 +120,7 @@ rcl_subscription_event_init( rmw_event_type = RMW_EVENT_LIVELINESS_CHANGED; break; case RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS: - rmw_event_type = RMW_EVENT_REQUESTED_INCOMPATIBLE_QOS; + rmw_event_type = RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE; break; default: RCL_SET_ERROR_MSG("Event type for subscription not supported"); diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index ee4c16e33..32267de43 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -23,6 +23,7 @@ #include "rcl/rcl.h" #include "rcl/subscription.h" #include "rcl/error_handling.h" +#include "rmw/incompatible_qos_events_statuses.h" #include "test_msgs/msg/strings.h" #include "rosidl_generator_c/string_functions.h" @@ -47,7 +48,7 @@ constexpr seconds MAX_WAIT_PER_TESTCASE = 10s; #define EXPECT_OK(varname) EXPECT_EQ(varname, RCL_RET_OK) << rcl_get_error_string().str -class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test +class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::TestWithParam> { public: void SetUp() @@ -578,9 +579,9 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible if (!is_opensplice) { GTEST_SKIP(); } - // a vector of tuples that holds the expected qos_policy_id, publisher qos profile, + // a vector of tuples that holds the expected qos_policy_kind, publisher qos profile, // subscription qos profile and the error message, in that order. - std::vector> input; // durability rmw_qos_profile_t pub_qos_profile = default_qos_profile; @@ -588,7 +589,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible pub_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; sub_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; input.push_back(std::make_tuple( - RMW_QOS_POLICY_ID_DURABILITY, + RMW_QOS_POLICY_DURABILITY, pub_qos_profile, sub_qos_profile, "Incompatible qos durability")); @@ -599,7 +600,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible sub_qos_profile = default_qos_profile; sub_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0}; input.push_back(std::make_tuple( - RMW_QOS_POLICY_ID_DEADLINE, + RMW_QOS_POLICY_DEADLINE, pub_qos_profile, sub_qos_profile, "Incompatible qos deadline")); @@ -610,7 +611,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible sub_qos_profile = default_qos_profile; sub_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; input.push_back(std::make_tuple( - RMW_QOS_POLICY_ID_LIVELINESS, + RMW_QOS_POLICY_LIVELINESS, pub_qos_profile, sub_qos_profile, "Incompatible qos liveliness policy")); @@ -621,7 +622,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible sub_qos_profile = default_qos_profile; sub_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count(), 0}; input.push_back(std::make_tuple( - RMW_QOS_POLICY_ID_LIVELINESS, + RMW_QOS_POLICY_LIVELINESS, pub_qos_profile, sub_qos_profile, "Incompatible qos liveliness_lease_duration")); @@ -632,7 +633,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible sub_qos_profile = default_qos_profile; sub_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; input.push_back(std::make_tuple( - RMW_QOS_POLICY_ID_RELIABILITY, + RMW_QOS_POLICY_RELIABILITY, pub_qos_profile, sub_qos_profile, "Incompatible qos reliability")); @@ -640,7 +641,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible rcl_ret_t ret; for (const auto & element : input) { - const auto & qos_policy_id = std::get<0>(element); + const auto & qos_policy_kind = std::get<0>(element); const auto & publisher_qos_profile = std::get<1>(element); const auto & subscription_qos_profile = std::get<2>(element); const auto & error_msg = std::get<3>(element); @@ -656,30 +657,30 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible bool pub_event_success = false; bool sub_event_success = false; for (size_t i = 0; i < max_iterations; ++i) { - rmw_requested_incompatible_qos_status_t requested_incompatible_qos_status; + rmw_requested_qos_incompatible_event_status_t requested_incompatible_qos_status; ret = rcl_take_event(&subscription_event, &requested_incompatible_qos_status); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; const auto & pub_total_count = requested_incompatible_qos_status.total_count; const auto & pub_total_count_change = requested_incompatible_qos_status.total_count_change; - const auto & pub_last_policy_id = requested_incompatible_qos_status.last_policy_id; - if (pub_total_count != 0 && pub_total_count_change != 0 && pub_last_policy_id != 0) { + const auto & pub_last_policy_kind = requested_incompatible_qos_status.last_policy_kind; + if (pub_total_count != 0 && pub_total_count_change != 0 && pub_last_policy_kind != 0) { pub_event_success = true; EXPECT_EQ(pub_total_count, 1) << error_msg; EXPECT_EQ(pub_total_count_change, 1) << error_msg; - EXPECT_EQ(pub_last_policy_id, qos_policy_id) << error_msg; + EXPECT_EQ(pub_last_policy_kind, qos_policy_kind) << error_msg; } - rmw_offered_incompatible_qos_status_t offered_incompatible_qos_status; + rmw_offered_qos_incompatible_event_status_t offered_incompatible_qos_status; ret = rcl_take_event(&publisher_event, &offered_incompatible_qos_status); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; const auto & sub_total_count = offered_incompatible_qos_status.total_count; const auto & sub_total_count_change = offered_incompatible_qos_status.total_count_change; - const auto & sub_last_policy_id = offered_incompatible_qos_status.last_policy_id; - if (sub_total_count != 0 && sub_total_count_change != 0 && sub_last_policy_id != 0) { + const auto & sub_last_policy_kind = offered_incompatible_qos_status.last_policy_kind; + if (sub_total_count != 0 && sub_total_count_change != 0 && sub_last_policy_kind != 0) { sub_event_success = true; EXPECT_EQ(sub_total_count, 1) << error_msg; EXPECT_EQ(sub_total_count_change, 1) << error_msg; - EXPECT_EQ(sub_last_policy_id, qos_policy_id) << error_msg; + EXPECT_EQ(sub_last_policy_kind, qos_policy_kind) << error_msg; } if (pub_event_success && sub_event_success) { From 8e6d6259841ca1307a633bfb5e06ac9b3c9d6607 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Tue, 3 Mar 2020 22:07:37 -0800 Subject: [PATCH 7/8] use gtest's parameterized tests Signed-off-by: Miaofei --- rcl/test/rcl/test_events.cpp | 257 ++++++++++++++++++++--------------- 1 file changed, 146 insertions(+), 111 deletions(-) diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index 32267de43..38bda5164 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -14,11 +14,12 @@ #include +#include +#include #include #include #include #include -#include #include "rcl/rcl.h" #include "rcl/subscription.h" @@ -47,8 +48,11 @@ constexpr seconds MAX_WAIT_PER_TESTCASE = 10s; #define EXPECT_OK(varname) EXPECT_EQ(varname, RCL_RET_OK) << rcl_get_error_string().str +using TestIncompatibleQosEventParams = std::tuple; -class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::TestWithParam> +class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) + : public ::testing::TestWithParam { public: void SetUp() @@ -76,15 +80,6 @@ class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::TestWi ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); - - default_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - default_qos_profile.depth = 0; - default_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - default_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT; - default_qos_profile.lifespan = {0, 0}; - default_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0}; - default_qos_profile.liveliness_lease_duration = {LIVELINESS_LEASE_DURATION_IN_S.count(), 0}; - default_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; } rcl_ret_t setup_publisher(const rmw_qos_profile_t qos_profile) @@ -205,6 +200,20 @@ class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::TestWi EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } + struct PrintToStringParamName + { + template + std::string operator()(const ::testing::TestParamInfo & info) const + { + const auto & test_params = static_cast(info.param); + std::string error_msg = std::get<3>(test_params); + std::replace(error_msg.begin(), error_msg.end(), ' ', '_'); + return error_msg; + } + }; + + static const rmw_qos_profile_t default_qos_profile; + protected: rcl_context_t * context_ptr; rcl_node_t * node_ptr; @@ -216,7 +225,20 @@ class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::TestWi bool is_opensplice; const char * topic = "rcl_test_publisher_subscription_events"; const rosidl_message_type_support_t * ts; - rmw_qos_profile_t default_qos_profile; +}; + +using TestEventFixture = CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION); + +const rmw_qos_profile_t TestEventFixture::default_qos_profile = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, // history + 0, // depth + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, // reliability + RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, // durability + {DEADLINE_PERIOD_IN_S.count(), 0}, // deadline + {0, 0}, // lifespan + RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, // liveliness + {LIVELINESS_LEASE_DURATION_IN_S.count(), 0}, // liveliness_lease_duration + false // avoid_ros_namespace_conventions }; rcl_ret_t @@ -349,7 +371,7 @@ conditional_wait_for_msgs_and_events( /* * Basic test of publisher and subscriber deadline events, with first message sent before deadline */ -TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_missed) +TEST_F(TestEventFixture, test_pubsub_no_deadline_missed) { setup_publisher_and_subscriber_and_assert_discovery( RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, @@ -415,7 +437,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_ /* * Basic test of publisher and subscriber deadline events, with first message sent after deadline */ -TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_missed) +TEST_F(TestEventFixture, test_pubsub_deadline_missed) { setup_publisher_and_subscriber_and_assert_discovery( RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, @@ -489,7 +511,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_mis /* * Basic test of publisher and subscriber liveliness events, with publisher killed */ -TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_kill_pub) +TEST_F(TestEventFixture, test_pubsub_liveliness_kill_pub) { setup_publisher_and_subscriber_and_assert_discovery( RCL_PUBLISHER_LIVELINESS_LOST, @@ -574,125 +596,138 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k * Basic test of publisher and subscriber incompatible qos callback events. * Only implemented in opensplice at the moment. */ -TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_incompatible_qos) +TEST_P(TestEventFixture, test_pubsub_incompatible_qos) { if (!is_opensplice) { GTEST_SKIP(); } - // a vector of tuples that holds the expected qos_policy_kind, publisher qos profile, + + const auto & input = GetParam(); + const auto & qos_policy_kind = std::get<0>(input); + const auto & publisher_qos_profile = std::get<1>(input); + const auto & subscription_qos_profile = std::get<2>(input); + const auto & error_msg = std::get<3>(input); + setup_publisher_and_subscriber( + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, + publisher_qos_profile, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, + subscription_qos_profile); + + rcl_ret_t ret; + + // try to receive event, retry for 10s + static const size_t max_iterations = 1000; + static const auto wait_period = 10ms; + bool pub_event_success = false; + bool sub_event_success = false; + for (size_t i = 0; i < max_iterations; ++i) { + rmw_requested_qos_incompatible_event_status_t requested_incompatible_qos_status; + ret = rcl_take_event(&subscription_event, &requested_incompatible_qos_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const auto & pub_total_count = requested_incompatible_qos_status.total_count; + const auto & pub_total_count_change = requested_incompatible_qos_status.total_count_change; + const auto & pub_last_policy_kind = requested_incompatible_qos_status.last_policy_kind; + if (pub_total_count != 0 && pub_total_count_change != 0 && pub_last_policy_kind != 0) { + pub_event_success = true; + EXPECT_EQ(pub_total_count, 1) << error_msg; + EXPECT_EQ(pub_total_count_change, 1) << error_msg; + EXPECT_EQ(pub_last_policy_kind, qos_policy_kind) << error_msg; + } + + rmw_offered_qos_incompatible_event_status_t offered_incompatible_qos_status; + ret = rcl_take_event(&publisher_event, &offered_incompatible_qos_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const auto & sub_total_count = offered_incompatible_qos_status.total_count; + const auto & sub_total_count_change = offered_incompatible_qos_status.total_count_change; + const auto & sub_last_policy_kind = offered_incompatible_qos_status.last_policy_kind; + if (sub_total_count != 0 && sub_total_count_change != 0 && sub_last_policy_kind != 0) { + sub_event_success = true; + EXPECT_EQ(sub_total_count, 1) << error_msg; + EXPECT_EQ(sub_total_count_change, 1) << error_msg; + EXPECT_EQ(sub_last_policy_kind, qos_policy_kind) << error_msg; + } + + if (pub_event_success && sub_event_success) { + break; + } + std::this_thread::sleep_for(wait_period); + } + EXPECT_TRUE(pub_event_success) << "Publisher incompatible qos event timed out for: " + + error_msg; + EXPECT_TRUE(sub_event_success) << "Subscription incompatible qos event timed out for: " + + error_msg; + // clean up + tear_down_publisher_subscriber(); +} + +static +std::array +get_test_pubsub_incompatible_qos_inputs() +{ + // an array of tuples that holds the expected qos_policy_kind, publisher qos profile, // subscription qos profile and the error message, in that order. - std::vector> input; + std::array inputs; + // durability - rmw_qos_profile_t pub_qos_profile = default_qos_profile; - rmw_qos_profile_t sub_qos_profile = default_qos_profile; + rmw_qos_profile_t pub_qos_profile = TestEventFixture::default_qos_profile; + rmw_qos_profile_t sub_qos_profile = TestEventFixture::default_qos_profile; pub_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; sub_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - input.push_back(std::make_tuple( - RMW_QOS_POLICY_DURABILITY, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos durability")); + inputs[0] = std::make_tuple( + RMW_QOS_POLICY_DURABILITY, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos durability"); // deadline - pub_qos_profile = default_qos_profile; + pub_qos_profile = TestEventFixture::default_qos_profile; pub_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; - sub_qos_profile = default_qos_profile; + sub_qos_profile = TestEventFixture::default_qos_profile; sub_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0}; - input.push_back(std::make_tuple( - RMW_QOS_POLICY_DEADLINE, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos deadline")); + inputs[1] = std::make_tuple( + RMW_QOS_POLICY_DEADLINE, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos deadline"); // liveliness - pub_qos_profile = default_qos_profile; + pub_qos_profile = TestEventFixture::default_qos_profile; pub_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; - sub_qos_profile = default_qos_profile; + sub_qos_profile = TestEventFixture::default_qos_profile; sub_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - input.push_back(std::make_tuple( - RMW_QOS_POLICY_LIVELINESS, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos liveliness policy")); + inputs[2] = std::make_tuple( + RMW_QOS_POLICY_LIVELINESS, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos liveliness policy"); // liveliness lease duration - pub_qos_profile = default_qos_profile; + pub_qos_profile = TestEventFixture::default_qos_profile; pub_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; - sub_qos_profile = default_qos_profile; + sub_qos_profile = TestEventFixture::default_qos_profile; sub_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count(), 0}; - input.push_back(std::make_tuple( - RMW_QOS_POLICY_LIVELINESS, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos liveliness_lease_duration")); + inputs[3] = std::make_tuple( + RMW_QOS_POLICY_LIVELINESS, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos liveliness_lease_duration"); // reliability - pub_qos_profile = default_qos_profile; + pub_qos_profile = TestEventFixture::default_qos_profile; pub_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - sub_qos_profile = default_qos_profile; + sub_qos_profile = TestEventFixture::default_qos_profile; sub_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - input.push_back(std::make_tuple( - RMW_QOS_POLICY_RELIABILITY, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos reliability")); - - rcl_ret_t ret; + inputs[4] = std::make_tuple( + RMW_QOS_POLICY_RELIABILITY, + pub_qos_profile, + sub_qos_profile, + "Incompatible qos reliability"); - for (const auto & element : input) { - const auto & qos_policy_kind = std::get<0>(element); - const auto & publisher_qos_profile = std::get<1>(element); - const auto & subscription_qos_profile = std::get<2>(element); - const auto & error_msg = std::get<3>(element); - setup_publisher_and_subscriber( - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, - publisher_qos_profile, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, - subscription_qos_profile); - - // try to receive event, retry for 10s - static const size_t max_iterations = 1000; - static const auto wait_period = 10ms; - bool pub_event_success = false; - bool sub_event_success = false; - for (size_t i = 0; i < max_iterations; ++i) { - rmw_requested_qos_incompatible_event_status_t requested_incompatible_qos_status; - ret = rcl_take_event(&subscription_event, &requested_incompatible_qos_status); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - const auto & pub_total_count = requested_incompatible_qos_status.total_count; - const auto & pub_total_count_change = requested_incompatible_qos_status.total_count_change; - const auto & pub_last_policy_kind = requested_incompatible_qos_status.last_policy_kind; - if (pub_total_count != 0 && pub_total_count_change != 0 && pub_last_policy_kind != 0) { - pub_event_success = true; - EXPECT_EQ(pub_total_count, 1) << error_msg; - EXPECT_EQ(pub_total_count_change, 1) << error_msg; - EXPECT_EQ(pub_last_policy_kind, qos_policy_kind) << error_msg; - } - - rmw_offered_qos_incompatible_event_status_t offered_incompatible_qos_status; - ret = rcl_take_event(&publisher_event, &offered_incompatible_qos_status); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - const auto & sub_total_count = offered_incompatible_qos_status.total_count; - const auto & sub_total_count_change = offered_incompatible_qos_status.total_count_change; - const auto & sub_last_policy_kind = offered_incompatible_qos_status.last_policy_kind; - if (sub_total_count != 0 && sub_total_count_change != 0 && sub_last_policy_kind != 0) { - sub_event_success = true; - EXPECT_EQ(sub_total_count, 1) << error_msg; - EXPECT_EQ(sub_total_count_change, 1) << error_msg; - EXPECT_EQ(sub_last_policy_kind, qos_policy_kind) << error_msg; - } - - if (pub_event_success && sub_event_success) { - break; - } - std::this_thread::sleep_for(wait_period); - } - EXPECT_TRUE(pub_event_success) << "Publisher incompatible qos event timed out for: " + - error_msg; - EXPECT_TRUE(sub_event_success) << "Subscription incompatible qos event timed out for: " + - error_msg; - // clean up - tear_down_publisher_subscriber(); - } + return inputs; } + +INSTANTIATE_TEST_CASE_P( + TestPubSubIncompatibilityWithDifferentQosSettings, + TestEventFixture, + ::testing::ValuesIn(get_test_pubsub_incompatible_qos_inputs()), + TestEventFixture::PrintToStringParamName()); From f5cac5f48c10085d85ba54501a9f4787b7323626 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Wed, 25 Mar 2020 23:15:02 -0700 Subject: [PATCH 8/8] address PR comments Signed-off-by: Miaofei --- rcl/test/rcl/test_events.cpp | 250 +++++++++++++++++++---------------- 1 file changed, 138 insertions(+), 112 deletions(-) diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index 38bda5164..5f68a1c43 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -48,8 +48,14 @@ constexpr seconds MAX_WAIT_PER_TESTCASE = 10s; #define EXPECT_OK(varname) EXPECT_EQ(varname, RCL_RET_OK) << rcl_get_error_string().str -using TestIncompatibleQosEventParams = std::tuple; +struct TestIncompatibleQosEventParams +{ + std::string testcase_name; + rmw_qos_policy_kind_t qos_policy_kind; + rmw_qos_profile_t publisher_qos_profile; + rmw_qos_profile_t subscription_qos_profile; + std::string error_msg; +}; class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::TestWithParam @@ -57,7 +63,7 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) public: void SetUp() { - is_opensplice = (std::string(rmw_get_implementation_identifier()).find("rmw_opensplice") == 0); + is_fastrtps = (std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0); rcl_ret_t ret; { @@ -110,41 +116,44 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) &subscription_options); } - void setup_publisher_and_subscriber( - const rcl_publisher_event_type_t & pub_event_type, + void setup_publisher_subscriber( const rmw_qos_profile_t pub_qos_profile, - const rcl_subscription_event_type_t & sub_event_type, const rmw_qos_profile_t sub_qos_profile) { rcl_ret_t ret; + // init publisher ret = setup_publisher(pub_qos_profile); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + // init subscription + ret = setup_subscriber(sub_qos_profile); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + void setup_publisher_subscriber_events( + const rcl_publisher_event_type_t & pub_event_type, + const rcl_subscription_event_type_t & sub_event_type) + { + rcl_ret_t ret; + // init publisher events publisher_event = rcl_get_zero_initialized_event(); ret = rcl_publisher_event_init(&publisher_event, &publisher, pub_event_type); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - // init subscription - ret = setup_subscriber(sub_qos_profile); - ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - // init subscription event subscription_event = rcl_get_zero_initialized_event(); ret = rcl_subscription_event_init(&subscription_event, &subscription, sub_event_type); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } - void setup_publisher_and_subscriber_and_assert_discovery( + void setup_publisher_subscriber_and_events_and_assert_discovery( const rcl_publisher_event_type_t & pub_event_type, const rcl_subscription_event_type_t & sub_event_type) { - setup_publisher_and_subscriber( - pub_event_type, - default_qos_profile, - sub_event_type, - default_qos_profile); + setup_publisher_subscriber(default_qos_profile, default_qos_profile); + setup_publisher_subscriber_events(pub_event_type, sub_event_type); rcl_ret_t ret; // wait for discovery, time out after 10s @@ -171,16 +180,21 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) { rcl_ret_t ret; - ret = rcl_event_fini(&subscription_event); + ret = rcl_subscription_fini(&subscription, this->node_ptr); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - ret = rcl_subscription_fini(&subscription, this->node_ptr); + ret = rcl_publisher_fini(&publisher, this->node_ptr); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } - ret = rcl_event_fini(&publisher_event); + void tear_down_publisher_subscriber_events() + { + rcl_ret_t ret; + + ret = rcl_event_fini(&subscription_event); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - ret = rcl_publisher_fini(&publisher, this->node_ptr); + ret = rcl_event_fini(&publisher_event); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } @@ -206,9 +220,7 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) std::string operator()(const ::testing::TestParamInfo & info) const { const auto & test_params = static_cast(info.param); - std::string error_msg = std::get<3>(test_params); - std::replace(error_msg.begin(), error_msg.end(), ' ', '_'); - return error_msg; + return test_params.testcase_name; } }; @@ -222,7 +234,7 @@ class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) rcl_event_t publisher_event; rcl_subscription_t subscription; rcl_event_t subscription_event; - bool is_opensplice; + bool is_fastrtps; const char * topic = "rcl_test_publisher_subscription_events"; const rosidl_message_type_support_t * ts; }; @@ -373,7 +385,7 @@ conditional_wait_for_msgs_and_events( */ TEST_F(TestEventFixture, test_pubsub_no_deadline_missed) { - setup_publisher_and_subscriber_and_assert_discovery( + setup_publisher_subscriber_and_events_and_assert_discovery( RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); rcl_ret_t ret; @@ -431,6 +443,7 @@ TEST_F(TestEventFixture, test_pubsub_no_deadline_missed) } // clean up + tear_down_publisher_subscriber_events(); tear_down_publisher_subscriber(); } @@ -439,7 +452,7 @@ TEST_F(TestEventFixture, test_pubsub_no_deadline_missed) */ TEST_F(TestEventFixture, test_pubsub_deadline_missed) { - setup_publisher_and_subscriber_and_assert_discovery( + setup_publisher_subscriber_and_events_and_assert_discovery( RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); rcl_ret_t ret; @@ -505,6 +518,7 @@ TEST_F(TestEventFixture, test_pubsub_deadline_missed) } // clean up + tear_down_publisher_subscriber_events(); tear_down_publisher_subscriber(); } @@ -513,7 +527,7 @@ TEST_F(TestEventFixture, test_pubsub_deadline_missed) */ TEST_F(TestEventFixture, test_pubsub_liveliness_kill_pub) { - setup_publisher_and_subscriber_and_assert_discovery( + setup_publisher_subscriber_and_events_and_assert_discovery( RCL_PUBLISHER_LIVELINESS_LOST, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); rcl_ret_t ret; @@ -567,13 +581,7 @@ TEST_F(TestEventFixture, test_pubsub_liveliness_kill_pub) ret = rcl_take_event(&subscription_event, &liveliness_status); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_EQ(liveliness_status.alive_count, 0); - // TODO(mm3188): Connext and OpenSplice seem to be tracking alive_count_change differently. - // Issue has been raised at https://github.com/ADLINK-IST/opensplice/issues/88 - if (is_opensplice) { - EXPECT_EQ(liveliness_status.alive_count_change, 2); - } else { - EXPECT_EQ(liveliness_status.alive_count_change, 0); - } + EXPECT_EQ(liveliness_status.alive_count_change, 0); EXPECT_EQ(liveliness_status.not_alive_count, 1); EXPECT_EQ(liveliness_status.not_alive_count_change, 1); } @@ -589,6 +597,7 @@ TEST_F(TestEventFixture, test_pubsub_liveliness_kill_pub) } // clean up + tear_down_publisher_subscriber_events(); tear_down_publisher_subscriber(); } @@ -598,65 +607,92 @@ TEST_F(TestEventFixture, test_pubsub_liveliness_kill_pub) */ TEST_P(TestEventFixture, test_pubsub_incompatible_qos) { - if (!is_opensplice) { - GTEST_SKIP(); - } - const auto & input = GetParam(); - const auto & qos_policy_kind = std::get<0>(input); - const auto & publisher_qos_profile = std::get<1>(input); - const auto & subscription_qos_profile = std::get<2>(input); - const auto & error_msg = std::get<3>(input); - setup_publisher_and_subscriber( - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, - publisher_qos_profile, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, - subscription_qos_profile); + const auto & qos_policy_kind = input.qos_policy_kind; + const auto & publisher_qos_profile = input.publisher_qos_profile; + const auto & subscription_qos_profile = input.subscription_qos_profile; + const auto & error_msg = input.error_msg; - rcl_ret_t ret; + setup_publisher_subscriber(publisher_qos_profile, subscription_qos_profile); + if (is_fastrtps) { + rcl_ret_t ret; + + // init publisher events + publisher_event = rcl_get_zero_initialized_event(); + ret = rcl_publisher_event_init( + &publisher_event, + &publisher, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + EXPECT_EQ(ret, RCL_RET_UNSUPPORTED); + + // init subscription event + subscription_event = rcl_get_zero_initialized_event(); + ret = rcl_subscription_event_init( + &subscription_event, + &subscription, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + EXPECT_EQ(ret, RCL_RET_UNSUPPORTED); + + // clean up and exit test early + tear_down_publisher_subscriber(); + return; + } else { + setup_publisher_subscriber_events( + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } + + WaitConditionPredicate events_ready = []( + const bool & /*msg_persist_ready*/, + const bool & subscription_persist_ready, + const bool & publisher_persist_ready) { + return subscription_persist_ready && publisher_persist_ready; + }; + bool msg_persist_ready, subscription_persist_ready, publisher_persist_ready; + rcl_ret_t wait_res = conditional_wait_for_msgs_and_events( + context_ptr, MAX_WAIT_PER_TESTCASE, events_ready, + &subscription, &subscription_event, &publisher_event, + &msg_persist_ready, &subscription_persist_ready, &publisher_persist_ready); + EXPECT_EQ(wait_res, RCL_RET_OK); - // try to receive event, retry for 10s - static const size_t max_iterations = 1000; - static const auto wait_period = 10ms; - bool pub_event_success = false; - bool sub_event_success = false; - for (size_t i = 0; i < max_iterations; ++i) { + // test that the subscriber/datareader discovered an incompatible publisher/datawriter + EXPECT_TRUE(subscription_persist_ready); + if (subscription_persist_ready) { rmw_requested_qos_incompatible_event_status_t requested_incompatible_qos_status; - ret = rcl_take_event(&subscription_event, &requested_incompatible_qos_status); + rcl_ret_t ret = rcl_take_event(&subscription_event, &requested_incompatible_qos_status); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; const auto & pub_total_count = requested_incompatible_qos_status.total_count; const auto & pub_total_count_change = requested_incompatible_qos_status.total_count_change; const auto & pub_last_policy_kind = requested_incompatible_qos_status.last_policy_kind; if (pub_total_count != 0 && pub_total_count_change != 0 && pub_last_policy_kind != 0) { - pub_event_success = true; EXPECT_EQ(pub_total_count, 1) << error_msg; EXPECT_EQ(pub_total_count_change, 1) << error_msg; EXPECT_EQ(pub_last_policy_kind, qos_policy_kind) << error_msg; + } else { + ADD_FAILURE() << "Subscription incompatible qos event timed out for: " << error_msg; } + } + // test that the publisher/datawriter discovered an incompatible subscription/datareader + EXPECT_TRUE(publisher_persist_ready); + if (publisher_persist_ready) { rmw_offered_qos_incompatible_event_status_t offered_incompatible_qos_status; - ret = rcl_take_event(&publisher_event, &offered_incompatible_qos_status); + rcl_ret_t ret = rcl_take_event(&publisher_event, &offered_incompatible_qos_status); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; const auto & sub_total_count = offered_incompatible_qos_status.total_count; const auto & sub_total_count_change = offered_incompatible_qos_status.total_count_change; const auto & sub_last_policy_kind = offered_incompatible_qos_status.last_policy_kind; if (sub_total_count != 0 && sub_total_count_change != 0 && sub_last_policy_kind != 0) { - sub_event_success = true; EXPECT_EQ(sub_total_count, 1) << error_msg; EXPECT_EQ(sub_total_count_change, 1) << error_msg; EXPECT_EQ(sub_last_policy_kind, qos_policy_kind) << error_msg; + } else { + ADD_FAILURE() << "Publisher incompatible qos event timed out for: " << error_msg; } - - if (pub_event_success && sub_event_success) { - break; - } - std::this_thread::sleep_for(wait_period); } - EXPECT_TRUE(pub_event_success) << "Publisher incompatible qos event timed out for: " + - error_msg; - EXPECT_TRUE(sub_event_success) << "Subscription incompatible qos event timed out for: " + - error_msg; + // clean up + tear_down_publisher_subscriber_events(); tear_down_publisher_subscriber(); } @@ -669,59 +705,49 @@ get_test_pubsub_incompatible_qos_inputs() std::array inputs; // durability - rmw_qos_profile_t pub_qos_profile = TestEventFixture::default_qos_profile; - rmw_qos_profile_t sub_qos_profile = TestEventFixture::default_qos_profile; - pub_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; - sub_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - inputs[0] = std::make_tuple( - RMW_QOS_POLICY_DURABILITY, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos durability"); + inputs[0].testcase_name = "IncompatibleQoS_Durability"; + inputs[0].qos_policy_kind = RMW_QOS_POLICY_DURABILITY; + inputs[0].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[0].publisher_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + inputs[0].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[0].subscription_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + inputs[0].error_msg = "Incompatible qos durability"; // deadline - pub_qos_profile = TestEventFixture::default_qos_profile; - pub_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; - sub_qos_profile = TestEventFixture::default_qos_profile; - sub_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0}; - inputs[1] = std::make_tuple( - RMW_QOS_POLICY_DEADLINE, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos deadline"); + inputs[1].testcase_name = "IncompatibleQoS_Deadline"; + inputs[1].qos_policy_kind = RMW_QOS_POLICY_DEADLINE; + inputs[1].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[1].publisher_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; + inputs[1].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[1].subscription_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0}; + inputs[1].error_msg = "Incompatible qos deadline"; // liveliness - pub_qos_profile = TestEventFixture::default_qos_profile; - pub_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; - sub_qos_profile = TestEventFixture::default_qos_profile; - sub_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - inputs[2] = std::make_tuple( - RMW_QOS_POLICY_LIVELINESS, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos liveliness policy"); + inputs[2].testcase_name = "IncompatibleQoS_LivelinessPolicy"; + inputs[2].qos_policy_kind = RMW_QOS_POLICY_LIVELINESS; + inputs[2].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[2].publisher_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; + inputs[2].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[2].subscription_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + inputs[2].error_msg = "Incompatible qos liveliness policy"; // liveliness lease duration - pub_qos_profile = TestEventFixture::default_qos_profile; - pub_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; - sub_qos_profile = TestEventFixture::default_qos_profile; - sub_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count(), 0}; - inputs[3] = std::make_tuple( - RMW_QOS_POLICY_LIVELINESS, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos liveliness_lease_duration"); + inputs[3].testcase_name = "IncompatibleQoS_LivelinessLeaseDuration"; + inputs[3].qos_policy_kind = RMW_QOS_POLICY_LIVELINESS; + inputs[3].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[3].publisher_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; + inputs[3].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[3].subscription_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count(), 0}; + inputs[3].error_msg = "Incompatible qos liveliness lease duration"; // reliability - pub_qos_profile = TestEventFixture::default_qos_profile; - pub_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - sub_qos_profile = TestEventFixture::default_qos_profile; - sub_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - inputs[4] = std::make_tuple( - RMW_QOS_POLICY_RELIABILITY, - pub_qos_profile, - sub_qos_profile, - "Incompatible qos reliability"); + inputs[4].testcase_name = "IncompatibleQoS_Reliability"; + inputs[4].qos_policy_kind = RMW_QOS_POLICY_RELIABILITY; + inputs[4].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[4].publisher_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + inputs[4].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[4].subscription_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + inputs[4].error_msg = "Incompatible qos reliability"; return inputs; }