diff --git a/cartographer/common/CMakeLists.txt b/cartographer/common/CMakeLists.txt index b0aea2c..aaf9bd4 100644 --- a/cartographer/common/CMakeLists.txt +++ b/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 +) diff --git a/cartographer/common/rate_timer.h b/cartographer/common/rate_timer.h new file mode 100644 index 0000000..b578dc3 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#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 +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(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>( + 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 ComputeDeltasInSeconds() const { + CHECK_GT(events_.size(), 1); + const size_t count = events_.size() - 1; + std::vector 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 events_; + const common::Duration window_duration_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_RATE_TIMER_H_ diff --git a/cartographer/common/rate_timer_test.cc b/cartographer/common/rate_timer_test.cc new file mode 100644 index 0000000..4acde59 --- /dev/null +++ b/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 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( + std::chrono::duration(0.05)); + } + EXPECT_NEAR(2., rate_timer.ComputeWallTimeRateRatio(), 1e-3); +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/cartographer/common/time.cc b/cartographer/common/time.cc index 760ebc0..6874de2 100644 --- a/cartographer/common/time.cc +++ b/cartographer/common/time.cc @@ -22,10 +22,14 @@ namespace cartographer { namespace common { Duration FromSeconds(const double seconds) { - return Duration(static_cast(1e7 * seconds)); + return std::chrono::duration_cast( + std::chrono::duration(seconds)); } -double ToSeconds(const Duration duration) { return duration.count() * 1e-7; } +double ToSeconds(const Duration duration) { + return std::chrono::duration_cast>(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) { - return common::Duration(milliseconds * 10000); +common::Duration FromMilliseconds(const int64 milliseconds) { + return std::chrono::duration_cast( + std::chrono::milliseconds(milliseconds)); } } // namespace common