avoid adding notify waitable twice to events-executor collection (ros2#2564)

* avoid adding notify waitable twice to events-executor entities collection

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>

* remove redundant mutex lock

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>

---------

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>
(cherry picked from commit f27bdbf)

Author: alsora

rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp

@@ -432,14 +432,11 @@ EventsExecutor::refresh_current_collection_from_callback_groups()
   // We could explicitly check for the notify waitable ID when we receive a waitable event
   // but I think that it's better if the waitable was in the collection and it could be
   // retrieved in the "standard" way.
-  // To do it, we need to add the notify waitable as an entry in both the new and
-  // current collections such that it's neither added or removed.
+  // To do it, we need to add the notify waitable as an entry in the new collection
+  // such that it's neither added or removed (it should have already been added
+  // to the current collection in the constructor)
   this->add_notify_waitable_to_collection(new_collection.waitables);
 
-  // Acquire lock before modifying the current collection
-  std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
-  this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
-
   this->refresh_current_collection(new_collection);
 }
 

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -694,6 +694,121 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode)
   }
 }
 
+// This test verifies the thread-safety of adding and removing a node
+// while the executor is spinning and events are ready.
+// This test does not contain expectations, but rather it verifies that
+// we can run a "stressful routine" without crashing.
+TYPED_TEST(TestExecutors, stressAddRemoveNode)
+{
+  using ExecutorType = TypeParam;
+  // rmw_connextdds doesn't support events-executor
+  if (
+    std::is_same<ExecutorType, rclcpp::experimental::executors::EventsExecutor>() &&
+    std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0)
+  {
+    GTEST_SKIP();
+  }
+
+  ExecutorType executor;
+
+  // A timer that is "always" ready (the timer callback doesn't do anything)
+  auto timer = this->node->create_wall_timer(std::chrono::nanoseconds(1), []() {});
+
+  // This thread spins the executor until it's cancelled
+  std::thread spinner_thread([&]() {
+      executor.spin();
+    });
+
+  // This thread publishes data in a busy loop (the node has a subscription)
+  std::thread publisher_thread1([&]() {
+      for (size_t i = 0; i < 100000; i++) {
+        this->publisher->publish(test_msgs::msg::Empty());
+      }
+    });
+  std::thread publisher_thread2([&]() {
+      for (size_t i = 0; i < 100000; i++) {
+        this->publisher->publish(test_msgs::msg::Empty());
+      }
+    });
+
+  // This thread adds/remove the node that contains the entities in a busy loop
+  std::thread add_remove_thread([&]() {
+      for (size_t i = 0; i < 100000; i++) {
+        executor.add_node(this->node);
+        executor.remove_node(this->node);
+      }
+    });
+
+  // Wait for the threads that do real work to finish
+  publisher_thread1.join();
+  publisher_thread2.join();
+  add_remove_thread.join();
+
+  executor.cancel();
+  spinner_thread.join();
+}
+
+// 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++;
+    });
+
+  // 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++;
+    });
+
+  // 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();
+}
+
 // Check spin_until_future_complete with node base pointer (instantiates its own executor)
 TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr)
 {