From c6608a4945b0854844b5bb72724ee1f6d35476ac Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Tue, 28 Nov 2023 14:35:05 -0800
Subject: [PATCH 01/13] fix

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../rclcpp/experimental/timers_manager.hpp     | 12 +++++++++---
 .../src/rclcpp/experimental/timers_manager.cpp | 18 ++++++++++++++----
 2 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
index 688684376a..f89801ca00 100644
--- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp
+++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
@@ -25,7 +25,6 @@
 #include <thread>
 #include <utility>
 #include <vector>
-
 #include "rclcpp/context.hpp"
 #include "rclcpp/timer.hpp"
 
@@ -496,7 +495,14 @@ class TimersManager
     static bool timer_greater(TimerPtr a, TimerPtr b)
     {
       // TODO(alsora): this can cause an error if timers are using different clocks
-      return a->time_until_trigger() > b->time_until_trigger();
+      bool both_canceled_or_active = a->is_canceled() == b->is_canceled();
+      if (both_canceled_or_active) {
+        return a->time_until_trigger() > b->time_until_trigger();
+      }
+      else if (a->is_canceled()) {
+        return true;
+      }
+      return false;
     }
 
     std::vector<TimerPtr> owned_heap_;
@@ -517,7 +523,7 @@ class TimersManager
    * or std::chrono::nanoseconds::max() if the heap is empty.
    * This function is not thread safe, acquire the timers_mutex_ before calling it.
    */
-  std::chrono::nanoseconds get_head_timeout_unsafe();
+  std::chrono::nanoseconds get_head_timeout_unsafe(bool& head_was_canceled);
 
   /**
    * @brief Executes all the timers currently ready when the function is invoked
diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
index e179787b36..b6f562c03b 100644
--- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp
+++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
@@ -109,7 +109,8 @@ std::chrono::nanoseconds TimersManager::get_head_timeout()
   }
 
   std::unique_lock<std::mutex> lock(timers_mutex_);
-  return this->get_head_timeout_unsafe();
+  bool head_was_cancelled;
+  return this->get_head_timeout_unsafe(head_was_cancelled);
 }
 
 size_t TimersManager::get_number_ready_timers()
@@ -169,7 +170,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
   }
 }
 
-std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
+std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe(bool& head_was_cancelled)
 {
   // If we don't have any weak pointer, then we just return maximum timeout
   if (weak_timers_heap_.empty()) {
@@ -191,6 +192,7 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
     }
     head_timer = locked_heap.front();
   }
+  head_was_cancelled = head_timer->is_canceled();
 
   return head_timer->time_until_trigger();
 }
@@ -242,14 +244,22 @@ void TimersManager::run_timers()
     // Lock mutex
     std::unique_lock<std::mutex> lock(timers_mutex_);
 
-    std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe();
+    bool head_was_cancelled = false;
+    std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe(head_was_cancelled);
 
     // No need to wait if a timer is already available
     if (time_to_sleep > std::chrono::nanoseconds::zero()) {
       if (time_to_sleep != std::chrono::nanoseconds::max()) {
         // Wait until timeout or notification that timers have been updated
         timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;});
-      } else {
+      }
+      else if (head_was_cancelled) {
+        // Wait until notification that timers have been updated
+        TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+        locked_heap.heapify();
+        weak_timers_heap_.store(locked_heap);
+      }
+      else {
         // Wait until notification that timers have been updated
         timers_cv_.wait(lock, [this]() {return timers_updated_;});
       }

From 11050a1d1bb3aa7f296177abb4329e7c62120c6a Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Wed, 29 Nov 2023 12:58:15 -0800
Subject: [PATCH 02/13] add timer cancel tests

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../test/rclcpp/executors/test_executors.cpp  | 102 ++++++++++++++++++
 rclcpp/test/rclcpp/test_timers_manager.cpp    |  48 +++++++++
 2 files changed, 150 insertions(+)

diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
index 653f06fb9c..5cb9d88e31 100644
--- a/rclcpp/test/rclcpp/executors/test_executors.cpp
+++ b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -818,3 +818,105 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) {
   executor.spin();
   EXPECT_EQ(kNumMessages, this->callback_count.load());
 }
+
+class TimerNode : public rclcpp::Node {
+   public:
+    TimerNode(std::string subname)
+        : Node("timer_node", subname) {
+
+        
+        timer1_ = rclcpp::create_timer(this->get_node_base_interface(), get_node_timers_interface(),
+                                      get_clock(), 1ms,
+                                      std::bind(&TimerNode::Timer1Callback, this));
+
+        timer2_ =
+              rclcpp::create_timer(this->get_node_base_interface(), get_node_timers_interface(),
+                                      get_clock(), 1ms,
+                                      std::bind(&TimerNode::Timer2Callback, this));
+    }
+
+    int GetTimer1Cnt() { return cnt1_; }
+    int GetTimer2Cnt() { return cnt2_; }
+   private:
+    void Timer1Callback() {
+        RCLCPP_DEBUG(this->get_logger(), "Timer 1!");
+        if (++cnt1_ > 5) {
+            RCLCPP_DEBUG(this->get_logger(), "Timer cancelling itself!");
+            timer1_->cancel();
+        }
+    }
+
+    void Timer2Callback() {
+        RCLCPP_DEBUG(this->get_logger(), "Timer 2!");
+        cnt2_++;
+    }
+
+    rclcpp::TimerBase::SharedPtr timer1_;
+    rclcpp::TimerBase::SharedPtr timer2_;
+    int cnt1_ = 0;
+    int cnt2_ = 0;
+};
+
+
+template<typename T>
+class TestTimerCancelBehavior : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    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<TimerNode>(test_name.str());
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+
+  std::shared_ptr<TimerNode> node;
+};
+
+TYPED_TEST_SUITE(TestTimerCancelBehavior, ExecutorTypes, ExecutorTypeNames);
+
+TYPED_TEST(TestTimerCancelBehavior, testOneTimerCancelledWithExecutorSpin) {
+  // Validate that cancelling one timer yields no change in behavior for other
+  // timers. Specifically, this tests the behavior when using spin() to run the
+  // executor, which is the most common usecase.
+
+  // Spin the executor in a standalone thread
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+  std::thread real_time_thread([&executor]() {
+    executor.spin();
+  });
+
+  // Cancel to stop the spin after some time.
+  std::this_thread::sleep_for(15ms);
+  executor.cancel();
+
+  size_t t1_runs = this->node->GetTimer1Cnt();
+  size_t t2_runs = this->node->GetTimer2Cnt();
+  EXPECT_NE(t1_runs, t2_runs);
+  // Check that t2 has significantly more calls
+  EXPECT_LT(t1_runs + 5, t2_runs);
+
+  // Clean up thread object
+  if (real_time_thread.joinable()) {
+    real_time_thread.join();
+  }
+}
+
diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp
index 167523934c..8ac9a3be63 100644
--- a/rclcpp/test/rclcpp/test_timers_manager.cpp
+++ b/rclcpp/test/rclcpp/test_timers_manager.cpp
@@ -21,6 +21,8 @@
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/experimental/timers_manager.hpp"
+#include "rclcpp/node.hpp"
+#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
 
 using namespace std::chrono_literals;
 
@@ -359,3 +361,49 @@ TEST_F(TestTimersManager, infinite_loop)
   EXPECT_LT(0u, t1_runs);
   EXPECT_LT(0u, t2_runs);
 }
+
+// Validate that cancelling one timer yields no change in behavior for other
+// timers.
+TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t1_runs = 0;
+  std::shared_ptr<TimerT> t1;
+  // After a while cancel t1. Don't remove it though.
+  // Simulates typical usage in a Node where a timer is cancelled but not removed,
+  // since typical users aren't going to mess around with the timer manager.
+  t1 = TimerT::make_shared(
+    1ms,
+    [&t1_runs, &t1]() {
+      t1_runs++;
+      if (t1_runs == 5) {
+        t1->cancel();
+      }
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t2_runs = 0;
+  auto t2 = TimerT::make_shared(
+    1ms,
+    [&t2_runs]() {
+      t2_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  // Add timers
+  timers_manager->add_timer(t1);
+  timers_manager->add_timer(t2);
+
+  // Start timers thread
+  timers_manager->start();
+
+  std::this_thread::sleep_for(15ms);
+
+  // t1 has stopped running
+  EXPECT_NE(t1_runs, t2_runs);
+  // Check that t2 has significantly more calls
+  EXPECT_LT(t1_runs + 5, t2_runs);
+  timers_manager->stop();
+}

From 067271af34c81addb3fd9a2f9bf4a2264f8d5619 Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Wed, 29 Nov 2023 13:05:46 -0800
Subject: [PATCH 03/13] cleanup header include

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 rclcpp/test/rclcpp/test_timers_manager.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp
index 8ac9a3be63..0e49da08e1 100644
--- a/rclcpp/test/rclcpp/test_timers_manager.cpp
+++ b/rclcpp/test/rclcpp/test_timers_manager.cpp
@@ -21,8 +21,6 @@
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/experimental/timers_manager.hpp"
-#include "rclcpp/node.hpp"
-#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
 
 using namespace std::chrono_literals;
 

From ac2005d5ce42e0a3bd7dc8ef89fb267a89949936 Mon Sep 17 00:00:00 2001
From: Gus Brigantino <gusbrig97@gmail.com>
Date: Wed, 29 Nov 2023 14:32:58 -0800
Subject: [PATCH 04/13] reverting change to timer_greater function

Signed-off-by: Gus Brigantino <gusbrig97@gmail.com>
---
 rclcpp/include/rclcpp/experimental/timers_manager.hpp | 9 +--------
 1 file changed, 1 insertion(+), 8 deletions(-)

diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
index f89801ca00..a5c237d6a5 100644
--- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp
+++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
@@ -495,14 +495,7 @@ class TimersManager
     static bool timer_greater(TimerPtr a, TimerPtr b)
     {
       // TODO(alsora): this can cause an error if timers are using different clocks
-      bool both_canceled_or_active = a->is_canceled() == b->is_canceled();
-      if (both_canceled_or_active) {
-        return a->time_until_trigger() > b->time_until_trigger();
-      }
-      else if (a->is_canceled()) {
-        return true;
-      }
-      return false;
+      return a->time_until_trigger() > b->time_until_trigger();
     }
 
     std::vector<TimerPtr> owned_heap_;

From 3a8e7e9d3200662fdb3fabc71d54fd6600876103 Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Thu, 30 Nov 2023 14:14:19 -0800
Subject: [PATCH 05/13] use std::optional, and handle edgecase of 1 cancelled
 timer

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../rclcpp/experimental/timers_manager.hpp    | 20 +++++++--
 .../events_executor/events_executor.cpp       |  7 +--
 .../rclcpp/experimental/timers_manager.cpp    | 45 +++++++++++--------
 3 files changed, 47 insertions(+), 25 deletions(-)

diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
index a5c237d6a5..fbd0e42bc4 100644
--- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp
+++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
@@ -22,6 +22,7 @@
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <thread>
 #include <utility>
 #include <vector>
@@ -171,13 +172,14 @@ class TimersManager
    * @brief Get the amount of time before the next timer triggers.
    * This function is thread safe.
    *
-   * @return std::chrono::nanoseconds to wait,
+   * @return std::optional<std::chrono::nanoseconds> to wait,
    * the returned value could be negative if the timer is already expired
    * or std::chrono::nanoseconds::max() if there are no timers stored in the object.
+   * If the head timer was cancelled, then this will return a nullopt.
    * @throws std::runtime_error if the timers thread was already running.
    */
   RCLCPP_PUBLIC
-  std::chrono::nanoseconds get_head_timeout();
+  std::optional<std::chrono::nanoseconds> get_head_timeout();
 
 private:
   RCLCPP_DISABLE_COPY(TimersManager)
@@ -270,6 +272,15 @@ class TimersManager
       return weak_heap_.empty();
     }
 
+    /**
+     * @brief Returns the size of the heap.
+     * @return the number of valid timers in the heap.
+     */
+    size_t size() const
+    {
+      return weak_heap_.size();
+    }
+
     /**
      * @brief This function restores the current object as a valid heap
      * and it returns a locked version of it.
@@ -511,12 +522,13 @@ class TimersManager
    * @brief Get the amount of time before the next timer triggers.
    * This function is not thread safe, acquire a mutex before calling it.
    *
-   * @return std::chrono::nanoseconds to wait,
+   * @return std::optional<std::chrono::nanoseconds> to wait,
    * the returned value could be negative if the timer is already expired
    * or std::chrono::nanoseconds::max() if the heap is empty.
+   * If the head timer was cancelled, then this will return a nullopt.
    * This function is not thread safe, acquire the timers_mutex_ before calling it.
    */
-  std::chrono::nanoseconds get_head_timeout_unsafe(bool& head_was_canceled);
+  std::optional<std::chrono::nanoseconds> get_head_timeout_unsafe();
 
   /**
    * @brief Executes all the timers currently ready when the function is invoked
diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp
index 64b07c0814..c977c8c904 100644
--- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp
+++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp
@@ -206,11 +206,12 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
     timeout = std::chrono::nanoseconds::max();
   }
 
-  // Select the smallest between input timeout and timer timeout
+  // Select the smallest between input timeout and timer timeout.
+  // Cancelled timers are not considered.
   bool is_timer_timeout = false;
   auto next_timer_timeout = timers_manager_->get_head_timeout();
-  if (next_timer_timeout < timeout) {
-    timeout = next_timer_timeout;
+  if (next_timer_timeout.has_value() && next_timer_timeout.value() < timeout) {
+    timeout = next_timer_timeout.value();
     is_timer_timeout = true;
   }
 
diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
index b6f562c03b..e5cee5f025 100644
--- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp
+++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
@@ -100,7 +100,7 @@ void TimersManager::stop()
   }
 }
 
-std::chrono::nanoseconds TimersManager::get_head_timeout()
+std::optional<std::chrono::nanoseconds> TimersManager::get_head_timeout()
 {
   // Do not allow to interfere with the thread running
   if (running_) {
@@ -109,8 +109,7 @@ std::chrono::nanoseconds TimersManager::get_head_timeout()
   }
 
   std::unique_lock<std::mutex> lock(timers_mutex_);
-  bool head_was_cancelled;
-  return this->get_head_timeout_unsafe(head_was_cancelled);
+  return this->get_head_timeout_unsafe();
 }
 
 size_t TimersManager::get_number_ready_timers()
@@ -170,7 +169,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
   }
 }
 
-std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe(bool& head_was_cancelled)
+std::optional<std::chrono::nanoseconds> TimersManager::get_head_timeout_unsafe()
 {
   // If we don't have any weak pointer, then we just return maximum timeout
   if (weak_timers_heap_.empty()) {
@@ -192,8 +191,9 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe(bool& head_was_c
     }
     head_timer = locked_heap.front();
   }
-  head_was_cancelled = head_timer->is_canceled();
-
+  if (head_timer->is_canceled()) {
+    return std::nullopt;
+  }
   return head_timer->time_until_trigger();
 }
 
@@ -244,20 +244,29 @@ void TimersManager::run_timers()
     // Lock mutex
     std::unique_lock<std::mutex> lock(timers_mutex_);
 
-    bool head_was_cancelled = false;
-    std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe(head_was_cancelled);
-
+    std::optional<std::chrono::nanoseconds> time_to_sleep = get_head_timeout_unsafe();
+
+    if (!time_to_sleep.has_value()) {
+        // If head was cancelled, and there is more than 1 timer, we need to re-heapify. This
+        // is a case where the next up timer should really be used, so the next loop iteration
+        // will pick up the correct head.
+        if (weak_timers_heap_.size() > 1) {
+          TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+          locked_heap.heapify();
+          weak_timers_heap_.store(locked_heap);
+        }
+        // Otherwise, we can just wait indefinitely for a new timer to be added, since the only
+        // timer is invalid.
+        else {
+          // Wait until notification that timers have been updated
+          timers_cv_.wait(lock, [this]() {return timers_updated_;});
+        }
+    }
     // No need to wait if a timer is already available
-    if (time_to_sleep > std::chrono::nanoseconds::zero()) {
-      if (time_to_sleep != std::chrono::nanoseconds::max()) {
+    else if (time_to_sleep.value() > std::chrono::nanoseconds::zero()) {
+      if (time_to_sleep.value() != std::chrono::nanoseconds::max()) {
         // Wait until timeout or notification that timers have been updated
-        timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;});
-      }
-      else if (head_was_cancelled) {
-        // Wait until notification that timers have been updated
-        TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
-        locked_heap.heapify();
-        weak_timers_heap_.store(locked_heap);
+        timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
       }
       else {
         // Wait until notification that timers have been updated

From 68ea151fd4ee61ab1ee8f718e6199b6f026e823a Mon Sep 17 00:00:00 2001
From: Gus Brigantino <gusbrig97@gmail.com>
Date: Mon, 4 Dec 2023 15:33:56 -0700
Subject: [PATCH 06/13] clean up run_timers func

Signed-off-by: Gus Brigantino <gusbrig97@gmail.com>
---
 .../rclcpp/experimental/timers_manager.cpp    | 50 +++++++++++++------
 1 file changed, 35 insertions(+), 15 deletions(-)

diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
index e5cee5f025..55527655bd 100644
--- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp
+++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
@@ -246,28 +246,48 @@ void TimersManager::run_timers()
 
     std::optional<std::chrono::nanoseconds> time_to_sleep = get_head_timeout_unsafe();
 
-    if (!time_to_sleep.has_value()) {
-        // If head was cancelled, and there is more than 1 timer, we need to re-heapify. This
-        // is a case where the next up timer should really be used, so the next loop iteration
-        // will pick up the correct head.
-        if (weak_timers_heap_.size() > 1) {
-          TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
-          locked_heap.heapify();
-          weak_timers_heap_.store(locked_heap);
+    // Head timer is not cancelled
+    if (time_to_sleep.has_value()) {
+      // No need to wait if a timer is already available
+      if (time_to_sleep.value() > std::chrono::nanoseconds::zero()) {
+        // Heap is not empty
+        if (time_to_sleep.value() != std::chrono::nanoseconds::max()) {
+          // Check if head timer is in cancelled_timers_
+          TimerPtr head_timer = weak_timers_heap_.front().lock();
+          auto itr = std::find(cancelled_timers_.begin(), cancelled_timers_.end(), head_timer);
+
+          if (itr != cancelled_timers_.end()) {
+            // Remove if present bc recieved valid time_to_sleep so no longer cancelled
+            cancelled_timers_.erase(itr);
+          }
+
+          // Wait until timeout or notification that timers have been updated
+          timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
         }
-        // Otherwise, we can just wait indefinitely for a new timer to be added, since the only
-        // timer is invalid.
+        // Heap is empty
         else {
           // Wait until notification that timers have been updated
           timers_cv_.wait(lock, [this]() {return timers_updated_;});
         }
+      }
     }
-    // No need to wait if a timer is already available
-    else if (time_to_sleep.value() > std::chrono::nanoseconds::zero()) {
-      if (time_to_sleep.value() != std::chrono::nanoseconds::max()) {
-        // Wait until timeout or notification that timers have been updated
-        timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
+    // Head timer is cancelled
+    else {
+      // Check if head timer is in cancelled_timers_ already
+      TimerPtr head_timer = weak_timers_heap_.front().lock();
+      auto itr = std::find(cancelled_timers_.begin(), cancelled_timers_.end(), head_timer);
+
+      if (itr == cancelled_timers_.end()) {
+        // Append to vector if not present so it will be treated as a cancelled timer
+        cancelled_timers_.push_back(head_timer);
+
+        // Re-heap to move cancelled timer from head of heap
+        TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+        locked_heap.heapify();
+        weak_timers_heap_.store(locked_heap);
       }
+      // Head timer has already been recongized as cancelled
+      // This handles the case where all timers in the heap are cancelled
       else {
         // Wait until notification that timers have been updated
         timers_cv_.wait(lock, [this]() {return timers_updated_;});

From 0f20255e543ed65483adf149960eb5f92e4fa054 Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Tue, 12 Dec 2023 15:04:29 -0800
Subject: [PATCH 07/13] some fixes and added tests for cancel then reset of
 timers.

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../rclcpp/experimental/timers_manager.cpp    |   6 +
 .../test/rclcpp/executors/test_executors.cpp  | 202 +++++++++++++++++-
 2 files changed, 203 insertions(+), 5 deletions(-)

diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
index 55527655bd..13afa3f11b 100644
--- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp
+++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
@@ -291,6 +291,12 @@ void TimersManager::run_timers()
       else {
         // Wait until notification that timers have been updated
         timers_cv_.wait(lock, [this]() {return timers_updated_;});
+
+        // Re-heap in case ordering changed due to a cancelled timer
+        // re-activating.
+        TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+        locked_heap.heapify();
+        weak_timers_heap_.store(locked_heap);
       }
     }
 
diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
index 5cb9d88e31..36c974178e 100644
--- a/rclcpp/test/rclcpp/executors/test_executors.cpp
+++ b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -837,13 +837,29 @@ class TimerNode : public rclcpp::Node {
 
     int GetTimer1Cnt() { return cnt1_; }
     int GetTimer2Cnt() { return cnt2_; }
+
+    void ResetTimer1() {
+        timer1_->reset();
+    }
+
+    void ResetTimer2() {
+        timer2_->reset();
+    }
+
+    void CancelTimer1() {
+      RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!");
+      timer1_->cancel();
+    }
+
+    void CancelTimer2() {
+      RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!");
+      timer2_->cancel();
+    }
+
    private:
     void Timer1Callback() {
         RCLCPP_DEBUG(this->get_logger(), "Timer 1!");
-        if (++cnt1_ > 5) {
-            RCLCPP_DEBUG(this->get_logger(), "Timer cancelling itself!");
-            timer1_->cancel();
-        }
+        cnt1_++;
     }
 
     void Timer2Callback() {
@@ -885,6 +901,53 @@ class TestTimerCancelBehavior : public ::testing::Test
     node.reset();
   }
 
+  /*void RunBothTimerCancelTest(bool cancel_timer1_first) {
+    // Spin the executor in a standalone thread
+    using ExecutorType = TypeParam;
+    ExecutorType executor;
+    executor.add_node(this->node);
+    std::thread real_time_thread([&executor]() {
+      executor.spin();
+    });
+
+    // Cancel to stop the spin after some time.
+    std::this_thread::sleep_for(5ms);
+    if (cancel_timer1_first) {
+      this->node->CancelTimer1();
+      this->node->CancelTimer2();
+    }
+    else {
+      this->node->CancelTimer2();  
+      this->node->CancelTimer1();
+    }
+    std::this_thread::sleep_for(10ms);
+    size_t t1_runs_initial = this->node->GetTimer1Cnt();
+    size_t t2_runs_initial = this->node->GetTimer2Cnt();
+    
+    // Manually reset timer 1, then sleep again
+    // Counts should update.
+    this->node->ResetTimer1();
+    std::this_thread::sleep_for(15ms);
+    size_t t1_runs_final = this->node->GetTimer1Cnt();
+    size_t t2_runs_final = this->node->GetTimer2Cnt();
+
+    executor.cancel();
+
+    // T1 and T2 should have the same initial count.
+    EXPECT_EQ(t1_runs_initial, t2_runs_initial);
+
+    // Expect that T1 has up to 15 more calls than t2. Add some buffer
+    // to account for jitter.
+    EXPECT_EQ(t2_runs_initial, t2_runs_final);
+    EXPECT_LT(t1_runs_initial + 13, t1_runs_final);
+
+    // Clean up thread object
+    if (real_time_thread.joinable()) {
+      real_time_thread.join();
+    }
+  }
+  */
+
 
   std::shared_ptr<TimerNode> node;
 };
@@ -905,7 +968,9 @@ TYPED_TEST(TestTimerCancelBehavior, testOneTimerCancelledWithExecutorSpin) {
   });
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(15ms);
+  std::this_thread::sleep_for(5ms);
+  this->node->CancelTimer1();
+  std::this_thread::sleep_for(10ms);
   executor.cancel();
 
   size_t t1_runs = this->node->GetTimer1Cnt();
@@ -920,3 +985,130 @@ TYPED_TEST(TestTimerCancelBehavior, testOneTimerCancelledWithExecutorSpin) {
   }
 }
 
+TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
+  // Validate that cancelling timer doesn't affect operation of other timers,
+  // and that the cancelled timer starts executing normally once reset manually.
+
+  // Spin the executor in a standalone thread
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+  std::thread real_time_thread([&executor]() {
+    executor.spin();
+  });
+
+  // Cancel to stop the spin after some time.
+  std::this_thread::sleep_for(5ms);
+  this->node->CancelTimer1();
+  std::this_thread::sleep_for(10ms);
+  size_t t1_runs_initial = this->node->GetTimer1Cnt();
+  size_t t2_runs_initial = this->node->GetTimer2Cnt();
+  
+  // Manually reset timer 1, then sleep again
+  // Counts should update.
+  this->node->ResetTimer1();
+  std::this_thread::sleep_for(15ms);
+  size_t t1_runs_final = this->node->GetTimer1Cnt();
+  size_t t2_runs_final = this->node->GetTimer2Cnt();
+
+  executor.cancel();
+
+  // T1 should have been restarted, and execute about 15 additional times.
+  // Check 10 greater than initial, to account for some timing jitter.
+  EXPECT_LT(t1_runs_initial + 10, t1_runs_final);
+
+  EXPECT_LT(t1_runs_initial + 5, t2_runs_initial);
+  // Check that t2 has significantly more calls, and keeps getting called.
+  EXPECT_LT(t2_runs_initial + 13, t2_runs_final);
+
+  // Clean up thread object
+  if (real_time_thread.joinable()) {
+    real_time_thread.join();
+  }
+}
+
+TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
+  // Validate behavior from cancelling 2 timers, then only re-enabling one of them.
+  // Ensure that only the reset timer is executed.
+
+  // Spin the executor in a standalone thread
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+  std::thread real_time_thread([&executor]() {
+    executor.spin();
+  });
+
+  // Cancel to stop the spin after some time.
+  std::this_thread::sleep_for(5ms);
+  this->node->CancelTimer1();
+  this->node->CancelTimer2();
+  std::this_thread::sleep_for(10ms);
+  size_t t1_runs_initial = this->node->GetTimer1Cnt();
+  size_t t2_runs_initial = this->node->GetTimer2Cnt();
+  
+  // Manually reset timer 1, then sleep again
+  // Counts should update.
+  this->node->ResetTimer1();
+  std::this_thread::sleep_for(15ms);
+  size_t t1_runs_final = this->node->GetTimer1Cnt();
+  size_t t2_runs_final = this->node->GetTimer2Cnt();
+
+  executor.cancel();
+
+  // T1 and T2 should have the same initial count.
+  EXPECT_EQ(t1_runs_initial, t2_runs_initial);
+
+  // Expect that T1 has up to 15 more calls than t2. Add some buffer
+  // to account for jitter.
+  EXPECT_EQ(t2_runs_initial, t2_runs_final);
+  EXPECT_LT(t1_runs_initial + 11, t1_runs_final);
+
+  // Clean up thread object
+  if (real_time_thread.joinable()) {
+    real_time_thread.join();
+  }
+}
+
+TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
+  // Validate behavior from cancelling 2 timers, then only re-enabling one of them.
+  // Ensure that only the reset timer is executed.
+
+  // Spin the executor in a standalone thread
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+  std::thread real_time_thread([&executor]() {
+    executor.spin();
+  });
+
+  // Cancel to stop the spin after some time.
+  std::this_thread::sleep_for(5ms);
+  this->node->CancelTimer1();
+  this->node->CancelTimer2();
+  std::this_thread::sleep_for(10ms);
+  size_t t1_runs_initial = this->node->GetTimer1Cnt();
+  size_t t2_runs_initial = this->node->GetTimer2Cnt();
+  
+  // Manually reset timer 1, then sleep again
+  // Counts should update.
+  this->node->ResetTimer2();
+  std::this_thread::sleep_for(15ms);
+  size_t t1_runs_final = this->node->GetTimer1Cnt();
+  size_t t2_runs_final = this->node->GetTimer2Cnt();
+
+  executor.cancel();
+
+  // T1 and T2 should have the same initial count.
+  EXPECT_EQ(t1_runs_initial, t2_runs_initial);
+
+  // Expect that T1 has up to 15 more calls than t2. Add some buffer
+  // to account for jitter.
+  EXPECT_EQ(t1_runs_initial, t1_runs_final);
+  EXPECT_LT(t2_runs_initial + 11, t2_runs_final);
+
+  // Clean up thread object
+  if (real_time_thread.joinable()) {
+    real_time_thread.join();
+  }
+}

From 97a44fd5b2fd0948702d6d47ac1ad6d3906f6208 Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Tue, 12 Dec 2023 16:28:06 -0800
Subject: [PATCH 08/13] refactor and clean up. remove cancelled timer tracking.

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../rclcpp/experimental/timers_manager.cpp    |  77 +++----
 .../test/rclcpp/executors/test_executors.cpp  | 212 ++++++++----------
 2 files changed, 121 insertions(+), 168 deletions(-)

diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
index 13afa3f11b..822c5b3c27 100644
--- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp
+++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
@@ -246,58 +246,33 @@ void TimersManager::run_timers()
 
     std::optional<std::chrono::nanoseconds> time_to_sleep = get_head_timeout_unsafe();
 
-    // Head timer is not cancelled
-    if (time_to_sleep.has_value()) {
-      // No need to wait if a timer is already available
-      if (time_to_sleep.value() > std::chrono::nanoseconds::zero()) {
-        // Heap is not empty
-        if (time_to_sleep.value() != std::chrono::nanoseconds::max()) {
-          // Check if head timer is in cancelled_timers_
-          TimerPtr head_timer = weak_timers_heap_.front().lock();
-          auto itr = std::find(cancelled_timers_.begin(), cancelled_timers_.end(), head_timer);
-
-          if (itr != cancelled_timers_.end()) {
-            // Remove if present bc recieved valid time_to_sleep so no longer cancelled
-            cancelled_timers_.erase(itr);
-          }
-
-          // Wait until timeout or notification that timers have been updated
-          timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
-        }
-        // Heap is empty
-        else {
-          // Wait until notification that timers have been updated
-          timers_cv_.wait(lock, [this]() {return timers_updated_;});
-        }
-      }
+    // If head timer was cancelled, try to reheap and get a new head.
+    // This avoids an edge condition where head timer is cancelled, but other
+    // valid timers remain in the heap.
+    if (!time_to_sleep.has_value()) {
+      // Re-heap to (possibly) move cancelled timer from head of heap. If
+      // entire heap is cancelled, this will still result in a nullopt.
+      TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+      locked_heap.heapify();
+      weak_timers_heap_.store(locked_heap);
+      time_to_sleep = get_head_timeout_unsafe();
     }
-    // Head timer is cancelled
-    else {
-      // Check if head timer is in cancelled_timers_ already
-      TimerPtr head_timer = weak_timers_heap_.front().lock();
-      auto itr = std::find(cancelled_timers_.begin(), cancelled_timers_.end(), head_timer);
-
-      if (itr == cancelled_timers_.end()) {
-        // Append to vector if not present so it will be treated as a cancelled timer
-        cancelled_timers_.push_back(head_timer);
-
-        // Re-heap to move cancelled timer from head of heap
-        TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
-        locked_heap.heapify();
-        weak_timers_heap_.store(locked_heap);
-      }
-      // Head timer has already been recongized as cancelled
-      // This handles the case where all timers in the heap are cancelled
-      else {
-        // Wait until notification that timers have been updated
-        timers_cv_.wait(lock, [this]() {return timers_updated_;});
-
-        // Re-heap in case ordering changed due to a cancelled timer
-        // re-activating.
-        TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
-        locked_heap.heapify();
-        weak_timers_heap_.store(locked_heap);
-      }
+
+    // If no timers, or all timers cancelled, wait for an update.
+    if (!time_to_sleep.has_value() || (time_to_sleep.value() == std::chrono::nanoseconds::max()) ) {
+      // Wait until notification that timers have been updated
+      timers_cv_.wait(lock, [this]() {return timers_updated_;});
+
+      // Re-heap in case ordering changed due to a cancelled timer
+      // re-activating.
+      TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+      locked_heap.heapify();
+      weak_timers_heap_.store(locked_heap);
+    }
+    else if (time_to_sleep.value() != std::chrono::nanoseconds::zero()) {
+        // If time_to_sleep is zero, we immediately execute. Otherwise, wait
+        // until timeout or notification that timers have been updated
+        timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
     }
 
     // Reset timers updated flag
diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
index 36c974178e..7c5f2515b4 100644
--- a/rclcpp/test/rclcpp/executors/test_executors.cpp
+++ b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -894,124 +894,86 @@ class TestTimerCancelBehavior : public ::testing::Test
     std::stringstream test_name;
     test_name << test_info->test_case_name() << "_" << test_info->name();
     node = std::make_shared<TimerNode>(test_name.str());
-  }
 
-  void TearDown()
-  {
-    node.reset();
-  }
-
-  /*void RunBothTimerCancelTest(bool cancel_timer1_first) {
     // Spin the executor in a standalone thread
-    using ExecutorType = TypeParam;
-    ExecutorType executor;
     executor.add_node(this->node);
-    std::thread real_time_thread([&executor]() {
+    standalone_thread = std::thread([this]() {
       executor.spin();
     });
+  }
 
-    // Cancel to stop the spin after some time.
-    std::this_thread::sleep_for(5ms);
-    if (cancel_timer1_first) {
-      this->node->CancelTimer1();
-      this->node->CancelTimer2();
-    }
-    else {
-      this->node->CancelTimer2();  
-      this->node->CancelTimer1();
-    }
-    std::this_thread::sleep_for(10ms);
-    size_t t1_runs_initial = this->node->GetTimer1Cnt();
-    size_t t2_runs_initial = this->node->GetTimer2Cnt();
-    
-    // Manually reset timer 1, then sleep again
-    // Counts should update.
-    this->node->ResetTimer1();
-    std::this_thread::sleep_for(15ms);
-    size_t t1_runs_final = this->node->GetTimer1Cnt();
-    size_t t2_runs_final = this->node->GetTimer2Cnt();
-
-    executor.cancel();
-
-    // T1 and T2 should have the same initial count.
-    EXPECT_EQ(t1_runs_initial, t2_runs_initial);
-
-    // Expect that T1 has up to 15 more calls than t2. Add some buffer
-    // to account for jitter.
-    EXPECT_EQ(t2_runs_initial, t2_runs_final);
-    EXPECT_LT(t1_runs_initial + 13, t1_runs_final);
+  void TearDown()
+  {
+    node.reset();
 
     // Clean up thread object
-    if (real_time_thread.joinable()) {
-      real_time_thread.join();
+    if (standalone_thread.joinable()) {
+      standalone_thread.join();
     }
   }
-  */
-
 
   std::shared_ptr<TimerNode> node;
+  std::thread standalone_thread;
+  T executor;
 };
 
 TYPED_TEST_SUITE(TestTimerCancelBehavior, ExecutorTypes, ExecutorTypeNames);
 
-TYPED_TEST(TestTimerCancelBehavior, testOneTimerCancelledWithExecutorSpin) {
+TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) {
   // Validate that cancelling one timer yields no change in behavior for other
   // timers. Specifically, this tests the behavior when using spin() to run the
   // executor, which is the most common usecase.
 
-  // Spin the executor in a standalone thread
-  using ExecutorType = TypeParam;
-  ExecutorType executor;
-  executor.add_node(this->node);
-  std::thread real_time_thread([&executor]() {
-    executor.spin();
-  });
-
   // Cancel to stop the spin after some time.
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer1();
   std::this_thread::sleep_for(10ms);
-  executor.cancel();
+  this->executor.cancel();
 
-  size_t t1_runs = this->node->GetTimer1Cnt();
-  size_t t2_runs = this->node->GetTimer2Cnt();
+  int t1_runs = this->node->GetTimer1Cnt();
+  int t2_runs = this->node->GetTimer2Cnt();
   EXPECT_NE(t1_runs, t2_runs);
   // Check that t2 has significantly more calls
   EXPECT_LT(t1_runs + 5, t2_runs);
+}
 
-  // Clean up thread object
-  if (real_time_thread.joinable()) {
-    real_time_thread.join();
-  }
+TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) {
+  // Validate that cancelling one timer yields no change in behavior for other
+  // timers. Specifically, this tests the behavior when using spin() to run the
+  // executor, which is the most common usecase.
+
+  // Cancel to stop the spin after some time.
+  std::this_thread::sleep_for(5ms);
+  this->node->CancelTimer2();
+  std::this_thread::sleep_for(10ms);
+  this->executor.cancel();
+
+  int t1_runs = this->node->GetTimer1Cnt();
+  int t2_runs = this->node->GetTimer2Cnt();
+  EXPECT_NE(t1_runs, t2_runs);
+  // Check that t1 has significantly more calls
+  EXPECT_LT(t2_runs + 5, t1_runs);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
   // Validate that cancelling timer doesn't affect operation of other timers,
   // and that the cancelled timer starts executing normally once reset manually.
 
-  // Spin the executor in a standalone thread
-  using ExecutorType = TypeParam;
-  ExecutorType executor;
-  executor.add_node(this->node);
-  std::thread real_time_thread([&executor]() {
-    executor.spin();
-  });
-
   // Cancel to stop the spin after some time.
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer1();
   std::this_thread::sleep_for(10ms);
-  size_t t1_runs_initial = this->node->GetTimer1Cnt();
-  size_t t2_runs_initial = this->node->GetTimer2Cnt();
+  int t1_runs_initial = this->node->GetTimer1Cnt();
+  int t2_runs_initial = this->node->GetTimer2Cnt();
   
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
   std::this_thread::sleep_for(15ms);
-  size_t t1_runs_final = this->node->GetTimer1Cnt();
-  size_t t2_runs_final = this->node->GetTimer2Cnt();
+  int t1_runs_final = this->node->GetTimer1Cnt();
+  int t2_runs_final = this->node->GetTimer2Cnt();
 
-  executor.cancel();
+  this->executor.cancel();
 
   // T1 should have been restarted, and execute about 15 additional times.
   // Check 10 greater than initial, to account for some timing jitter.
@@ -1020,95 +982,111 @@ TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
   EXPECT_LT(t1_runs_initial + 5, t2_runs_initial);
   // Check that t2 has significantly more calls, and keeps getting called.
   EXPECT_LT(t2_runs_initial + 13, t2_runs_final);
+}
 
-  // Clean up thread object
-  if (real_time_thread.joinable()) {
-    real_time_thread.join();
-  }
+TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
+  // Validate that cancelling timer doesn't affect operation of other timers,
+  // and that the cancelled timer starts executing normally once reset manually.
+
+  // Cancel to stop the spin after some time.
+  std::this_thread::sleep_for(5ms);
+  this->node->CancelTimer2();
+  std::this_thread::sleep_for(10ms);
+  int t1_runs_initial = this->node->GetTimer1Cnt();
+  int t2_runs_initial = this->node->GetTimer2Cnt();
+  
+  // Manually reset timer 1, then sleep again
+  // Counts should update.
+  this->node->ResetTimer2();
+  std::this_thread::sleep_for(15ms);
+  int t1_runs_final = this->node->GetTimer1Cnt();
+  int t2_runs_final = this->node->GetTimer2Cnt();
+
+  this->executor.cancel();
+
+  // T2 should have been restarted, and execute about 15 additional times.
+  // Check 10 greater than initial, to account for some timing jitter.
+  EXPECT_LT(t2_runs_initial + 10, t2_runs_final);
+
+  EXPECT_LT(t2_runs_initial + 5, t1_runs_initial);
+  // Check that t1 has significantly more calls, and keeps getting called.
+  EXPECT_LT(t1_runs_initial + 13, t1_runs_final);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
   // Validate behavior from cancelling 2 timers, then only re-enabling one of them.
   // Ensure that only the reset timer is executed.
 
-  // Spin the executor in a standalone thread
-  using ExecutorType = TypeParam;
-  ExecutorType executor;
-  executor.add_node(this->node);
-  std::thread real_time_thread([&executor]() {
-    executor.spin();
-  });
-
   // Cancel to stop the spin after some time.
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer1();
   this->node->CancelTimer2();
   std::this_thread::sleep_for(10ms);
-  size_t t1_runs_initial = this->node->GetTimer1Cnt();
-  size_t t2_runs_initial = this->node->GetTimer2Cnt();
+  int t1_runs_initial = this->node->GetTimer1Cnt();
+  int t2_runs_initial = this->node->GetTimer2Cnt();
   
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
   std::this_thread::sleep_for(15ms);
-  size_t t1_runs_final = this->node->GetTimer1Cnt();
-  size_t t2_runs_final = this->node->GetTimer2Cnt();
+  int t1_runs_intermediate = this->node->GetTimer1Cnt();
+  int t2_runs_intermediate = this->node->GetTimer2Cnt();
 
-  executor.cancel();
+  this->node->ResetTimer2();
+  std::this_thread::sleep_for(15ms);
+  int t1_runs_final = this->node->GetTimer1Cnt();
+  int t2_runs_final = this->node->GetTimer2Cnt();
+
+  this->executor.cancel();
 
   // T1 and T2 should have the same initial count.
-  EXPECT_EQ(t1_runs_initial, t2_runs_initial);
+  EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1);
 
   // Expect that T1 has up to 15 more calls than t2. Add some buffer
   // to account for jitter.
-  EXPECT_EQ(t2_runs_initial, t2_runs_final);
-  EXPECT_LT(t1_runs_initial + 11, t1_runs_final);
+  EXPECT_EQ(t2_runs_initial, t2_runs_intermediate);
+  EXPECT_LT(t1_runs_initial + 11, t1_runs_intermediate);
 
-  // Clean up thread object
-  if (real_time_thread.joinable()) {
-    real_time_thread.join();
-  }
+  // Expect that by end of test, both are running properly again.
+  EXPECT_LT(t1_runs_intermediate + 11, t1_runs_final);
+  EXPECT_LT(t2_runs_intermediate + 11, t2_runs_final);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
   // Validate behavior from cancelling 2 timers, then only re-enabling one of them.
   // Ensure that only the reset timer is executed.
 
-  // Spin the executor in a standalone thread
-  using ExecutorType = TypeParam;
-  ExecutorType executor;
-  executor.add_node(this->node);
-  std::thread real_time_thread([&executor]() {
-    executor.spin();
-  });
-
   // Cancel to stop the spin after some time.
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer1();
   this->node->CancelTimer2();
   std::this_thread::sleep_for(10ms);
-  size_t t1_runs_initial = this->node->GetTimer1Cnt();
-  size_t t2_runs_initial = this->node->GetTimer2Cnt();
+  int t1_runs_initial = this->node->GetTimer1Cnt();
+  int t2_runs_initial = this->node->GetTimer2Cnt();
   
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();
   std::this_thread::sleep_for(15ms);
-  size_t t1_runs_final = this->node->GetTimer1Cnt();
-  size_t t2_runs_final = this->node->GetTimer2Cnt();
+  int t1_runs_intermediate = this->node->GetTimer1Cnt();
+  int t2_runs_intermediate = this->node->GetTimer2Cnt();
 
-  executor.cancel();
+  this->node->ResetTimer1();
+  std::this_thread::sleep_for(15ms);
+  int t1_runs_final = this->node->GetTimer1Cnt();
+  int t2_runs_final = this->node->GetTimer2Cnt();
+
+  this->executor.cancel();
 
   // T1 and T2 should have the same initial count.
-  EXPECT_EQ(t1_runs_initial, t2_runs_initial);
+  EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1);
 
   // Expect that T1 has up to 15 more calls than t2. Add some buffer
   // to account for jitter.
-  EXPECT_EQ(t1_runs_initial, t1_runs_final);
-  EXPECT_LT(t2_runs_initial + 11, t2_runs_final);
+  EXPECT_EQ(t1_runs_initial, t1_runs_intermediate);
+  EXPECT_LT(t2_runs_initial + 11, t2_runs_intermediate);
 
-  // Clean up thread object
-  if (real_time_thread.joinable()) {
-    real_time_thread.join();
-  }
+  // Expect that by end of test, both are running properly again.
+  EXPECT_LT(t1_runs_intermediate + 11, t1_runs_final);
+  EXPECT_LT(t2_runs_intermediate + 11, t2_runs_final);
 }

From 43fc22d7208140c946071e3828fe25f2b2570830 Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Wed, 3 Jan 2024 16:32:00 -0800
Subject: [PATCH 09/13] remove unused method for size()

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 rclcpp/include/rclcpp/experimental/timers_manager.hpp | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
index fbd0e42bc4..197397e8b8 100644
--- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp
+++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp
@@ -272,15 +272,6 @@ class TimersManager
       return weak_heap_.empty();
     }
 
-    /**
-     * @brief Returns the size of the heap.
-     * @return the number of valid timers in the heap.
-     */
-    size_t size() const
-    {
-      return weak_heap_.size();
-    }
-
     /**
      * @brief This function restores the current object as a valid heap
      * and it returns a locked version of it.

From da1baa93a98dc6ea32cd7189d47f177ab38bcdb6 Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Wed, 10 Jan 2024 11:46:14 -0800
Subject: [PATCH 10/13] linting

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../rclcpp/experimental/timers_manager.cpp    |   9 +-
 .../test/rclcpp/executors/test_executors.cpp  | 111 ++++++++++--------
 2 files changed, 64 insertions(+), 56 deletions(-)

diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
index 822c5b3c27..39924afa56 100644
--- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp
+++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp
@@ -268,11 +268,10 @@ void TimersManager::run_timers()
       TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
       locked_heap.heapify();
       weak_timers_heap_.store(locked_heap);
-    }
-    else if (time_to_sleep.value() != std::chrono::nanoseconds::zero()) {
-        // If time_to_sleep is zero, we immediately execute. Otherwise, wait
-        // until timeout or notification that timers have been updated
-        timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
+    } else if (time_to_sleep.value() != std::chrono::nanoseconds::zero()) {
+      // If time_to_sleep is zero, we immediately execute. Otherwise, wait
+      // until timeout or notification that timers have been updated
+      timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;});
     }
 
     // Reset timers updated flag
diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
index 7c5f2515b4..d1cf215931 100644
--- a/rclcpp/test/rclcpp/executors/test_executors.cpp
+++ b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -819,58 +819,66 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) {
   EXPECT_EQ(kNumMessages, this->callback_count.load());
 }
 
-class TimerNode : public rclcpp::Node {
-   public:
-    TimerNode(std::string subname)
-        : Node("timer_node", subname) {
-
-        
-        timer1_ = rclcpp::create_timer(this->get_node_base_interface(), get_node_timers_interface(),
-                                      get_clock(), 1ms,
-                                      std::bind(&TimerNode::Timer1Callback, this));
-
-        timer2_ =
-              rclcpp::create_timer(this->get_node_base_interface(), get_node_timers_interface(),
-                                      get_clock(), 1ms,
-                                      std::bind(&TimerNode::Timer2Callback, this));
-    }
+class TimerNode : public rclcpp::Node
+{
+public:
+  explicit TimerNode(std::string subname)
+  : Node("timer_node", subname)
+  {
+    timer1_ = rclcpp::create_timer(
+      this->get_node_base_interface(), get_node_timers_interface(),
+      get_clock(), 1ms,
+      std::bind(&TimerNode::Timer1Callback, this));
+
+    timer2_ =
+      rclcpp::create_timer(
+      this->get_node_base_interface(), get_node_timers_interface(),
+      get_clock(), 1ms,
+      std::bind(&TimerNode::Timer2Callback, this));
+  }
 
-    int GetTimer1Cnt() { return cnt1_; }
-    int GetTimer2Cnt() { return cnt2_; }
+  int GetTimer1Cnt() {return cnt1_;}
+  int GetTimer2Cnt() {return cnt2_;}
 
-    void ResetTimer1() {
-        timer1_->reset();
-    }
+  void ResetTimer1()
+  {
+    timer1_->reset();
+  }
 
-    void ResetTimer2() {
-        timer2_->reset();
-    }
+  void ResetTimer2()
+  {
+    timer2_->reset();
+  }
 
-    void CancelTimer1() {
-      RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!");
-      timer1_->cancel();
-    }
+  void CancelTimer1()
+  {
+    RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!");
+    timer1_->cancel();
+  }
 
-    void CancelTimer2() {
-      RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!");
-      timer2_->cancel();
-    }
+  void CancelTimer2()
+  {
+    RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!");
+    timer2_->cancel();
+  }
 
-   private:
-    void Timer1Callback() {
-        RCLCPP_DEBUG(this->get_logger(), "Timer 1!");
-        cnt1_++;
-    }
+private:
+  void Timer1Callback()
+  {
+    RCLCPP_DEBUG(this->get_logger(), "Timer 1!");
+    cnt1_++;
+  }
 
-    void Timer2Callback() {
-        RCLCPP_DEBUG(this->get_logger(), "Timer 2!");
-        cnt2_++;
-    }
+  void Timer2Callback()
+  {
+    RCLCPP_DEBUG(this->get_logger(), "Timer 2!");
+    cnt2_++;
+  }
 
-    rclcpp::TimerBase::SharedPtr timer1_;
-    rclcpp::TimerBase::SharedPtr timer2_;
-    int cnt1_ = 0;
-    int cnt2_ = 0;
+  rclcpp::TimerBase::SharedPtr timer1_;
+  rclcpp::TimerBase::SharedPtr timer2_;
+  int cnt1_ = 0;
+  int cnt2_ = 0;
 };
 
 
@@ -897,9 +905,10 @@ class TestTimerCancelBehavior : public ::testing::Test
 
     // Spin the executor in a standalone thread
     executor.add_node(this->node);
-    standalone_thread = std::thread([this]() {
-      executor.spin();
-    });
+    standalone_thread = std::thread(
+      [this]() {
+        executor.spin();
+      });
   }
 
   void TearDown()
@@ -965,7 +974,7 @@ TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
   std::this_thread::sleep_for(10ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
-  
+
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
@@ -994,7 +1003,7 @@ TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
   std::this_thread::sleep_for(10ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
-  
+
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();
@@ -1024,7 +1033,7 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
   std::this_thread::sleep_for(10ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
-  
+
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
@@ -1063,7 +1072,7 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
   std::this_thread::sleep_for(10ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
-  
+
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();

From 385c3539083144889366b15df6e1030a2d4f01e6 Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Wed, 17 Jan 2024 12:02:51 -0800
Subject: [PATCH 11/13] relax timing constraints in tests

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../test/rclcpp/executors/test_executors.cpp  | 24 +++++++++----------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
index d1cf215931..6f19354516 100644
--- a/rclcpp/test/rclcpp/executors/test_executors.cpp
+++ b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -936,7 +936,7 @@ TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) {
   // Cancel to stop the spin after some time.
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer1();
-  std::this_thread::sleep_for(10ms);
+  std::this_thread::sleep_for(20ms);
   this->executor.cancel();
 
   int t1_runs = this->node->GetTimer1Cnt();
@@ -954,7 +954,7 @@ TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) {
   // Cancel to stop the spin after some time.
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(10ms);
+  std::this_thread::sleep_for(20ms);
   this->executor.cancel();
 
   int t1_runs = this->node->GetTimer1Cnt();
@@ -971,14 +971,14 @@ TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
   // Cancel to stop the spin after some time.
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer1();
-  std::this_thread::sleep_for(10ms);
+  std::this_thread::sleep_for(20ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(15ms);
+  std::this_thread::sleep_for(25ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1000,14 +1000,14 @@ TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
   // Cancel to stop the spin after some time.
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(10ms);
+  std::this_thread::sleep_for(20ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(15ms);
+  std::this_thread::sleep_for(25ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1030,19 +1030,19 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer1();
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(10ms);
+  std::this_thread::sleep_for(20ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(15ms);
+  std::this_thread::sleep_for(25ms);
   int t1_runs_intermediate = this->node->GetTimer1Cnt();
   int t2_runs_intermediate = this->node->GetTimer2Cnt();
 
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(15ms);
+  std::this_thread::sleep_for(25ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1069,19 +1069,19 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
   std::this_thread::sleep_for(5ms);
   this->node->CancelTimer1();
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(10ms);
+  std::this_thread::sleep_for(20ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(15ms);
+  std::this_thread::sleep_for(25ms);
   int t1_runs_intermediate = this->node->GetTimer1Cnt();
   int t2_runs_intermediate = this->node->GetTimer2Cnt();
 
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(15ms);
+  std::this_thread::sleep_for(25ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 

From 2fb28b988217a58dcedf49ea6a4895647f0549c8 Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Wed, 17 Jan 2024 14:03:34 -0800
Subject: [PATCH 12/13] further relax timing constraints to ensure windows
 tests are not flaky.

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../test/rclcpp/executors/test_executors.cpp  | 64 +++++++++----------
 1 file changed, 32 insertions(+), 32 deletions(-)

diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
index 6f19354516..77bcdeea67 100644
--- a/rclcpp/test/rclcpp/executors/test_executors.cpp
+++ b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -934,16 +934,16 @@ TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) {
   // executor, which is the most common usecase.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(5ms);
+  std::this_thread::sleep_for(100ms);
   this->node->CancelTimer1();
-  std::this_thread::sleep_for(20ms);
+  std::this_thread::sleep_for(300ms);
   this->executor.cancel();
 
   int t1_runs = this->node->GetTimer1Cnt();
   int t2_runs = this->node->GetTimer2Cnt();
   EXPECT_NE(t1_runs, t2_runs);
   // Check that t2 has significantly more calls
-  EXPECT_LT(t1_runs + 5, t2_runs);
+  EXPECT_LT(t1_runs + 150, t2_runs);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) {
@@ -952,16 +952,16 @@ TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) {
   // executor, which is the most common usecase.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(5ms);
+  std::this_thread::sleep_for(100ms);
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(20ms);
+  std::this_thread::sleep_for(300ms);
   this->executor.cancel();
 
   int t1_runs = this->node->GetTimer1Cnt();
   int t2_runs = this->node->GetTimer2Cnt();
   EXPECT_NE(t1_runs, t2_runs);
   // Check that t1 has significantly more calls
-  EXPECT_LT(t2_runs + 5, t1_runs);
+  EXPECT_LT(t2_runs + 150, t1_runs);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
@@ -969,16 +969,16 @@ TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
   // and that the cancelled timer starts executing normally once reset manually.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(5ms);
+  std::this_thread::sleep_for(100ms);
   this->node->CancelTimer1();
-  std::this_thread::sleep_for(20ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(25ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -986,11 +986,11 @@ TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
 
   // T1 should have been restarted, and execute about 15 additional times.
   // Check 10 greater than initial, to account for some timing jitter.
-  EXPECT_LT(t1_runs_initial + 10, t1_runs_final);
+  EXPECT_LT(t1_runs_initial + 150, t1_runs_final);
 
-  EXPECT_LT(t1_runs_initial + 5, t2_runs_initial);
+  EXPECT_LT(t1_runs_initial + 150, t2_runs_initial);
   // Check that t2 has significantly more calls, and keeps getting called.
-  EXPECT_LT(t2_runs_initial + 13, t2_runs_final);
+  EXPECT_LT(t2_runs_initial + 150, t2_runs_final);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
@@ -998,16 +998,16 @@ TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
   // and that the cancelled timer starts executing normally once reset manually.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(5ms);
+  std::this_thread::sleep_for(100ms);
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(20ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(25ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1015,11 +1015,11 @@ TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
 
   // T2 should have been restarted, and execute about 15 additional times.
   // Check 10 greater than initial, to account for some timing jitter.
-  EXPECT_LT(t2_runs_initial + 10, t2_runs_final);
+  EXPECT_LT(t2_runs_initial + 150, t2_runs_final);
 
-  EXPECT_LT(t2_runs_initial + 5, t1_runs_initial);
+  EXPECT_LT(t2_runs_initial + 150, t1_runs_initial);
   // Check that t1 has significantly more calls, and keeps getting called.
-  EXPECT_LT(t1_runs_initial + 13, t1_runs_final);
+  EXPECT_LT(t1_runs_initial + 150, t1_runs_final);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
@@ -1027,22 +1027,22 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
   // Ensure that only the reset timer is executed.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(5ms);
+  std::this_thread::sleep_for(100ms);
   this->node->CancelTimer1();
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(20ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(25ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_intermediate = this->node->GetTimer1Cnt();
   int t2_runs_intermediate = this->node->GetTimer2Cnt();
 
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(25ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1054,11 +1054,11 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
   // Expect that T1 has up to 15 more calls than t2. Add some buffer
   // to account for jitter.
   EXPECT_EQ(t2_runs_initial, t2_runs_intermediate);
-  EXPECT_LT(t1_runs_initial + 11, t1_runs_intermediate);
+  EXPECT_LT(t1_runs_initial + 150, t1_runs_intermediate);
 
   // Expect that by end of test, both are running properly again.
-  EXPECT_LT(t1_runs_intermediate + 11, t1_runs_final);
-  EXPECT_LT(t2_runs_intermediate + 11, t2_runs_final);
+  EXPECT_LT(t1_runs_intermediate + 150, t1_runs_final);
+  EXPECT_LT(t2_runs_intermediate + 150, t2_runs_final);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
@@ -1066,22 +1066,22 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
   // Ensure that only the reset timer is executed.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(5ms);
+  std::this_thread::sleep_for(100ms);
   this->node->CancelTimer1();
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(20ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(25ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_intermediate = this->node->GetTimer1Cnt();
   int t2_runs_intermediate = this->node->GetTimer2Cnt();
 
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(25ms);
+  std::this_thread::sleep_for(300ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1093,9 +1093,9 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
   // Expect that T1 has up to 15 more calls than t2. Add some buffer
   // to account for jitter.
   EXPECT_EQ(t1_runs_initial, t1_runs_intermediate);
-  EXPECT_LT(t2_runs_initial + 11, t2_runs_intermediate);
+  EXPECT_LT(t2_runs_initial + 150, t2_runs_intermediate);
 
   // Expect that by end of test, both are running properly again.
-  EXPECT_LT(t1_runs_intermediate + 11, t1_runs_final);
-  EXPECT_LT(t2_runs_intermediate + 11, t2_runs_final);
+  EXPECT_LT(t1_runs_intermediate + 150, t1_runs_final);
+  EXPECT_LT(t2_runs_intermediate + 150, t2_runs_final);
 }

From 666e9d5cce3dfd7ddee4f7bc0ffd02b30b9ebfbb Mon Sep 17 00:00:00 2001
From: Matt Condino <mwcondino@gmail.com>
Date: Thu, 18 Jan 2024 14:23:48 -0800
Subject: [PATCH 13/13] use sim clock for tests, pub clock at .25 realtime
 rate.

Signed-off-by: Matt Condino <mwcondino@gmail.com>
---
 .../test/rclcpp/executors/test_executors.cpp  | 162 ++++++++++++++----
 1 file changed, 130 insertions(+), 32 deletions(-)

diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
index 77bcdeea67..35eea01038 100644
--- a/rclcpp/test/rclcpp/executors/test_executors.cpp
+++ b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -36,8 +36,10 @@
 #include "rclcpp/duration.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time_source.hpp"
 
 #include "test_msgs/msg/empty.hpp"
+#include "rosgraph_msgs/msg/clock.hpp"
 
 using namespace std::chrono_literals;
 
@@ -881,6 +883,89 @@ class TimerNode : public rclcpp::Node
   int cnt2_ = 0;
 };
 
+// Sets up a separate thread to publish /clock messages.
+// Clock rate relative to real clock is controlled by realtime_update_rate.
+// This is set conservatively slow to ensure unit tests are reliable on Windows
+// environments, where timing performance is subpar.
+//
+// Use `sleep_for` in tests to advance the clock. Clock should run and be published
+// in separate thread continuously to ensure correct behavior in node under test.
+class ClockPublisher : public rclcpp::Node
+{
+public:
+  explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f)
+  : Node("clock_publisher"),
+    ros_update_duration_(0, 0),
+    realtime_clock_step_(0, 0),
+    rostime_(0, 0)
+  {
+    clock_publisher_ = this->create_publisher<rosgraph_msgs::msg::Clock>("clock", 10);
+    realtime_clock_step_ =
+      rclcpp::Duration::from_seconds(simulated_clock_step / realtime_update_rate);
+    ros_update_duration_ = rclcpp::Duration::from_seconds(simulated_clock_step);
+
+    timer_thread_ = std::thread(&ClockPublisher::RunTimer, this);
+  }
+
+  ~ClockPublisher()
+  {
+    running_ = false;
+    if (timer_thread_.joinable()) {
+      timer_thread_.join();
+    }
+  }
+
+  void sleep_for(rclcpp::Duration duration)
+  {
+    rclcpp::Time start_time(0, 0, RCL_ROS_TIME);
+    {
+      const std::lock_guard<std::mutex> lock(mutex_);
+      start_time = rostime_;
+    }
+    rclcpp::Time current_time = start_time;
+
+    while (true) {
+      {
+        const std::lock_guard<std::mutex> lock(mutex_);
+        current_time = rostime_;
+      }
+      if ((current_time - start_time) >= duration) {
+        return;
+      }
+      std::this_thread::sleep_for(realtime_clock_step_.to_chrono<std::chrono::milliseconds>());
+      rostime_ += ros_update_duration_;
+    }
+  }
+
+private:
+  void RunTimer()
+  {
+    while (running_) {
+      PublishClock();
+      std::this_thread::sleep_for(realtime_clock_step_.to_chrono<std::chrono::milliseconds>());
+    }
+  }
+
+  void PublishClock()
+  {
+    const std::lock_guard<std::mutex> lock(mutex_);
+    auto message = rosgraph_msgs::msg::Clock();
+    message.clock = rostime_;
+    clock_publisher_->publish(message);
+  }
+
+  rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_publisher_;
+
+  rclcpp::Duration ros_update_duration_;
+  rclcpp::Duration realtime_clock_step_;
+  // Rostime must be guarded by a mutex, since accessible in running thread
+  // as well as sleep_for
+  rclcpp::Time rostime_;
+  std::mutex mutex_;
+  std::thread timer_thread_;
+  std::atomic<bool> running_ = true;
+};
+
 
 template<typename T>
 class TestTimerCancelBehavior : public ::testing::Test
@@ -902,6 +987,17 @@ class TestTimerCancelBehavior : public ::testing::Test
     std::stringstream test_name;
     test_name << test_info->test_case_name() << "_" << test_info->name();
     node = std::make_shared<TimerNode>(test_name.str());
+    param_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+    ASSERT_TRUE(param_client->wait_for_service(5s));
+
+    auto set_parameters_results = param_client->set_parameters(
+      {rclcpp::Parameter("use_sim_time", false)});
+    for (auto & result : set_parameters_results) {
+      ASSERT_TRUE(result.successful);
+    }
+
+    // Run standalone thread to publish clock time
+    sim_clock_node = std::make_shared<ClockPublisher>();
 
     // Spin the executor in a standalone thread
     executor.add_node(this->node);
@@ -922,6 +1018,8 @@ class TestTimerCancelBehavior : public ::testing::Test
   }
 
   std::shared_ptr<TimerNode> node;
+  std::shared_ptr<ClockPublisher> sim_clock_node;
+  rclcpp::SyncParametersClient::SharedPtr param_client;
   std::thread standalone_thread;
   T executor;
 };
@@ -934,16 +1032,16 @@ TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) {
   // executor, which is the most common usecase.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(100ms);
+  this->sim_clock_node->sleep_for(50ms);
   this->node->CancelTimer1();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   this->executor.cancel();
 
   int t1_runs = this->node->GetTimer1Cnt();
   int t2_runs = this->node->GetTimer2Cnt();
   EXPECT_NE(t1_runs, t2_runs);
   // Check that t2 has significantly more calls
-  EXPECT_LT(t1_runs + 150, t2_runs);
+  EXPECT_LT(t1_runs + 50, t2_runs);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) {
@@ -952,16 +1050,16 @@ TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) {
   // executor, which is the most common usecase.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(100ms);
+  this->sim_clock_node->sleep_for(50ms);
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   this->executor.cancel();
 
   int t1_runs = this->node->GetTimer1Cnt();
   int t2_runs = this->node->GetTimer2Cnt();
   EXPECT_NE(t1_runs, t2_runs);
   // Check that t1 has significantly more calls
-  EXPECT_LT(t2_runs + 150, t1_runs);
+  EXPECT_LT(t2_runs + 50, t1_runs);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
@@ -969,16 +1067,16 @@ TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
   // and that the cancelled timer starts executing normally once reset manually.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(100ms);
+  this->sim_clock_node->sleep_for(50ms);
   this->node->CancelTimer1();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -986,11 +1084,11 @@ TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) {
 
   // T1 should have been restarted, and execute about 15 additional times.
   // Check 10 greater than initial, to account for some timing jitter.
-  EXPECT_LT(t1_runs_initial + 150, t1_runs_final);
+  EXPECT_LT(t1_runs_initial + 50, t1_runs_final);
 
-  EXPECT_LT(t1_runs_initial + 150, t2_runs_initial);
+  EXPECT_LT(t1_runs_initial + 50, t2_runs_initial);
   // Check that t2 has significantly more calls, and keeps getting called.
-  EXPECT_LT(t2_runs_initial + 150, t2_runs_final);
+  EXPECT_LT(t2_runs_initial + 50, t2_runs_final);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
@@ -998,16 +1096,16 @@ TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
   // and that the cancelled timer starts executing normally once reset manually.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(100ms);
+  this->sim_clock_node->sleep_for(50ms);
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1015,11 +1113,11 @@ TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) {
 
   // T2 should have been restarted, and execute about 15 additional times.
   // Check 10 greater than initial, to account for some timing jitter.
-  EXPECT_LT(t2_runs_initial + 150, t2_runs_final);
+  EXPECT_LT(t2_runs_initial + 50, t2_runs_final);
 
-  EXPECT_LT(t2_runs_initial + 150, t1_runs_initial);
+  EXPECT_LT(t2_runs_initial + 50, t1_runs_initial);
   // Check that t1 has significantly more calls, and keeps getting called.
-  EXPECT_LT(t1_runs_initial + 150, t1_runs_final);
+  EXPECT_LT(t1_runs_initial + 50, t1_runs_final);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
@@ -1027,22 +1125,22 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
   // Ensure that only the reset timer is executed.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(100ms);
+  this->sim_clock_node->sleep_for(50ms);
   this->node->CancelTimer1();
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_intermediate = this->node->GetTimer1Cnt();
   int t2_runs_intermediate = this->node->GetTimer2Cnt();
 
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1054,11 +1152,11 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) {
   // Expect that T1 has up to 15 more calls than t2. Add some buffer
   // to account for jitter.
   EXPECT_EQ(t2_runs_initial, t2_runs_intermediate);
-  EXPECT_LT(t1_runs_initial + 150, t1_runs_intermediate);
+  EXPECT_LT(t1_runs_initial + 50, t1_runs_intermediate);
 
   // Expect that by end of test, both are running properly again.
-  EXPECT_LT(t1_runs_intermediate + 150, t1_runs_final);
-  EXPECT_LT(t2_runs_intermediate + 150, t2_runs_final);
+  EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final);
+  EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final);
 }
 
 TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
@@ -1066,22 +1164,22 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
   // Ensure that only the reset timer is executed.
 
   // Cancel to stop the spin after some time.
-  std::this_thread::sleep_for(100ms);
+  this->sim_clock_node->sleep_for(50ms);
   this->node->CancelTimer1();
   this->node->CancelTimer2();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_initial = this->node->GetTimer1Cnt();
   int t2_runs_initial = this->node->GetTimer2Cnt();
 
   // Manually reset timer 1, then sleep again
   // Counts should update.
   this->node->ResetTimer2();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_intermediate = this->node->GetTimer1Cnt();
   int t2_runs_intermediate = this->node->GetTimer2Cnt();
 
   this->node->ResetTimer1();
-  std::this_thread::sleep_for(300ms);
+  this->sim_clock_node->sleep_for(150ms);
   int t1_runs_final = this->node->GetTimer1Cnt();
   int t2_runs_final = this->node->GetTimer2Cnt();
 
@@ -1093,9 +1191,9 @@ TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) {
   // Expect that T1 has up to 15 more calls than t2. Add some buffer
   // to account for jitter.
   EXPECT_EQ(t1_runs_initial, t1_runs_intermediate);
-  EXPECT_LT(t2_runs_initial + 150, t2_runs_intermediate);
+  EXPECT_LT(t2_runs_initial + 50, t2_runs_intermediate);
 
   // Expect that by end of test, both are running properly again.
-  EXPECT_LT(t1_runs_intermediate + 150, t1_runs_final);
-  EXPECT_LT(t2_runs_intermediate + 150, t2_runs_final);
+  EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final);
+  EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final);
 }