new implementation of get_head_timeout_unsafe

Signed-off-by: Soragna, Alberto <[email protected]>

Author: alsora

rclcpp/include/rclcpp/executors/timers_manager.hpp

@@ -65,7 +65,7 @@ class TimersManager
 
   /**
    * @brief Construct a new TimersManager object
-   * 
+   *
    * @param context custom context to be used.
    * Shared ownership of the context is held until destruction.
    */
@@ -79,7 +79,7 @@ class TimersManager
   /**
    * @brief Adds a new timer to the storage, maintaining weak ownership of it.
    * Function is thread safe and it can be called regardless of the state of the timers thread.
-   * 
+   *
    * @param timer the timer to add.
    */
   void add_timer(rclcpp::TimerBase::SharedPtr timer);
@@ -88,7 +88,7 @@ class TimersManager
    * @brief Remove a single timer from the object storage.
    * Will do nothing if the timer was not being stored here.
    * Function is thread safe and it can be called regardless of the state of the timers thread.
-   * 
+   *
    * @param timer the timer to remove.
    */
   void remove_timer(rclcpp::TimerBase::SharedPtr timer);
@@ -121,7 +121,7 @@ class TimersManager
   /**
    * @brief Executes head timer if ready at time point.
    * Function is thread safe, but it will throw an error if the timers thread is running.
-   * 
+   *
    * @param tp the time point to check for, where `max()` denotes that no check will be performed.
    * @return true if head timer was ready at time point.
    */
@@ -132,7 +132,7 @@ class TimersManager
   /**
    * @brief Get the amount of time before the next timer expires.
    * Function is thread safe, but it will throw an error if the timers thread is running.
-   * 
+   *
    * @return std::chrono::nanoseconds to wait,
    * the returned value could be negative if the timer is already expired
    * or MAX_TIME if there are no timers stored in the object.
@@ -203,10 +203,26 @@ class TimersManager
       return removed;
     }
 
+    /**
+     * @brief Returns a const reference to the front element
+     */
+    const WeakTimerPtr & front() const
+    {
+      return weak_heap_.front();
+    }
+
+    /**
+     * @brief Returns whether the heap is empty or not
+     */
+    bool empty() const
+    {
+      return weak_heap_.empty();
+    }
+
     /**
      * @brief This function restores the current object as a valid heap
      * and it returns a locked version of it.
-     * It is the only public API to access the stored timers.
+     * It is the only public API to access and manipulate the stored timers.
      *
      * @return TimersHeap owned timers corresponding to the current object
      */
@@ -411,13 +427,7 @@ class TimersManager
    * or MAX_TIME 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(const TimersHeap & heap)
-  {
-    if (heap.empty()) {
-      return MAX_TIME;
-    }
-    return (heap.front())->time_until_trigger();
-  }
+  std::chrono::nanoseconds get_head_timeout_unsafe();
 
   /**
    * @brief Executes all the timers currently ready when the function is invoked

rclcpp/src/rclcpp/executors/timers_manager.cpp

@@ -95,8 +95,7 @@ std::chrono::nanoseconds TimersManager::get_head_timeout()
   }
 
   std::unique_lock<std::mutex> lock(timers_mutex_);
-  TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();
-  return this->get_head_timeout_unsafe(timers_heap);
+  return this->get_head_timeout_unsafe();
 }
 
 void TimersManager::execute_ready_timers()
@@ -108,10 +107,7 @@ void TimersManager::execute_ready_timers()
   }
 
   std::unique_lock<std::mutex> lock(timers_mutex_);
-
-  TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();
-  this->execute_ready_timers_unsafe(timers_heap);
-  weak_timers_heap_.store(timers_heap);
+  this->execute_ready_timers_unsafe();
 }
 
 bool TimersManager::execute_head_timer(
@@ -153,55 +149,77 @@ bool TimersManager::execute_head_timer(
   return false;
 }
 
-void TimersManager::execute_ready_timers_unsafe(TimersHeap & heap)
+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()) {
+    return MAX_TIME;
+  }
+
+  // Weak heap is not empty, so try to lock the first element
+  TimerPtr head_timer = weak_timers_heap_.front().lock();
+  // If it is still a valid pointer, it is guaranteed to be the correct head
+  if (head_timer != nullptr) {
+    return head_timer->time_until_trigger();
+  }
+
+  // If the first elements has expired, we can't make other assumptions on the heap
+  // and we need to entirely validate it.
+  TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+
+  // NOTE: the following operations will not modify any element in the heap, so we
+  // don't have to call `weak_timers_heap_.store(locked_heap)` at the end.
+
+  if (locked_heap.empty()) {
+    return MAX_TIME;
+  }
+  return locked_heap.front()->time_until_trigger();
+}
+
+void TimersManager::execute_ready_timers_unsafe()
+{
+  // We start by locking the timers
+  TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+
   // Nothing to do if we don't have any timer
-  if (heap.empty()) {
+  if (locked_heap.empty()) {
     return;
   }
 
   // Keep executing timers until they are ready and they were already ready when we started.
   // The second check prevents this function from blocking indefinitely if the
   // time required for executing the timers is longer than their period.
 
-  TimerPtr head = heap.front();
+  TimerPtr head = locked_heap.front();
   auto start_time = std::chrono::steady_clock::now();
   while (head->is_ready() && this->timer_was_ready_at_tp(head, start_time)) {
     // Execute head timer
     head->execute_callback();
     // Executing a timer will result in updating its time_until_trigger, so re-heapify
-    heap.heapify_root();
+    locked_heap.heapify_root();
     // Get new head timer
-    head = heap.front();
+    head = locked_heap.front();
   }
+
+  // After having performed work on the locked heap we reflect the changes to weak one.
+  // Timers will be already sorted the next time we need them if none went out of scope.
+  weak_timers_heap_.store(locked_heap);
 }
 
 void TimersManager::run_timers()
 {
-  std::chrono::nanoseconds time_to_sleep;
-  {
-    std::unique_lock<std::mutex> lock(timers_mutex_);
-    TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();
-    time_to_sleep = this->get_head_timeout_unsafe(timers_heap);
-  }
-
   while (rclcpp::ok(context_) && running_) {
     // Lock mutex
     std::unique_lock<std::mutex> timers_lock(timers_mutex_);
 
+    std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe();
     // Wait until timeout or notification that timers have been updated
     timers_cv_.wait_for(timers_lock, time_to_sleep, [this]() {return timers_updated_;});
     // Reset timers updated flag
     timers_updated_ = false;
 
-    // Get ownership of timers
-    TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();
     // Execute timers
-    this->execute_ready_timers_unsafe(timers_heap);
-    // Store updated order of elements to efficiently re-use it next iteration
-    weak_timers_heap_.store(timers_heap);
-    // Get next timeout
-    time_to_sleep = this->get_head_timeout_unsafe(timers_heap);
+    this->execute_ready_timers_unsafe();
   }
 
   // Make sure the running flag is set to false when we exit from this function