[events executor] - Fix Behavior with Timer Cancel

rclcpp/include/rclcpp/experimental/timers_manager.hpp

@@ -22,10 +22,10 @@
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <thread>
 #include <utility>
 #include <vector>
-
 #include "rclcpp/context.hpp"
 #include "rclcpp/timer.hpp"
 
@@ -172,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)
@@ -512,12 +513,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();
+  std::optional<std::chrono::nanoseconds> get_head_timeout_unsafe();
 
   /**
    * @brief Executes all the timers currently ready when the function is invoked

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;
   }
 

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_) {
@@ -169,7 +169,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
   }
 }
 
-std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
+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()) {
@@ -191,7 +191,9 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
     }
     head_timer = locked_heap.front();
   }
-
+  if (head_timer->is_canceled()) {
+    return std::nullopt;
+  }
   return head_timer->time_until_trigger();
 }
 
@@ -242,17 +244,34 @@ void TimersManager::run_timers()
     // Lock mutex
     std::unique_lock<std::mutex> lock(timers_mutex_);
 
-    std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe();
+    std::optional<std::chrono::nanoseconds> time_to_sleep = get_head_timeout_unsafe();
+
+    // 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();
+    }
 
-    // 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 {
-        // Wait until notification that timers have been updated
-        timers_cv_.wait(lock, [this]() {return timers_updated_;});
-      }
+    // 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