Omnibus fixes for running tests with Connext. (#2684)

* Omnibus fixes for running tests with Connext.

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]>

* Fixes for executors.

Signed-off-by: Chris Lalancette <[email protected]>

* One more fix for services.

Signed-off-by: Chris Lalancette <[email protected]>

* More fixes for service_introspection.

Signed-off-by: Chris Lalancette <[email protected]>

* More fixes for introspection.

Signed-off-by: Chris Lalancette <[email protected]>

---------

Signed-off-by: Chris Lalancette <[email protected]>
(cherry picked from commit 9984197)

# Conflicts:
#	rclcpp/test/rclcpp/executors/test_executors.cpp
#	rclcpp/test/rclcpp/test_generic_service.cpp

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -701,6 +701,84 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode)
   }
 }
 
+<<<<<<< HEAD
+=======
+// Check that executors are correctly notified while they are spinning
+// we notify twice to ensure that the notify waitable is still working
+// after the first notification
+TYPED_TEST(TestExecutors, notifyTwiceWhileSpinning)
+{
+  using ExecutorType = TypeParam;
+
+  // Create executor, add the node and start spinning
+  ExecutorType executor;
+  executor.add_node(this->node);
+  std::thread spinner([&]() {executor.spin();});
+
+  // Wait for executor to be spinning
+  while (!executor.is_spinning()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+
+  // Create the first subscription while the executor is already spinning
+  std::atomic<size_t> sub1_msg_count {0};
+  auto sub1 = this->node->template create_subscription<test_msgs::msg::Empty>(
+    this->publisher->get_topic_name(),
+    rclcpp::QoS(10),
+    [&sub1_msg_count](test_msgs::msg::Empty::ConstSharedPtr) {
+      sub1_msg_count++;
+    });
+
+  // Wait for the subscription to be matched
+  size_t tries = 10000;
+  while (this->publisher->get_subscription_count() < 2 && tries-- > 0) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+  ASSERT_EQ(this->publisher->get_subscription_count(), 2);
+
+  // Publish a message and verify it's received
+  this->publisher->publish(test_msgs::msg::Empty());
+  auto start = std::chrono::steady_clock::now();
+  while (sub1_msg_count == 0 && (std::chrono::steady_clock::now() - start) < 10s) {
+    std::this_thread::sleep_for(1ms);
+  }
+  EXPECT_EQ(sub1_msg_count, 1u);
+
+  // Create a second subscription while the executor is already spinning
+  std::atomic<size_t> sub2_msg_count {0};
+  auto sub2 = this->node->template create_subscription<test_msgs::msg::Empty>(
+    this->publisher->get_topic_name(),
+    rclcpp::QoS(10),
+    [&sub2_msg_count](test_msgs::msg::Empty::ConstSharedPtr) {
+      sub2_msg_count++;
+    });
+
+  // Wait for the subscription to be matched
+  tries = 10000;
+  while (this->publisher->get_subscription_count() < 3 && tries-- > 0) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+  ASSERT_EQ(this->publisher->get_subscription_count(), 3);
+
+  // Publish a message and verify it's received by both subscriptions
+  this->publisher->publish(test_msgs::msg::Empty());
+  start = std::chrono::steady_clock::now();
+  while (
+    sub1_msg_count == 1 &&
+    sub2_msg_count == 0 &&
+    (std::chrono::steady_clock::now() - start) < 10s)
+  {
+    std::this_thread::sleep_for(1ms);
+  }
+  EXPECT_EQ(sub1_msg_count, 2u);
+  EXPECT_EQ(sub2_msg_count, 1u);
+
+  // Cancel needs to be called before join, so that executor.spin() returns.
+  executor.cancel();
+  spinner.join();
+}
+
+>>>>>>> 9984197c (Omnibus fixes for running tests with Connext. (#2684))
 // Check spin_until_future_complete with node base pointer (instantiates its own executor)
 TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr)
 {

rclcpp/test/rclcpp/test_client_common.cpp

@@ -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);