otimal

include/robot_depth_image_proc/processing_rate_limiter.h

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <chrono>
+#include <cmath>
+
+namespace depth_image_proc
+{
+
+// Wall-clock limiter for CPU-bound sensor callbacks. It deliberately does not
+// catch up after a delayed callback: processing a burst of old frames would
+// increase latency and CPU without helping a latest-sample costmap consumer.
+class ProcessingRateLimiter
+{
+public:
+  using Clock = std::chrono::steady_clock;
+  using TimePoint = Clock::time_point;
+
+  explicit ProcessingRateLimiter(double rate_hz = 0.0)
+  {
+    setRate(rate_hz);
+  }
+
+  void setRate(double rate_hz)
+  {
+    enabled_ = std::isfinite(rate_hz) && rate_hz > 0.0;
+    if (enabled_)
+    {
+      period_ = std::chrono::duration_cast<Clock::duration>(
+          std::chrono::duration<double>(1.0 / rate_hz));
+      early_tolerance_ = period_ / 20;
+    }
+    reset();
+  }
+
+  bool shouldProcess()
+  {
+    return shouldProcessAt(Clock::now());
+  }
+
+  bool shouldProcessAt(const TimePoint now)
+  {
+    if (!enabled_)
+      return true;
+
+    if (!initialized_)
+    {
+      initialized_ = true;
+      next_process_time_ = now + period_;
+      return true;
+    }
+
+    if (now + early_tolerance_ >= next_process_time_)
+    {
+      next_process_time_ += period_;
+      if (now + early_tolerance_ >= next_process_time_)
+        next_process_time_ = now + period_;
+      return true;
+    }
+    return false;
+  }
+
+  void reset()
+  {
+    initialized_ = false;
+    next_process_time_ = TimePoint{};
+  }
+
+  bool enabled() const
+  {
+    return enabled_;
+  }
+
+private:
+  bool enabled_{false};
+  bool initialized_{false};
+  Clock::duration period_{Clock::duration::zero()};
+  Clock::duration early_tolerance_{Clock::duration::zero()};
+  TimePoint next_process_time_{};
+};
+
+}  // namespace depth_image_proc