timers_manager.hpp

// Copyright 2023 iRobot Corporation.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_
#define RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_
 
#include <algorithm>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <thread>
#include <utility>
#include <vector>
#include "rclcpp/context.hpp"
#include "rclcpp/timer.hpp"
 
namespace rclcpp
{
namespace experimental
{
 
class TimersManager
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimersManager)
 
  
  RCLCPP_PUBLIC
  TimersManager(
    std::shared_ptr<rclcpp::Context> context,
    std::function<void(const rclcpp::TimerBase *,
    const std::shared_ptr<void> &)> on_ready_callback = nullptr);
 
  RCLCPP_PUBLIC
  ~TimersManager();
 
  RCLCPP_PUBLIC
  void add_timer(rclcpp::TimerBase::SharedPtr timer);
 
  RCLCPP_PUBLIC
  void remove_timer(rclcpp::TimerBase::SharedPtr timer);
 
  RCLCPP_PUBLIC
  void clear();
 
  RCLCPP_PUBLIC
  void start();
 
  RCLCPP_PUBLIC
  void stop();
 
  RCLCPP_PUBLIC
  size_t get_number_ready_timers();
 
  RCLCPP_PUBLIC
  bool execute_head_timer();
 
  RCLCPP_PUBLIC
  void execute_ready_timer(const rclcpp::TimerBase * timer_id, const std::shared_ptr<void> & data);
 
  RCLCPP_PUBLIC
  std::optional<std::chrono::nanoseconds> get_head_timeout();
 
private:
  RCLCPP_DISABLE_COPY(TimersManager)
 
  using TimerPtr = rclcpp::TimerBase::SharedPtr;
  using WeakTimerPtr = rclcpp::TimerBase::WeakPtr;
 
  // Forward declaration
  class TimersHeap;
 
  class WeakTimersHeap
  {
public:
    bool add_timer(TimerPtr timer)
    {
      TimersHeap locked_heap = this->validate_and_lock();
      bool added = locked_heap.add_timer(std::move(timer));
 
      if (added) {
        // Re-create the weak heap with the new timer added
        this->store(locked_heap);
      }
 
      return added;
    }
 
    bool remove_timer(TimerPtr timer)
    {
      TimersHeap locked_heap = this->validate_and_lock();
      bool removed = locked_heap.remove_timer(std::move(timer));
 
      if (removed) {
        // Re-create the weak heap with the timer removed
        this->store(locked_heap);
      }
 
      return removed;
    }
 
    TimerPtr get_timer(const rclcpp::TimerBase * timer_id)
    {
      for (auto & weak_timer : weak_heap_) {
        auto timer = weak_timer.lock();
        if (timer.get() == timer_id) {
          return timer;
        }
      }
      return nullptr;
    }
 
    const WeakTimerPtr & front() const
    {
      return weak_heap_.front();
    }
 
    bool empty() const
    {
      return weak_heap_.empty();
    }
 
    TimersHeap validate_and_lock()
    {
      TimersHeap locked_heap;
      bool any_timer_destroyed = false;
 
      for (auto weak_timer : weak_heap_) {
        auto timer = weak_timer.lock();
        if (timer) {
          // This timer is valid, so add it to the locked heap
          // Note: we access friend private `owned_heap_` member field.
          locked_heap.owned_heap_.push_back(std::move(timer));
        } else {
          // This timer went out of scope, so we don't add it to locked heap
          // and we mark the corresponding flag.
          // It's not needed to erase it from weak heap, as we are going to re-heapify.
          // Note: we can't exit from the loop here, as we need to find all valid timers.
          any_timer_destroyed = true;
        }
      }
 
      // If a timer has gone out of scope, then the remaining elements do not represent
      // a valid heap anymore. We need to re-heapify the timers heap.
      if (any_timer_destroyed) {
        locked_heap.heapify();
        // Re-create the weak heap now that elements have been heapified again
        this->store(locked_heap);
      }
 
      return locked_heap;
    }
 
    void store(const TimersHeap & heap)
    {
      weak_heap_.clear();
      // Note: we access friend private `owned_heap_` member field.
      for (auto t : heap.owned_heap_) {
        weak_heap_.push_back(t);
      }
    }
 
    void clear()
    {
      weak_heap_.clear();
    }
 
private:
    std::vector<WeakTimerPtr> weak_heap_;
  };
 
  class TimersHeap
  {
public:
    bool add_timer(TimerPtr timer)
    {
      // Nothing to do if the timer is already stored here
      auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
      if (it != owned_heap_.end()) {
        return false;
      }
 
      owned_heap_.push_back(std::move(timer));
      std::push_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
 
      return true;
    }
 
    bool remove_timer(TimerPtr timer)
    {
      // Nothing to do if the timer is not stored here
      auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
      if (it == owned_heap_.end()) {
        return false;
      }
 
      owned_heap_.erase(it);
      this->heapify();
 
      return true;
    }
 
    TimerPtr & front()
    {
      return owned_heap_.front();
    }
 
    const TimerPtr & front() const
    {
      return owned_heap_.front();
    }
 
    bool empty() const
    {
      return owned_heap_.empty();
    }
 
    size_t size() const
    {
      return owned_heap_.size();
    }
 
    size_t get_number_ready_timers() const
    {
      size_t ready_timers = 0;
 
      for (TimerPtr t : owned_heap_) {
        if (t->is_ready()) {
          ready_timers++;
        }
      }
 
      return ready_timers;
    }
 
    void heapify_root()
    {
      // The following code is a more efficient version than doing
      // pop_heap, pop_back, push_back, push_heap
      // as it removes the need for the last push_heap
 
      // Push the modified element (i.e. the current root) at the bottom of the heap
      owned_heap_.push_back(owned_heap_[0]);
      // Exchange first and last-1 elements and reheapify
      std::pop_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
      // Remove last element
      owned_heap_.pop_back();
    }
 
    void heapify()
    {
      std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
    }
 
    void clear_timers_on_reset_callbacks()
    {
      for (TimerPtr & t : owned_heap_) {
        t->clear_on_reset_callback();
      }
    }
 
    friend TimersHeap WeakTimersHeap::validate_and_lock();
 
    friend void WeakTimersHeap::store(const TimersHeap & heap);
 
private:
    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();
    }
 
    std::vector<TimerPtr> owned_heap_;
  };
 
  void run_timers();
 
  std::optional<std::chrono::nanoseconds> get_head_timeout_unsafe();
 
  void execute_ready_timers_unsafe();
 
  // Callback to be called when timer is ready
  std::function<void(const rclcpp::TimerBase *,
    const std::shared_ptr<void> &)> on_ready_callback_ = nullptr;
 
  // Thread used to run the timers execution task
  std::thread timers_thread_;
  // Protects access to timers
  std::mutex timers_mutex_;
  // Protects access to stop()
  std::mutex stop_mutex_;
  // Notifies the timers thread whenever timers are added/removed
  std::condition_variable timers_cv_;
  // Flag used as predicate by timers_cv_ that denotes one or more timers being added/removed
  bool timers_updated_ {false};
  // Indicates whether the timers thread is currently running or not
  std::atomic<bool> running_ {false};
  // Parent context used to understand if ROS is still active
  std::shared_ptr<rclcpp::Context> context_;
  // Timers heap storage with weak ownership
  WeakTimersHeap weak_timers_heap_;
};
 
}  // namespace experimental
}  // namespace rclcpp
 
#endif  // RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_