Adds rate timer. (#8)

cartographer/common/CMakeLists.txt

@@ -138,6 +138,15 @@ google_library(common_port
     port.h
 )
 
+google_library(common_rate_timer
+  HDRS
+    rate_timer.h
+  DEPENDS
+    common_math
+    common_port
+    common_time
+)
+
 google_library(common_thread_pool
   USES_CERES
   SRCS
@@ -196,3 +205,10 @@ google_test(common_ordered_multi_queue_test
     common_make_unique
     common_ordered_multi_queue
 )
+
+google_test(common_rate_timer_test
+  SRCS
+    rate_timer_test.cc
+  DEPENDS
+    common_rate_timer
+)

cartographer/common/rate_timer.h

@@ -0,0 +1,136 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * 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 CARTOGRAPHER_COMMON_RATE_TIMER_H_
+#define CARTOGRAPHER_COMMON_RATE_TIMER_H_
+
+#include <chrono>
+#include <deque>
+#include <iomanip>
+#include <numeric>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "cartographer/common/math.h"
+#include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+
+namespace cartographer {
+namespace common {
+
+// Computes the rate at which pulses come in.
+template <typename ClockType = std::chrono::steady_clock>
+class RateTimer {
+ public:
+  // Computes the rate at which pulses come in over 'window_duration' in wall
+  // time.
+  explicit RateTimer(const common::Duration window_duration)
+      : window_duration_(window_duration) {}
+  ~RateTimer() {}
+
+  RateTimer(const RateTimer&) = delete;
+  RateTimer& operator=(const RateTimer&) = delete;
+
+  // Returns the pulse rate in Hz.
+  double ComputeRate() const {
+    if (events_.empty()) {
+      return 0.;
+    }
+    return static_cast<double>(events_.size() - 1) /
+           common::ToSeconds((events_.back().time - events_.front().time));
+  }
+
+  // Returns the ratio of the pulse rate (with supplied times) to the wall time
+  // rate. For example, if a sensor produces pulses at 10 Hz, but we call Pulse
+  // at 20 Hz wall time, this will return 2.
+  double ComputeWallTimeRateRatio() const {
+    if (events_.empty()) {
+      return 0.;
+    }
+    return common::ToSeconds((events_.back().time - events_.front().time)) /
+           std::chrono::duration_cast<std::chrono::duration<double>>(
+               events_.back().wall_time - events_.front().wall_time)
+               .count();
+  }
+
+  // Records an event that will contribute to the computed rate.
+  void Pulse(common::Time time) {
+    events_.push_back(Event{time, ClockType::now()});
+    while (events_.size() > 2 &&
+           (events_.back().wall_time - events_.front().wall_time) >
+               window_duration_) {
+      events_.pop_front();
+    }
+  }
+
+  // Returns a debug string representation.
+  string DebugString() const {
+    if (events_.size() < 2) {
+      return "unknown";
+    }
+    std::ostringstream out;
+    out << std::fixed << std::setprecision(2) << ComputeRate() << " Hz "
+        << DeltasDebugString() << " (pulsed at "
+        << ComputeWallTimeRateRatio() * 100. << "% real time)";
+    return out.str();
+  }
+
+ private:
+  struct Event {
+    common::Time time;
+    typename ClockType::time_point wall_time;
+  };
+
+  // Computes all differences in seconds between consecutive pulses.
+  std::vector<double> ComputeDeltasInSeconds() const {
+    CHECK_GT(events_.size(), 1);
+    const size_t count = events_.size() - 1;
+    std::vector<double> result;
+    result.reserve(count);
+    for (size_t i = 0; i != count; ++i) {
+      result.push_back(
+          common::ToSeconds(events_[i + 1].time - events_[i].time));
+    }
+    return result;
+  }
+
+  // Returns the average and standard deviation of the deltas.
+  string DeltasDebugString() const {
+    const auto deltas = ComputeDeltasInSeconds();
+    const double sum = std::accumulate(deltas.begin(), deltas.end(), 0.);
+    const double mean = sum / deltas.size();
+
+    double squared_sum = 0.;
+    for (const double x : deltas) {
+      squared_sum += common::Pow2(x - mean);
+    }
+    const double sigma = std::sqrt(squared_sum / (deltas.size() - 1));
+
+    std::ostringstream out;
+    out << std::scientific << std::setprecision(2) << mean << " s +/- " << sigma
+        << " s";
+    return out.str();
+  }
+
+  std::deque<Event> events_;
+  const common::Duration window_duration_;
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_RATE_TIMER_H_

cartographer/common/rate_timer_test.cc

@@ -0,0 +1,63 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * 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.
+ */
+
+#include "cartographer/common/rate_timer.h"
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace common {
+namespace {
+
+TEST(RateTimerTest, ComputeRate) {
+  RateTimer<> rate_timer(common::FromSeconds(1.));
+  common::Time time = common::FromUniversal(42);
+  for (int i = 0; i < 100; ++i) {
+    rate_timer.Pulse(time);
+    time += common::FromSeconds(0.1);
+  }
+  EXPECT_NEAR(10., rate_timer.ComputeRate(), 1e-3);
+}
+
+struct SimulatedClock {
+  using rep = std::chrono::steady_clock::rep;
+  using period = std::chrono::steady_clock::period;
+  using duration = std::chrono::steady_clock::duration;
+  using time_point = std::chrono::steady_clock::time_point;
+  static constexpr bool is_steady = true;
+
+  static time_point time;
+  static time_point now() noexcept { return time; }
+};
+
+SimulatedClock::time_point SimulatedClock::time;
+
+TEST(RateTimerTest, ComputeWallTimeRateRatio) {
+  common::Time time = common::FromUniversal(42);
+  RateTimer<SimulatedClock> rate_timer(common::FromSeconds(1.));
+  for (int i = 0; i < 100; ++i) {
+    rate_timer.Pulse(time);
+    time += common::FromSeconds(0.1);
+    SimulatedClock::time +=
+        std::chrono::duration_cast<SimulatedClock::duration>(
+            std::chrono::duration<double>(0.05));
+  }
+  EXPECT_NEAR(2., rate_timer.ComputeWallTimeRateRatio(), 1e-3);
+}
+
+}  // namespace
+}  // namespace common
+}  // namespace cartographer

cartographer/common/time.cc

@@ -22,10 +22,14 @@ namespace cartographer {
 namespace common {
 
 Duration FromSeconds(const double seconds) {
-  return Duration(static_cast<int64>(1e7 * seconds));
+  return std::chrono::duration_cast<Duration>(
+      std::chrono::duration<double>(seconds));
 }
 
-double ToSeconds(const Duration duration) { return duration.count() * 1e-7; }
+double ToSeconds(const Duration duration) {
+  return std::chrono::duration_cast<std::chrono::duration<double>>(duration)
+      .count();
+}
 
 Time FromUniversal(const int64 ticks) { return Time(Duration(ticks)); }
 
@@ -36,8 +40,9 @@ std::ostream& operator<<(std::ostream& os, const Time time) {
   return os;
 }
 
-common::Duration FromMilliseconds(int64 milliseconds) {
+common::Duration FromMilliseconds(const int64 milliseconds) {
-  return common::Duration(milliseconds * 10000);
+  return std::chrono::duration_cast<Duration>(
+      std::chrono::milliseconds(milliseconds));
 }
 
 }  // namespace common