Allow timers to keep up the intended rate in MultiThreadedExecutor

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -87,6 +87,7 @@ class MultiThreadedExecutor : public rclcpp::Executor
   std::chrono::nanoseconds next_exec_timeout_;
 
   std::set<TimerBase::SharedPtr> scheduled_timers_;
+  std::mutex scheduled_timers_mutex_;
 };
 
 }  // namespace executors

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -76,7 +76,7 @@ MultiThreadedExecutor::run(size_t)
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_exec;
     {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+      std::lock_guard<std::mutex> wait_guard{wait_mutex_};
       if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
@@ -85,6 +85,13 @@ MultiThreadedExecutor::run(size_t)
       }
       if (any_exec.timer) {
         // Guard against multiple threads getting the same timer.
+
+        // THIS MUST BE DONE WITH BOTH THE WAIT_MUTEX AND SCHEDULED TIMERS MUTEX TAKEN!!
+        // It might seem unnecessary, but you avoid the following race:
+        // Thread A: get timer, context switch.
+        // Thread B: get timer, insert scheduled timer, execute timer, remove scheduled timer.
+        // Thread A: execute timer.
+        std::lock_guard<std::mutex> scheduled_timers_guard{scheduled_timers_mutex_};
         if (scheduled_timers_.count(any_exec.timer) != 0) {
           // Make sure that any_exec's callback group is reset before
           // the lock is released.
@@ -103,7 +110,15 @@ MultiThreadedExecutor::run(size_t)
     execute_any_executable(any_exec);
 
     if (any_exec.timer) {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+      // DON'T DELETE THE scheduled_timers_mutex_ AND REPLACE IT WITH THE wait_mutex_ here.
+      // Now, this mutex will only compete with ONE worker thread that's is trying to insert
+      // a timer.
+      // (and also, N other worker threads also trying to delete a timer).
+      // If the wait_mutex_ is used here, this will compete with all the other worker threads!!!
+      // If we're waiting too long too remove the scheduled timer, maybe we are discarding a timer
+      // execution that we shouldn't!
+      // Of course, this can still happen if the callback is too long ...
+      std::lock_guard<std::mutex> scheduled_timers_guard{scheduled_timers_mutex_};
       auto it = scheduled_timers_.find(any_exec.timer);
       if (it != scheduled_timers_.end()) {
         scheduled_timers_.erase(it);