From 3e7ce7615526009362b9683fc0e6a87105d49a83 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 1 Jul 2024 19:59:46 -0400 Subject: [PATCH] Cleanup test_count_matched test to handle non-DDS RMWs (#1164) * Make check_state a class method in test_count_matched. This allows us to pass fewer parameters into each each invocation, and allows us to hide some more of the implementation inside the class. * Rename "ops" to "opts" in test_count_matched. It just better reflects what these structures are. * Cleanup pub/subs with a scope_exit in test_count_matched. This just ensures that they are always cleaned up, even if we exit early. Note that we specifically do *not* use it for test_count_matched_functions, since the cleanup is intentionally interleaved with other tests. * Check with the RMW layer to see whether QoS is compatible. Some RMWs may have different compatibility than DDS, so check with the RMW layer to see what we should expect for the number of publishers and subscriptions. Signed-off-by: Chris Lalancette --- rcl/test/rcl/test_count_matched.cpp | 254 +++++++++++++++------------- 1 file changed, 137 insertions(+), 117 deletions(-) diff --git a/rcl/test/rcl/test_count_matched.cpp b/rcl/test/rcl/test_count_matched.cpp index 296741900..e751d889b 100644 --- a/rcl/test/rcl/test_count_matched.cpp +++ b/rcl/test/rcl/test_count_matched.cpp @@ -11,12 +11,16 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. + #include #include +#include #include #include +#include "osrf_testing_tools_cpp/scope_exit.hpp" + #include "rcl/rcl.h" #include "rcl/publisher.h" #include "rcl/subscription.h" @@ -28,73 +32,11 @@ #include "rcl/error_handling.h" -void check_state( - rcl_wait_set_t * wait_set_ptr, - rcl_publisher_t * publisher, - rcl_subscription_t * subscriber, - const rcl_guard_condition_t * graph_guard_condition, - size_t expected_subscriber_count, - size_t expected_publisher_count, - size_t number_of_tries) -{ - size_t subscriber_count = -1; - size_t publisher_count = -1; - - rcl_ret_t ret; - - for (size_t i = 0; i < number_of_tries; ++i) { - if (publisher) { - ret = rcl_publisher_get_subscription_count(publisher, &subscriber_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - } - - if (subscriber) { - ret = rcl_subscription_get_publisher_count(subscriber, &publisher_count); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - } - - if ( - expected_publisher_count == publisher_count && - expected_subscriber_count == subscriber_count) - { - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, " state correct!"); - break; - } - - if ((i + 1) == number_of_tries) { - // Don't wait for the graph to change on the last loop because we won't check again. - continue; - } - - ret = rcl_wait_set_clear(wait_set_ptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition, NULL); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); - RCUTILS_LOG_INFO_NAMED( - ROS_PACKAGE_NAME, - " state wrong, waiting up to '%s' nanoseconds for graph changes... ", - std::to_string(time_to_sleep.count()).c_str()); - ret = rcl_wait(wait_set_ptr, time_to_sleep.count()); - if (ret == RCL_RET_TIMEOUT) { - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "timeout"); - continue; - } - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "change occurred"); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } - EXPECT_EQ(expected_publisher_count, publisher_count); - EXPECT_EQ(expected_subscriber_count, subscriber_count); -} - class TestCountFixture : public ::testing::Test { public: rcl_node_t * node_ptr; - rcl_context_t * context_ptr; - rcl_wait_set_t * wait_set_ptr; + void SetUp() { rcl_ret_t ret; @@ -120,6 +62,8 @@ class TestCountFixture : public ::testing::Test ret = rcl_wait_set_init( this->wait_set_ptr, 0, 1, 0, 0, 0, 0, this->context_ptr, rcl_get_default_allocator()); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + graph_guard_condition = rcl_node_get_graph_guard_condition(this->node_ptr); } void TearDown() @@ -141,31 +85,93 @@ class TestCountFixture : public ::testing::Test delete this->context_ptr; EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } + +protected: + const rcl_guard_condition_t * graph_guard_condition; + rcl_context_t * context_ptr; + rcl_wait_set_t * wait_set_ptr; + + void check_state( + rcl_publisher_t * publisher, + rcl_subscription_t * subscriber, + size_t expected_subscriber_count, + size_t expected_publisher_count, + size_t number_of_tries) + { + size_t subscriber_count = -1; + size_t publisher_count = -1; + + rcl_ret_t ret; + + for (size_t i = 0; i < number_of_tries; ++i) { + if (publisher) { + ret = rcl_publisher_get_subscription_count(publisher, &subscriber_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + + if (subscriber) { + ret = rcl_subscription_get_publisher_count(subscriber, &publisher_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + + if ( + expected_publisher_count == publisher_count && + expected_subscriber_count == subscriber_count) + { + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "state correct!"); + break; + } + + if ((i + 1) == number_of_tries) { + // Don't wait for the graph to change on the last loop because we won't check again. + continue; + } + + ret = rcl_wait_set_clear(wait_set_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); + RCUTILS_LOG_INFO_NAMED( + ROS_PACKAGE_NAME, + "state wrong, waiting up to '%s' nanoseconds for graph changes... ", + std::to_string(time_to_sleep.count()).c_str()); + ret = rcl_wait(wait_set_ptr, time_to_sleep.count()); + if (ret == RCL_RET_TIMEOUT) { + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "timeout"); + continue; + } + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "change occurred"); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + EXPECT_EQ(expected_publisher_count, publisher_count); + EXPECT_EQ(expected_subscriber_count, subscriber_count); + } }; -TEST_F(TestCountFixture, test_count_matched_functions) { +TEST_F(TestCountFixture, test_count_matched_functions) +{ std::string topic_name("/test_count_matched_functions__"); rcl_ret_t ret; rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); - rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_opts); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - const rcl_guard_condition_t * graph_guard_condition = - rcl_node_get_graph_guard_condition(this->node_ptr); - - check_state(wait_set_ptr, &pub, nullptr, graph_guard_condition, 0, -1, 9); + check_state(&pub, nullptr, 0, -1, 9); rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); - rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); - ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops); + rcl_subscription_options_t sub_opts = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_opts); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 1, 1, 9); + check_state(&pub, &sub, 1, 1, 9); rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t sub2_ops = rcl_subscription_get_default_options(); @@ -173,15 +179,15 @@ TEST_F(TestCountFixture, test_count_matched_functions) { EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 2, 1, 9); - check_state(wait_set_ptr, &pub, &sub2, graph_guard_condition, 2, 1, 9); + check_state(&pub, &sub, 2, 1, 9); + check_state(&pub, &sub2, 2, 1, 9); ret = rcl_publisher_fini(&pub, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - check_state(wait_set_ptr, nullptr, &sub, graph_guard_condition, -1, 0, 9); - check_state(wait_set_ptr, nullptr, &sub2, graph_guard_condition, -1, 0, 9); + check_state(nullptr, &sub, -1, 0, 9); + check_state(nullptr, &sub2, -1, 0, 9); ret = rcl_subscription_fini(&sub, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -192,66 +198,80 @@ TEST_F(TestCountFixture, test_count_matched_functions) { rcl_reset_error(); } -TEST_F(TestCountFixture, test_count_matched_functions_mismatched_qos) { +TEST_F(TestCountFixture, test_count_matched_functions_mismatched_qos) +{ std::string topic_name("/test_count_matched_functions_mismatched_qos__"); rcl_ret_t ret; rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); - rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); - pub_ops.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - pub_ops.qos.depth = 10; - pub_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - pub_ops.qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; - pub_ops.qos.avoid_ros_namespace_conventions = false; - pub_ops.allocator = rcl_get_default_allocator(); + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + pub_opts.qos.depth = 10; + pub_opts.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + pub_opts.qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + pub_opts.qos.avoid_ros_namespace_conventions = false; + pub_opts.allocator = rcl_get_default_allocator(); auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_opts); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&pub, this->node_ptr)) << rcl_get_error_string().str; + }); - const rcl_guard_condition_t * graph_guard_condition = - rcl_node_get_graph_guard_condition(this->node_ptr); - - check_state(wait_set_ptr, &pub, nullptr, graph_guard_condition, 0, -1, 9); + check_state(&pub, nullptr, 0, -1, 9); rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); - rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); - sub_ops.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - sub_ops.qos.depth = 10; - sub_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - sub_ops.qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; - sub_ops.qos.avoid_ros_namespace_conventions = false; - sub_ops.allocator = rcl_get_default_allocator(); + rcl_subscription_options_t sub_opts = rcl_subscription_get_default_options(); + sub_opts.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + sub_opts.qos.depth = 10; + sub_opts.qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + sub_opts.qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + sub_opts.qos.avoid_ros_namespace_conventions = false; + sub_opts.allocator = rcl_get_default_allocator(); - ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops); + ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_opts); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - - // Expect that no publishers or subscribers should be matched due to qos. - check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 0, 0, 9); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, + rcl_subscription_fini(&sub, this->node_ptr)) << rcl_get_error_string().str; + }); + + rmw_qos_compatibility_type_t compat; + rmw_ret_t rmw_ret = + rmw_qos_profile_check_compatible(pub_opts.qos, sub_opts.qos, &compat, nullptr, 0); + ASSERT_EQ(rmw_ret, RMW_RET_OK); + + if (compat == RMW_QOS_COMPATIBILITY_OK) { + check_state(&pub, &sub, 1, 1, 9); + } else { + // Expect that no publishers or subscribers should be matched due to qos. + check_state(&pub, &sub, 0, 0, 9); + } rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t sub2_ops = rcl_subscription_get_default_options(); ret = rcl_subscription_init(&sub2, this->node_ptr, ts, topic_name.c_str(), &sub2_ops); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - - // Even multiple subscribers should not match - check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 0, 0, 9); - check_state(wait_set_ptr, &pub, &sub2, graph_guard_condition, 0, 0, 9); - - ret = rcl_subscription_fini(&sub, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_subscription_fini(&sub2, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_publisher_fini(&pub, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rcl_reset_error(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, + rcl_subscription_fini(&sub2, this->node_ptr)) << rcl_get_error_string().str; + }); + + if (compat == RMW_QOS_COMPATIBILITY_OK) { + check_state(&pub, &sub, 2, 1, 9); + check_state(&pub, &sub2, 2, 1, 9); + } else { + // Even multiple subscribers should not match + check_state(&pub, &sub, 0, 0, 9); + check_state(&pub, &sub2, 0, 0, 9); + } }