Skip to content

Commit

Permalink
Cleanup test_count_matched test to handle non-DDS RMWs (#1164)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>
  • Loading branch information
@clalancette
clalancette authored Jul 1, 2024
1 parent 2b159af commit 3e7ce76
Showing 1 changed file with 137 additions and 117 deletions.
254 changes: 137 additions & 117 deletions rcl/test/rcl/test_count_matched.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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;
Expand All @@ -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()
Expand All @@ -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;
Expand All @@ -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);
}
}

0 comments on commit 3e7ce76

Please sign in to comment.