From 73c408b7231fb8ee8142dd7cfdb3cd5e1fa1a8b5 Mon Sep 17 00:00:00 2001
From: Michael Carroll <michael@openrobotics.org>
Date: Wed, 28 Mar 2018 13:42:45 -0700
Subject: [PATCH] Add locking and test happy-path exit conditions.

---
 rclcpp/include/rclcpp/executor.hpp            |  2 ++
 rclcpp/src/rclcpp/executor.cpp                |  6 +++-
 .../test_multi_threaded_executor.cpp          | 31 ++++++++++++-------
 3 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index c8d1761904..925d3aa73e 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -352,6 +352,8 @@ class Executor
   RCLCPP_DISABLE_COPY(Executor)
 
   std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+
+  std::mutex taken_timers_mutex_;
   std::set<rclcpp::TimerBase::SharedPtr> taken_timers_;
 };
 
diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
index eff445afcc..bb2fb2058d 100644
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -251,6 +251,8 @@ Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
   }
   if (any_exec->timer) {
     execute_timer(any_exec->timer);
+
+    std::lock_guard<std::mutex> lock(taken_timers_mutex_);
     auto it = taken_timers_.find(any_exec->timer);
     if (it != taken_timers_.end()) {
       taken_timers_.erase(it);
@@ -525,9 +527,11 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
         continue;
       }
       for (auto & timer_ref : group->get_timer_ptrs()) {
+        std::lock_guard<std::mutex> lock(taken_timers_mutex_);
         auto timer = timer_ref.lock();
 
-        if (timer && timer->is_ready() && taken_timers_.count(timer) == 0) {
+        bool is_not_scheduled = (taken_timers_.count(timer) == 0);
+        if (timer && timer->is_ready() && is_not_scheduled) {
           any_exec->timer = timer;
           any_exec->callback_group = group;
           node = get_node_by_group(group);
diff --git a/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp/test/executors/test_multi_threaded_executor.cpp
index 1b670bef0d..4c623b3599 100644
--- a/rclcpp/test/executors/test_multi_threaded_executor.cpp
+++ b/rclcpp/test/executors/test_multi_threaded_executor.cpp
@@ -29,7 +29,7 @@ using namespace std::chrono_literals;
 
 using rcl_interfaces::msg::IntraProcessMessage;
 
-class TestMultiThreadedExecutor : public::testing::Test
+class TestMultiThreadedExecutor : public ::testing::Test
 {
 protected:
   static void SetUpTestCase()
@@ -64,21 +64,28 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) {
   std::mutex last_mutex;
   auto last = system_clock.now();
 
-  auto timer_callback = [&executor, &system_clock, &last_mutex, &last]() {
-    rclcpp::Time now = system_clock.now();
+  std::atomic_int timer_count {0};
 
-    {
-      std::lock_guard<std::mutex> lock(last_mutex);
-      double diff = labs((now - last).nanoseconds()) / 1.0e9;
-      last = now;
+  auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() {
+      rclcpp::Time now = system_clock.now();
+      timer_count++;
 
-      if (diff < 0.09 || diff > 0.11) {
+      if (timer_count > 20) {
         executor.cancel();
-        ASSERT_TRUE(diff > 0.09);
-        ASSERT_TRUE(diff < 0.11);
       }
-    }
-  };
+
+      {
+        std::lock_guard<std::mutex> lock(last_mutex);
+        double diff = labs((now - last).nanoseconds()) / 1.0e9;
+        last = now;
+
+        if (diff < 0.09 || diff > 0.11) {
+          executor.cancel();
+          ASSERT_TRUE(diff > 0.09);
+          ASSERT_TRUE(diff < 0.11);
+        }
+      }
+    };
 
   auto timer = node->create_wall_timer(100ms, timer_callback, cbg);
   executor.add_node(node);