Skip to content

Commit

Permalink
Omnibus fixes for running tests with Connext.
Browse files Browse the repository at this point in the history
When running the tests with RTI Connext as the default
RMW, some of the tests are failing.  There are three
different failures fixed here:

1.  Setting the liveliness duration to a value smaller than
a microsecond causes Connext to throw an error.  Set it to
a millisecond.

2.  Using the SystemDefaultsQoS sets the QoS to KEEP_LAST 1.
Connext is somewhat slow in this regard, so it can be the case
that we are overwriting a previous service introspection event
with the next one.  Switch to the ServicesDefaultQoS in the test,
which ensures we will not lose events.

3.  Connext is slow to match publishers and subscriptions.  Thus,
when creating a subscription "on-the-fly", we should wait for the
publisher to match it before expecting the subscription to actually
receive data from it.

With these fixes in place, the test_client_common, test_generic_service,
test_service_introspection, and test_executors tests all pass for
me with rmw_connextdds.

Signed-off-by: Chris Lalancette <[email protected]>
  • Loading branch information
clalancette committed Nov 22, 2024
1 parent 64dd644 commit 54455b3
Show file tree
Hide file tree
Showing 4 changed files with 32 additions and 20 deletions.
12 changes: 12 additions & 0 deletions rclcpp/test/rclcpp/executors/test_executors.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -714,6 +714,12 @@ TYPED_TEST(TestExecutors, notifyTwiceWhileSpinning)
sub1_msg_count++;
});

// Wait for the subscription to be matched
size_t tries = 1000;
while (this->publisher->get_subscription_count() < 1 && tries > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}

// Publish a message and verify it's received
this->publisher->publish(test_msgs::msg::Empty());
auto start = std::chrono::steady_clock::now();
Expand All @@ -731,6 +737,12 @@ TYPED_TEST(TestExecutors, notifyTwiceWhileSpinning)
sub2_msg_count++;
});

// Wait for the subscription to be matched
tries = 1000;
while (this->publisher->get_subscription_count() < 2 && tries > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}

// Publish a message and verify it's received by both subscriptions
this->publisher->publish(test_msgs::msg::Empty());
start = std::chrono::steady_clock::now();
Expand Down
2 changes: 1 addition & 1 deletion rclcpp/test/rclcpp/test_client_common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -431,7 +431,7 @@ TYPED_TEST(TestAllClientTypesWithServer, client_qos)

rclcpp::ServicesQoS qos_profile;
qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic);
rclcpp::Duration duration(std::chrono::nanoseconds(1));
rclcpp::Duration duration(std::chrono::milliseconds(1));
qos_profile.deadline(duration);
qos_profile.lifespan(duration);
qos_profile.liveliness_lease_duration(duration);
Expand Down
2 changes: 1 addition & 1 deletion rclcpp/test/rclcpp/test_generic_service.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -320,7 +320,7 @@ TEST_F(TestGenericService, rcl_service_request_subscription_get_actual_qos_error
TEST_F(TestGenericService, generic_service_qos) {
rclcpp::ServicesQoS qos_profile;
qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic);
rclcpp::Duration duration(std::chrono::nanoseconds(1));
rclcpp::Duration duration(std::chrono::milliseconds(1));
qos_profile.deadline(duration);
qos_profile.lifespan(duration);
qos_profile.liveliness_lease_duration(duration);
Expand Down
36 changes: 18 additions & 18 deletions rclcpp/test/rclcpp/test_service_introspection.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,9 +92,9 @@ TEST_F(TestServiceIntrospection, service_introspection_nominal)
request->set__int64_value(42);

client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);

auto future = client->async_send_request(request);
ASSERT_EQ(
Expand Down Expand Up @@ -163,9 +163,9 @@ TEST_F(TestServiceIntrospection, service_introspection_nominal)
TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events)
{
client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_OFF);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_OFF);

auto request = std::make_shared<BasicTypes::Request>();
request->set__bool_value(true);
Expand All @@ -183,9 +183,9 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events)
events.clear();

client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_OFF);

future = client->async_send_request(request);
ASSERT_EQ(
Expand All @@ -200,9 +200,9 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events)
events.clear();

client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_OFF);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_METADATA);

future = client->async_send_request(request);
ASSERT_EQ(
Expand All @@ -217,9 +217,9 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events)
events.clear();

client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_METADATA);

future = client->async_send_request(request);
ASSERT_EQ(
Expand All @@ -235,9 +235,9 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events)
TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_content)
{
client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_METADATA);

auto request = std::make_shared<BasicTypes::Request>();
request->set__bool_value(true);
Expand All @@ -259,9 +259,9 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_cont
events.clear();

client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_METADATA);

future = client->async_send_request(request);
ASSERT_EQ(
Expand Down Expand Up @@ -292,9 +292,9 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_cont
events.clear();

client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);

future = client->async_send_request(request);
ASSERT_EQ(
Expand Down Expand Up @@ -325,9 +325,9 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_cont
events.clear();

client->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
service->configure_introspection(
node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
node->get_clock(), rclcpp::ServicesQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);

future = client->async_send_request(request);
ASSERT_EQ(
Expand Down

0 comments on commit 54455b3

Please sign in to comment.