rate_throttle.hpp

#ifndef MRS_LIB_INTERNAL_RATE_THROTTLE_HPP_
#define MRS_LIB_INTERNAL_RATE_THROTTLE_HPP_
 
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <limits>
#include <optional>
 
#include <rclcpp/duration.hpp>
#include <rclcpp/time.hpp>
 
namespace mrs_lib::internal
{

  class RateThrottle
  {
  public:
    explicit RateThrottle(const double rate) : RateThrottle(std::chrono::nanoseconds(rate > 0 ? std::llround(std::min(1e9 / rate, double(max_period_ns_))) : 0))
    {
    }

    explicit RateThrottle(const std::chrono::nanoseconds period)
        : period_(rclcpp::Duration::from_nanoseconds(std::clamp<int64_t>(period.count(), 0, max_period_ns_))),
          jitter_tolerance_(rclcpp::Duration::from_nanoseconds(period_.nanoseconds() / 4)),
          max_catch_up_(rclcpp::Duration::from_nanoseconds(period_.nanoseconds() / 2)),
          clock_jump_threshold_(rclcpp::Duration::from_nanoseconds((3 * period_.nanoseconds()) / 2))
    {
    }

    bool accept(const rclcpp::Time& now)
    {
      if (period_.nanoseconds() <= 0)
      {
        return true;
      }
 
      // first message, or the clock jumped backwards (e.g., sim time reset)
      if (!next_deadline_ || now < *next_deadline_ - clock_jump_threshold_)
      {
        next_deadline_ = now;
      }
 
      if (now < *next_deadline_ - jitter_tolerance_)
      {
        return false;
      }
 
      next_deadline_ = std::max(*next_deadline_ + period_, now + max_catch_up_);
 
      return true;
    }
 
  private:
    // keeps the deadline arithmetic far from int64 overflow (~73 years)
    static constexpr int64_t max_period_ns_ = std::numeric_limits<int64_t>::max() / 4;
 
    rclcpp::Duration period_;
 
    rclcpp::Duration jitter_tolerance_;     // a message this much before the deadline still passes
    rclcpp::Duration max_catch_up_;         // the next deadline is at least this far after a passed message, limits catch-up after a pause
    rclcpp::Duration clock_jump_threshold_; // a message this much before the deadline means the clock jumped backwards
 
    std::optional<rclcpp::Time> next_deadline_;
  };
 
} // namespace mrs_lib::internal
 
#endif // MRS_LIB_INTERNAL_RATE_THROTTLE_HPP_