Added test cased for missing events & double call to take_data

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -41,6 +41,32 @@
 
 using namespace std::chrono_literals;
 
+
+template<typename T>
+class TestExecutorsOnlyNode : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+
+    const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
+    std::stringstream test_name;
+    test_name << test_info->test_case_name() << "_" << test_info->name();
+    node = std::make_shared<rclcpp::Node>("node", test_name.str());
+
+  }
+
+  void TearDown()
+  {
+    node.reset();
+
+    rclcpp::shutdown();
+  }
+
+  rclcpp::Node::SharedPtr node;
+};
+
 template<typename T>
 class TestExecutors : public ::testing::Test
 {
@@ -122,6 +148,8 @@ class ExecutorTypeNames
 // is updated.
 TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames);
 
+TYPED_TEST_SUITE(TestExecutorsOnlyNode, ExecutorTypes, ExecutorTypeNames);
+
 // StaticSingleThreadedExecutor is not included in these tests for now, due to:
 // https://github.com/ros2/rclcpp/issues/1219
 using StandardExecutors =
@@ -392,13 +420,24 @@ class TestWaitable : public rclcpp::Waitable
   bool
   is_ready(rcl_wait_set_t * wait_set) override
   {
-    (void)wait_set;
-    return true;
+    for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) {
+      if (&gc_.get_rcl_guard_condition() == wait_set->guard_conditions[i]) {
+        is_ready_called_before_take_data = true;
+        return true;
+      }
+    }
+    return false;
   }
 
   std::shared_ptr<void>
   take_data() override
   {
+    if (!is_ready_called_before_take_data) {
+      throw std::runtime_error(
+              "TestWaitable : Internal error, take data was called, but is_ready was not called before");
+    }
+
+    is_ready_called_before_take_data = false;
     return nullptr;
   }
 
@@ -442,10 +481,12 @@ class TestWaitable : public rclcpp::Waitable
   }
 
 private:
+  bool is_ready_called_before_take_data = false;
   size_t count_ = 0;
   rclcpp::GuardCondition gc_;
 };
 
+
 TYPED_TEST(TestExecutors, spinAll)
 {
   using ExecutorType = TypeParam;
@@ -488,6 +529,189 @@ TYPED_TEST(TestExecutors, spinAll)
   spinner.join();
 }
 
+TEST(TestExecutorsOnlyNode, double_take_data)
+{
+  rclcpp::init(0, nullptr);
+
+  const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
+  std::stringstream test_name;
+  test_name << test_info->test_case_name() << "_" << test_info->name();
+  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("node", test_name.str());
+
+  class MyExecutor : public rclcpp::executors::SingleThreadedExecutor
+  {
+public:
+    /**
+     * This is a copy of Executor::get_next_executable with a callback, to test
+     * for a special race condition
+     */
+    bool get_next_executable_with_callback(
+      rclcpp::AnyExecutable & any_executable,
+      std::chrono::nanoseconds timeout,
+      std::function<void(void)> inbetween)
+    {
+      bool success = false;
+      // Check to see if there are any subscriptions or timers needing service
+      // TODO(wjwwood): improve run to run efficiency of this function
+      success = get_next_ready_executable(any_executable);
+      // If there are none
+      if (!success) {
+
+        inbetween();
+
+        // Wait for subscriptions or timers to work on
+        wait_for_work(timeout);
+        if (!spinning.load()) {
+          return false;
+        }
+        // Try again
+        success = get_next_ready_executable(any_executable);
+      }
+      return success;
+    }
+
+    void spin_once_with_callback(
+      std::chrono::nanoseconds timeout,
+      std::function<void(void)> inbetween)
+    {
+      rclcpp::AnyExecutable any_exec;
+      if (get_next_executable_with_callback(any_exec, timeout, inbetween)) {
+        execute_any_executable(any_exec);
+      }
+    }
+
+  };
+
+  MyExecutor executor;
+
+  auto callback_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    true);
+
+  std::vector<std::shared_ptr<TestWaitable>> waitables;
+
+  auto waitable_interfaces = node->get_node_waitables_interface();
+
+  for (int i = 0; i < 3; i++) {
+    auto waitable = std::make_shared<TestWaitable>();
+    waitables.push_back(waitable);
+    waitable_interfaces->add_waitable(waitable, callback_group);
+  }
+  executor.add_node(node);
+
+  for (auto & waitable : waitables) {
+    waitable->trigger();
+  }
+
+  // a node has some default subscribers, that need to get executed first, therefore the loop
+  for (int i = 0; i < 10; i++) {
+    executor.spin_once(std::chrono::milliseconds(10));
+    if (waitables.front()->get_count() > 0) {
+      // stop execution, after the first waitable has been executed
+      break;
+    }
+  }
+
+  EXPECT_EQ(waitables.front()->get_count(), 1);
+
+  // block the callback group, this is something that may happen during multi threaded execution
+  // This removes my_waitable2 from the list of ready events, and triggers a call to wait_for_work
+  callback_group->can_be_taken_from().exchange(false);
+
+  bool no_ready_executable = false;
+
+  //now there should be no ready events now,
+  executor.spin_once_with_callback(
+    std::chrono::milliseconds(10), [&]() {
+      no_ready_executable = true;
+    });
+
+  EXPECT_TRUE(no_ready_executable);
+
+  //rearm, so that rmw_wait will push a second entry into the queue
+  for (auto & waitable : waitables) {
+    waitable->trigger();
+  }
+
+  no_ready_executable = false;
+
+  while (!no_ready_executable) {
+    executor.spin_once_with_callback(
+      std::chrono::milliseconds(10), [&]() {
+        //unblock the callback group
+        callback_group->can_be_taken_from().exchange(true);
+
+        no_ready_executable = true;
+
+      });
+  }
+  EXPECT_TRUE(no_ready_executable);
+
+  // now we process all events from get_next_ready_executable
+  EXPECT_NO_THROW(
+    for (int i = 0; i < 10; i++) {
+    executor.spin_once(std::chrono::milliseconds(1));
+  }
+  );
+
+  node.reset();
+
+  rclcpp::shutdown();
+}
+
+
+TYPED_TEST(TestExecutorsOnlyNode, missing_event)
+{
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  rclcpp::Node::SharedPtr node(this->node);
+  auto callback_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    true);
+
+  auto waitable_interfaces = node->get_node_waitables_interface();
+  auto my_waitable = std::make_shared<TestWaitable>();
+  auto my_waitable2 = std::make_shared<TestWaitable>();
+  waitable_interfaces->add_waitable(my_waitable, callback_group);
+  waitable_interfaces->add_waitable(my_waitable2, callback_group);
+  executor.add_node(this->node);
+
+  my_waitable->trigger();
+  my_waitable2->trigger();
+
+  // a node has some default subscribers, that need to get executed first, therefore the loop
+  for (int i = 0; i < 10; i++) {
+    executor.spin_once(std::chrono::milliseconds(10));
+    if (my_waitable->get_count() > 0) {
+      // stop execution, after the first waitable has been executed
+      break;
+    }
+  }
+
+  EXPECT_EQ(1u, my_waitable->get_count());
+  EXPECT_EQ(0u, my_waitable2->get_count());
+
+  // block the callback group, this is something that may happen during multi threaded execution
+  // This removes my_waitable2 from the list of ready events, and triggers a call to wait_for_work
+  callback_group->can_be_taken_from().exchange(false);
+
+  //now there should be no ready event
+  executor.spin_once(std::chrono::milliseconds(10));
+
+  EXPECT_EQ(1u, my_waitable->get_count());
+  EXPECT_EQ(0u, my_waitable2->get_count());
+
+  //unblock the callback group
+  callback_group->can_be_taken_from().exchange(true);
+
+  //now the second waitable should get processed
+  executor.spin_once(std::chrono::milliseconds(10));
+
+  EXPECT_EQ(1u, my_waitable->get_count());
+  EXPECT_EQ(1u, my_waitable2->get_count());
+}
+
 TYPED_TEST(TestExecutors, spinSome)
 {
   using ExecutorType = TypeParam;