common: C++ RateKeeper (#29374)

* c++ RateKeeper

* add to files_common

* use util::random_int

* improve monotor_time

* remove ~ratekeeper
old-commit-hash: 3eef63af9b4f4a25bc4c3a0d6ad450bba9f62205

common/SConscript

@@ -12,6 +12,7 @@ common_libs = [
   'util.cc',
   'i2c.cc',
   'watchdog.cc',
+  'ratekeeper.cc'
 ]
 
 if arch != "Darwin":
@@ -29,6 +30,7 @@ Export('_common', '_gpucommon')
 if GetOption('test'):
   env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
   env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])
+  env.Program('tests/test_ratekeeper', ['tests/test_ratekeeper.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])
 
 # Cython
 envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])

common/ratekeeper.cc

@@ -0,0 +1,38 @@
+#include "common/ratekeeper.h"
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+RateKeeper::RateKeeper(const std::string &name, float rate, float print_delay_threshold)
+    : name(name),
+      print_delay_threshold(std::max(0.f, print_delay_threshold)) {
+  interval = 1 / rate;
+  last_monitor_time = seconds_since_boot();
+  next_frame_time = last_monitor_time + interval;
+}
+
+bool RateKeeper::keepTime() {
+  bool lagged = monitorTime();
+  if (remaining_ > 0) {
+    util::sleep_for(remaining_ * 1000);
+  }
+  return lagged;
+}
+
+bool RateKeeper::monitorTime() {
+  ++frame_;
+  last_monitor_time = seconds_since_boot();
+  remaining_ = next_frame_time - last_monitor_time;
+
+  bool lagged = remaining_ < 0;
+  if (lagged) {
+    if (print_delay_threshold > 0 && remaining_ < -print_delay_threshold) {
+      LOGW("%s lagging by %.2f ms", name.c_str(), -remaining_ * 1000);
+    }
+    next_frame_time = last_monitor_time + interval;
+  } else {
+    next_frame_time += interval;
+  }
+  return lagged;
+}

common/ratekeeper.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <string>
+
+class RateKeeper {
+public:
+  RateKeeper(const std::string &name, float rate, float print_delay_threshold = 0);
+  ~RateKeeper() {}
+  bool keepTime();
+  bool monitorTime();
+  inline double frame() const { return frame_; }
+  inline double remaining() const { return remaining_; }
+
+private:
+  double interval;
+  double next_frame_time;
+  double last_monitor_time;
+  double remaining_ = 0;
+  float print_delay_threshold = 0;
+  uint64_t frame_ = 0;
+  std::string name;
+};

common/tests/.gitignore

@@ -1,2 +1,3 @@
+test_ratekeeper
 test_util
 test_swaglog

common/tests/test_ratekeeper.cc

@@ -0,0 +1,17 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"
+#include "common/ratekeeper.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+TEST_CASE("RateKeeper") {
+  float freq = GENERATE(10, 50, 100);
+  RateKeeper rk("Test RateKeeper", freq);
+  for (int i = 0; i < freq; ++i) {
+    double begin = seconds_since_boot();
+    util::sleep_for(util::random_int(0, 1000.0 / freq - 1));
+    bool lagged = rk.keepTime();
+    REQUIRE(std::abs(seconds_since_boot() - begin - (1 / freq)) < 1e-3);
+    REQUIRE(lagged == false);
+  }
+}