executor could take more than once incorrectly

rclcpp/CMakeLists.txt

@@ -387,6 +387,14 @@ if(BUILD_TESTING)
       "rcl")
     target_link_libraries(test_utilities ${PROJECT_NAME})
   endif()
+
+  ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_multi_threaded_executor)
+    ament_target_dependencies(test_multi_threaded_executor
+      "rcl")
+    target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package(

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -15,7 +15,9 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
+#include <memory>
 #include <mutex>
+#include <set>
 #include <thread>
 #include <unordered_map>
 
@@ -34,10 +36,22 @@ class MultiThreadedExecutor : public executor::Executor
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
 
+  /// Constructor for MultiThreadedExecutor.
+  /**
+   * For the yield_before_execute option, when true std::this_thread::yield()
+   * will be called after acquiring work (as an AnyExecutable) and after
+   * releasing the spinning lock, but before executing the work.
+   * This is useful for reproducing some bugs related to taking work more than
+   * once.
+   *
+   * \param args common arguments for all executors
+   * \param yield_before_execute if true std::this_thread::yield() is called
+   */
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
     const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments(),
-    size_t number_of_threads = 0);
+    size_t number_of_threads = 0,
+    bool yield_before_execute = false);
 
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();
@@ -60,6 +74,10 @@ class MultiThreadedExecutor : public executor::Executor
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;
+  bool yield_before_execute_;
+
+  std::mutex scheduled_mutex_;
+  std::set<std::shared_ptr<executor::AnyExecutable>> scheduled_;
 };
 
 }  // namespace executors

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -16,6 +16,7 @@
 
 #include <chrono>
 #include <functional>
+#include <memory>
 #include <vector>
 
 #include "rclcpp/utilities.hpp"
@@ -25,8 +26,9 @@ using rclcpp::executors::MultiThreadedExecutor;
 
 MultiThreadedExecutor::MultiThreadedExecutor(
   const rclcpp::executor::ExecutorArgs & args,
-  size_t number_of_threads)
-: executor::Executor(args)
+  size_t number_of_threads,
+  bool yield_before_execute)
+: executor::Executor(args), yield_before_execute_(yield_before_execute)
 {
   number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
   if (number_of_threads_ == 0) {
@@ -69,16 +71,33 @@ void
 MultiThreadedExecutor::run(size_t)
 {
   while (rclcpp::ok() && spinning.load()) {
-    executor::AnyExecutable any_exec;
+    auto any_exec = std::make_shared<executor::AnyExecutable>();
     {
       std::lock_guard<std::mutex> wait_lock(wait_mutex_);
       if (!rclcpp::ok() || !spinning.load()) {
         return;
       }
-      if (!get_next_executable(any_exec)) {
+      if (!get_next_executable(*any_exec)) {
         continue;
       }
+      {
+        std::lock_guard<std::mutex> lock(scheduled_mutex_);
+        if (scheduled_.count(any_exec) != 0) {
+          continue;
+        }
+        scheduled_.insert(any_exec);
+      }
+    }
+    if (yield_before_execute_) {
+      std::this_thread::yield();
+    }
+    execute_any_executable(*any_exec);
+    {
+      std::lock_guard<std::mutex> lock(scheduled_mutex_);
+      auto it = scheduled_.find(any_exec);
+      if (it != scheduled_.end()) {
+        scheduled_.erase(it);
+      }
     }
-    execute_any_executable(any_exec);
   }
 }

rclcpp/test/executors/test_multi_threaded_executor.cpp

@@ -0,0 +1,91 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// 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 <string>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executors.hpp"
+
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+
+using namespace std::chrono_literals;
+
+using rcl_interfaces::msg::IntraProcessMessage;
+
+class TestMultiThreadedExecutor : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+/*
+   Test that timers are not taken multiple times when using reentrant callback groups.
+ */
+TEST_F(TestMultiThreadedExecutor, timer_over_take) {
+  struct sched_param param;
+  param.sched_priority = 0;
+  if (sched_setscheduler(0, SCHED_BATCH, &param) != 0) {
+    perror("sched_setscheduler");
+  }
+
+  rclcpp::executors::MultiThreadedExecutor executor(
+    rclcpp::executor::create_default_executor_arguments(), 0, true);
+
+  ASSERT_GT(executor.get_number_of_threads(), 1u);
+
+  std::shared_ptr<rclcpp::Node> node =
+    std::make_shared<rclcpp::Node>("test_multi_threaded_executor_timer_over_take");
+
+  auto cbg = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant);
+
+  rclcpp::Clock system_clock(RCL_STEADY_TIME);
+  std::mutex last_mutex;
+  auto last = system_clock.now();
+
+  std::atomic_int timer_count {0};
+
+  auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() {
+      rclcpp::Time now = system_clock.now();
+      timer_count++;
+
+      if (timer_count > 20) {
+        executor.cancel();
+      }
+
+      {
+        std::lock_guard<std::mutex> lock(last_mutex);
+        double diff = labs((now - last).nanoseconds()) / 1.0e9;
+        last = now;
+
+        if (diff < 0.009 || diff > 0.011) {
+          executor.cancel();
+          ASSERT_TRUE(diff > 0.009);
+          ASSERT_TRUE(diff < 0.011);
+        }
+      }
+    };
+
+  auto timer = node->create_wall_timer(10ms, timer_callback, cbg);
+  executor.add_node(node);
+  executor.spin();
+}