Cleanup test_count_matched test to handle non-DDS RMWs (ros2#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 <clalancette@gmail.com>

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 <gtest/gtest.h>
 
 #include <chrono>
+#include <cstddef>
 #include <string>
 #include <thread>
 
+#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,47 +85,109 @@ 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();
   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();
 
-  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);
+  }
 }