Support inserting times out of order
When we start wanting to use the timestamp on timestamps for the filter,
we will end up with timestamp observations which are out of order. Find
the right spot in the deque, insert, and then remove points until valid.
Add a *ton* of tests for this. And while we are here, test that
freezing works too.
Change-Id: If421f0e54628fbd2bd39f25ed8dd58214fc1dac4
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index 453180e..524ef19 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -160,14 +160,14 @@
const monotonic_clock::time_point e = monotonic_clock::epoch();
const monotonic_clock::time_point ta = e + chrono::milliseconds(500);
- NoncausalTimestampFilter a;
+ NoncausalTimestampFilter a(nullptr);
// Delivered at e, sent at e + offset.
// Sent at 1.001, received at 0
a.Sample(e, chrono::milliseconds(1001));
a.Sample(e + chrono::milliseconds(1000), chrono::milliseconds(1001));
a.Sample(e + chrono::milliseconds(3000), chrono::milliseconds(999));
- NoncausalTimestampFilter b;
+ NoncausalTimestampFilter b(nullptr);
// Sent at 0.001s, received at 1.000s
b.Sample(e + chrono::milliseconds(1000), -chrono::milliseconds(999));
b.Sample(e + chrono::milliseconds(2000), -chrono::milliseconds(1000));
@@ -207,11 +207,11 @@
// Now do the second line segment.
{
- NoncausalTimestampFilter a;
+ NoncausalTimestampFilter a(nullptr);
a.Sample(e + chrono::milliseconds(1000), chrono::milliseconds(1001));
a.Sample(e + chrono::milliseconds(3000), chrono::milliseconds(999));
- NoncausalTimestampFilter b;
+ NoncausalTimestampFilter b(nullptr);
b.Sample(e + chrono::milliseconds(2000), -chrono::milliseconds(1000));
b.Sample(e + chrono::milliseconds(4000), -chrono::milliseconds(1002));
{
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index a6325f1..32f796c 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -13,9 +13,26 @@
namespace aos {
namespace message_bridge {
namespace {
-
namespace chrono = std::chrono;
+std::string TimeString(const aos::monotonic_clock::time_point t,
+ std::chrono::nanoseconds o) {
+ std::stringstream ss;
+ ss << "O(" << t << ") = " << o.count() << ", remote " << t + o;
+ return ss.str();
+}
+std::string TimeString(const std::tuple<aos::monotonic_clock::time_point,
+ std::chrono::nanoseconds, bool>
+ t) {
+ return TimeString(std::get<0>(t), std::get<1>(t));
+}
+
+std::string TimeString(
+ const std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>
+ t) {
+ return TimeString(std::get<0>(t), std::get<1>(t));
+}
+
void ClippedAverageFilterPrintHeader(FILE *fp) {
fprintf(fp,
"# time_since_start, sample_ns, filtered_offset, offset, "
@@ -466,19 +483,6 @@
return std::make_tuple(std::get<0>(t), std::get<1>(t));
}
-std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
-NoncausalTimestampFilter::Timestamps() const {
- std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- result;
-
- for (const auto x : timestamps_) {
- result.emplace_back(TrimTuple(x));
- }
- return result;
-}
-
void NoncausalTimestampFilter::FlushSavedSamples() {
for (const std::tuple<aos::monotonic_clock::time_point,
std::chrono::nanoseconds> &sample : saved_samples_) {
@@ -868,6 +872,7 @@
// The first sample is easy. Just do it!
if (timestamps_.size() == 0) {
+ VLOG(1) << "Initial sample of " << TimeString(monotonic_now, sample_ns);
timestamps_.emplace_back(std::make_tuple(monotonic_now, sample_ns, false));
CHECK(!fully_frozen_)
<< ": Returned a horizontal line previously and then got a new "
@@ -877,91 +882,234 @@
.count()
<< " seconds after the last sample at " << std::get<0>(timestamps_[0])
<< " " << csv_file_name_ << ".";
- } else {
- // Future samples get quite a bit harder. We want the line to track the
- // highest point without volating the slope constraint.
- std::tuple<aos::monotonic_clock::time_point, chrono::nanoseconds, bool>
- back = timestamps_.back();
+ return;
+ }
- aos::monotonic_clock::duration dt = monotonic_now - std::get<0>(back);
- aos::monotonic_clock::duration doffset = sample_ns - std::get<1>(back);
+ // Future samples get quite a bit harder. We want the line to track the
+ // highest point without volating the slope constraint.
+ std::tuple<aos::monotonic_clock::time_point, chrono::nanoseconds, bool> back =
+ timestamps_.back();
- if (dt == chrono::nanoseconds(0) && doffset == chrono::nanoseconds(0)) {
+ aos::monotonic_clock::duration dt = monotonic_now - std::get<0>(back);
+ aos::monotonic_clock::duration doffset = sample_ns - std::get<1>(back);
+
+ if (dt == chrono::nanoseconds(0) && doffset == chrono::nanoseconds(0)) {
+ VLOG(1) << "Duplicate sample of O(" << monotonic_now
+ << ") = " << sample_ns.count() << ", remote time "
+ << monotonic_now + sample_ns;
+
+ return;
+ }
+
+ // Lets handle the easy case first. We are just appending.
+ if (dt > chrono::nanoseconds(0)) {
+ // We are trying to draw a line through the most positive points which
+ // adheres to our +- velocity constraint. If the point is less than the max
+ // negative slope, the point violates our constraint and will never be worth
+ // considering. Ignore it.
+ if (doffset < -dt * kMaxVelocity()) {
+ VLOG(1) << std::setprecision(1) << std::fixed << "Rejected sample of "
+ << TimeString(monotonic_now, sample_ns) << " because "
+ << doffset.count() << " < " << (-dt * kMaxVelocity()).count()
+ << " len " << timestamps_.size();
return;
}
- // If the point is higher than the max negative slope, the slope will either
- // adhere to our constraint, or will be too positive. If it is too
- // positive, we need to back propagate and remove offending points which
- // were too low rather than reject this new point. We never want a point to
- // be higher than the line.
- if (-dt * kMaxVelocity() <= doffset) {
- // TODO(austin): If the slope is the same, and the (to be newly in the
- // middle) point is not frozen, drop the point out of the middle. This
- // won't happen in the real world, but happens a lot with tests.
+ // Be overly conservative here. It either won't make a difference, or
+ // will give us an error with an actual useful time difference.
+ CHECK(!fully_frozen_)
+ << ": Returned a horizontal line previously and then got a new "
+ "sample at "
+ << monotonic_now << ", "
+ << chrono::duration<double>(monotonic_now - std::get<0>(timestamps_[0]))
+ .count()
+ << " seconds after the last sample at " << std::get<0>(timestamps_[0])
+ << " " << csv_file_name_ << ".";
- // Be overly conservative here. It either won't make a difference, or
- // will give us an error with an actual useful time difference.
- CHECK(!fully_frozen_)
- << ": Returned a horizontal line previously and then got a new "
- "sample at "
- << monotonic_now << ", "
- << chrono::duration<double>(monotonic_now -
- std::get<0>(timestamps_[0]))
- .count()
- << " seconds after the last sample at " << std::get<0>(timestamps_[0])
- << " " << csv_file_name_ << ".";
+ // Back propagate the max velocity and remove any elements violating the
+ // velocity constraint. This is to handle the case where the offsets were
+ // becoming gradually more negative, and then there's a sudden positive
+ // jump.
+ //
+ // 1 4
+ // 2
+ // 3
+ //
+ // In this case, point 3 is now violating our constraint and we need to
+ // remove it. This is the non-causal part of the filter.
+ while (dt * kMaxVelocity() < doffset && timestamps_.size() > 1u) {
+ CHECK(!std::get<2>(back)) << ": Can't pop an already frozen sample.";
+ VLOG(1) << "Removing now invalid sample during back propegation of "
+ << TimeString(back);
+ timestamps_.pop_back();
- // Back propagate the max velocity and remove any elements violating the
- // velocity constraint.
- while (dt * kMaxVelocity() < doffset && timestamps_.size() > 1u) {
- CHECK(!std::get<2>(back)) << ": Can't pop an already frozen sample.";
- timestamps_.pop_back();
+ back = timestamps_.back();
+ dt = monotonic_now - std::get<0>(back);
+ doffset = sample_ns - std::get<1>(back);
+ }
- back = timestamps_.back();
- dt = monotonic_now - std::get<0>(back);
- doffset = sample_ns - std::get<1>(back);
+ timestamps_.emplace_back(std::make_tuple(monotonic_now, sample_ns, false));
+ return;
+ }
+
+ // Since it didn't fit at the end, now figure out where to insert our new
+ // point. lower_bound returns the element which we are supposed to insert
+ // "before".
+ auto it = std::lower_bound(
+ timestamps_.begin(), timestamps_.end(), monotonic_now,
+ [](const std::tuple<aos::monotonic_clock::time_point,
+ std::chrono::nanoseconds, bool>
+ x,
+ monotonic_clock::time_point t) { return std::get<0>(x) < t; });
+
+ CHECK (it != timestamps_.end());
+
+ CHECK(!std::get<2>(*(it)))
+ << ": Tried to insert " << monotonic_now << " before " << std::get<0>(*it)
+ << ", which is a frozen time.";
+
+ if (it == timestamps_.begin()) {
+ // We are being asked to add at the beginning.
+ {
+ const chrono::nanoseconds dt = std::get<0>(*it) - monotonic_now;
+ const chrono::nanoseconds original_offset = std::get<1>(*it);
+ const chrono::nanoseconds doffset = original_offset - sample_ns;
+
+ if (dt == chrono::nanoseconds(0) && doffset >= chrono::nanoseconds(0)) {
+ VLOG(1) << "Redundant timestamp "
+ << TimeString(monotonic_now, sample_ns) << " because "
+ << TimeString(timestamps_.front())
+ << " is at the same time and a better solution.";
+ return;
+ }
+ }
+
+ VLOG(1) << "Added sample at beginning "
+ << TimeString(monotonic_now, sample_ns);
+ timestamps_.insert(it, std::make_tuple(monotonic_now, sample_ns, false));
+
+ while (true) {
+ // First point was too positive, so we need to remove points after it
+ // until we are valid.
+ auto second = timestamps_.begin() + 1;
+ if (second != timestamps_.end()) {
+ const chrono::nanoseconds dt = std::get<0>(*second) - monotonic_now;
+ const chrono::nanoseconds doffset = std::get<1>(*second) - sample_ns;
+
+ if (doffset < -dt * kMaxVelocity()) {
+ VLOG(1) << "Removing redundant sample of " << TimeString(*second)
+ << " because " << TimeString(timestamps_.front())
+ << " would make the slope too negative.";
+ timestamps_.erase(second);
+ continue;
+ }
+
+ auto third = second + 1;
+ if (third != timestamps_.end()) {
+ // The second point might need to be popped. This shows up when
+ // the first point violated the constraints, but in timestamp(), we
+ // were clipping it to be valid. When a point is added before it, the
+ // prior first point might now be invalid and need to be cleaned up.
+ //
+ // 3
+ //
+ // 1 2
+ //
+ // Point 2 was invalid before, but was clipped in timestamp(), but can
+ // now be removed.
+ const chrono::nanoseconds dt =
+ std::get<0>(*third) - std::get<0>(*second);
+ const chrono::nanoseconds doffset =
+ std::get<1>(*third) - std::get<1>(*second);
+
+ if (doffset > dt * kMaxVelocity()) {
+ VLOG(1) << "Removing invalid sample of " << TimeString(*second)
+ << " because " << TimeString(*third)
+ << " would make the slope too positive.";
+ timestamps_.erase(second);
+ continue;
+ }
+ }
}
- timestamps_.emplace_back(
- std::make_tuple(monotonic_now, sample_ns, false));
+ break;
+ }
+ return;
+ } else {
+ VLOG(1) << "Found the next time " << std::get<0>(*(it - 1)) << " < "
+ << monotonic_now << " < " << std::get<0>(*it);
- // If we are early in the log file, the filter hasn't had time to get
- // started. We might only have 2 samples, and the first sample was
- // incredibly delayed, violating our velocity constraint. In that case,
- // modify the first sample (rather than remove it) to retain the knowledge
- // of the velocity, but adhere to the constraints.
- if (dt * kMaxVelocity() < doffset) {
- CHECK_EQ(timestamps_.size(), 2u);
- const aos::monotonic_clock::duration adjusted_initial_time =
- sample_ns - aos::monotonic_clock::duration(
- static_cast<aos::monotonic_clock::duration::rep>(
- dt.count() * kMaxVelocity()));
+ {
+ chrono::nanoseconds prior_dt = monotonic_now - std::get<0>(*(it - 1));
+ chrono::nanoseconds prior_doffset = sample_ns - std::get<1>(*(it - 1));
+ chrono::nanoseconds next_dt = std::get<0>(*it) - monotonic_now;
+ chrono::nanoseconds next_doffset = std::get<1>(*it) - sample_ns;
- VLOG(1) << csv_file_name_ << " a [(" << std::get<0>(timestamps_[0])
- << " -> " << std::get<1>(timestamps_[0]).count() << "ns), ("
- << std::get<0>(timestamps_[1]) << " -> "
- << std::get<1>(timestamps_[1]).count()
- << "ns) => {dt: " << std::fixed << std::setprecision(6)
- << chrono::duration<double, std::milli>(
- std::get<0>(timestamps_[1]) -
- std::get<0>(timestamps_[0]))
- .count()
- << "ms, do: " << std::fixed << std::setprecision(6)
- << chrono::duration<double, std::milli>(
- std::get<1>(timestamps_[1]) -
- std::get<1>(timestamps_[0]))
- .count()
- << "ms}]";
- VLOG(1) << "Back is out of range, clipping from "
- << std::get<1>(timestamps_[0]).count() << " to "
- << adjusted_initial_time.count();
-
- std::get<1>(timestamps_[0]) = adjusted_initial_time;
+ // If we are worse than either the previous or next point, discard.
+ if (prior_doffset < -prior_dt * kMaxVelocity()) {
+ VLOG(1) << "Ignoring timestamp " << TimeString(monotonic_now, sample_ns)
+ << " because " << TimeString(*(it - 1))
+ << " is before and the slope would be too negative.";
+ return;
}
- } else {
- VLOG(1) << "Rejecting sample because " << doffset.count() << " > "
- << (-dt * kMaxVelocity()).count();
+ if (next_doffset > next_dt * kMaxVelocity()) {
+ VLOG(1) << "Ignoring timestamp " << TimeString(monotonic_now, sample_ns)
+ << " because " << TimeString(*it)
+ << " is following and the slope would be too positive.";
+ return;
+ }
+ }
+
+ // Now, insert and start propagating forwards and backwards anything we've
+ // made invalid. Do this simultaneously so we keep discovering anything
+ // new.
+ auto middle_it = timestamps_.insert(
+ it, std::make_tuple(monotonic_now, sample_ns, false));
+ VLOG(1) << "Inserted " << TimeString(*middle_it);
+
+ while (middle_it != timestamps_.end() && middle_it != timestamps_.begin()) {
+ auto next_it =
+ (middle_it == timestamps_.end()) ? timestamps_.end() : middle_it + 1;
+ auto prior_it = (middle_it == timestamps_.begin()) ? timestamps_.begin()
+ : middle_it - 1;
+
+ // See if the next can be popped. If so, pop it.
+ if (next_it != timestamps_.end()) {
+ const chrono::nanoseconds next_dt =
+ std::get<0>(*next_it) - std::get<0>(*middle_it);
+ const chrono::nanoseconds next_doffset =
+ std::get<1>(*next_it) - std::get<1>(*middle_it);
+
+ if (next_doffset < -next_dt * kMaxVelocity()) {
+ VLOG(1) << "Next slope is too negative, removing next point "
+ << TimeString(*next_it);
+ next_it = timestamps_.erase(next_it);
+ // erase invalidates all iterators, and this code uses middle as the
+ // state. Update middle.
+ middle_it = next_it - 1;
+ continue;
+ }
+ }
+
+ // See if the previous point can be popped.
+ if (prior_it != timestamps_.begin()) {
+ const chrono::nanoseconds prior_dt =
+ std::get<0>(*middle_it) - std::get<0>(*prior_it);
+ const chrono::nanoseconds prior_doffset =
+ std::get<1>(*middle_it) - std::get<1>(*prior_it);
+
+ if (prior_doffset > prior_dt * kMaxVelocity()) {
+ CHECK(!std::get<2>(*prior_it))
+ << ": Can't pop an already frozen sample.";
+ VLOG(1) << "Prior slope is too positive, removing prior point "
+ << TimeString(*prior_it);
+ prior_it = timestamps_.erase(prior_it);
+ middle_it = prior_it;
+ continue;
+ }
+ }
+ // Made no modifications, bail.
+ break;
}
}
}
@@ -982,7 +1130,7 @@
if (timestamps_.empty() || next_to_consume_ >= timestamps_.size()) {
return std::nullopt;
}
- return TrimTuple(timestamps_[next_to_consume_]);
+ return timestamp(next_to_consume_);
}
std::optional<std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
@@ -991,7 +1139,7 @@
return std::nullopt;
}
- auto result = TrimTuple(timestamps_[next_to_consume_]);
+ auto result = timestamp(next_to_consume_);
++next_to_consume_;
return result;
}
@@ -999,15 +1147,18 @@
void NoncausalTimestampFilter::FreezeUntil(
aos::monotonic_clock::time_point node_monotonic_now) {
for (size_t i = 0; i < timestamps_.size(); ++i) {
- if (std::get<0>(timestamps_[i]) > node_monotonic_now) {
+ // Freeze 1 point past the match.
+ std::get<2>(timestamps_[i]) = true;
+ if (std::get<0>(timestamps_[i]) >= node_monotonic_now) {
return;
}
- std::get<2>(timestamps_[i]) = true;
}
- if (timestamps_.size() < 2u) {
- // This will evaluate to a line. We can't support adding points to a line
- // yet.
+ if (timestamps_.empty()) {
+ fully_frozen_ = true;
+ } else if (node_monotonic_now > std::get<0>(timestamps_.back())) {
+ // We've been asked to freeze past the last point. It isn't safe to add any
+ // more points or we will change this region.
fully_frozen_ = true;
}
}
@@ -1015,31 +1166,21 @@
void NoncausalTimestampFilter::FreezeUntilRemote(
aos::monotonic_clock::time_point remote_monotonic_now) {
for (size_t i = 0; i < timestamps_.size(); ++i) {
- if (std::get<0>(timestamps_[i]) + std::get<1>(timestamps_[i]) >
+ // Freeze 1 point past the match.
+ std::get<2>(timestamps_[i]) = true;
+ if (std::get<0>(timestamp(i)) + std::get<1>(timestamp(i)) >=
remote_monotonic_now) {
return;
}
- std::get<2>(timestamps_[i]) = true;
}
- if (timestamps_.size() < 2u) {
- // This will evaluate to a line. We can't support adding points to a line
- // yet.
+ if (timestamps_.empty()) {
fully_frozen_ = true;
- }
-}
-
-void NoncausalTimestampFilter::Freeze() {
- if (timestamps_.size() >= 1u) {
- std::get<2>(timestamps_[0]) = true;
- }
-
- if (timestamps_.size() < 2u) {
- // This will evaluate to a line. We can't support adding points to a line
- // yet.
+ } else if (remote_monotonic_now > std::get<0>(timestamps_.back()) +
+ std::get<1>(timestamps_.back())) {
+ // We've been asked to freeze past the last point. It isn't safe to add any
+ // more points or we will change this region.
fully_frozen_ = true;
- } else {
- std::get<2>(timestamps_[1]) = true;
}
}
@@ -1066,8 +1207,19 @@
PrintNoncausalTimestampFilterSamplesHeader(samples_fp_);
}
+void NoncausalTimestampFilter::PopFront() {
+ VLOG(1) << node_->name()->string_view() << " Popped sample of "
+ << TimeString(timestamp(0));
+ MaybeWriteTimestamp(timestamp(0));
+ timestamps_.pop_front();
+ has_popped_ = true;
+ if (next_to_consume_ > 0u) {
+ next_to_consume_--;
+ }
+}
+
void NoncausalTimestampFilter::MaybeWriteTimestamp(
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds, bool>
+ std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>
timestamp) {
if (fp_ && first_time_ != aos::monotonic_clock::min_time) {
fprintf(fp_, "%.9f, %.9f, %.9f\n",
@@ -1086,7 +1238,7 @@
void NoncausalOffsetEstimator::Sample(
const Node *node, aos::monotonic_clock::time_point node_delivered_time,
aos::monotonic_clock::time_point other_node_sent_time) {
- VLOG(1) << "Sample delivered " << node_delivered_time << " sent "
+ VLOG(1) << "Sample delivered " << node_delivered_time << " sent "
<< other_node_sent_time << " to " << node->name()->string_view();
if (node == node_a_) {
a_.Sample(node_delivered_time, other_node_sent_time - node_delivered_time);
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index ef86a75..7227fc7 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -235,6 +235,7 @@
// precision and handling the remainder with double precision.
class NoncausalTimestampFilter {
public:
+ NoncausalTimestampFilter(const Node *node) : node_(node) {}
~NoncausalTimestampFilter();
// Returns the offset for the point in time, using the timestamps in the deque
@@ -286,17 +287,42 @@
// Returns true if any points were popped.
bool Pop(aos::monotonic_clock::time_point time);
- // Returns the current list of timestamps in our list.
- std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- Timestamps() const;
-
std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>
timestamp(size_t i) const {
+ if (i == 0u && timestamps_.size() >= 2u && !has_popped_) {
+ std::chrono::nanoseconds dt =
+ std::get<0>(timestamps_[1]) - std::get<0>(timestamps_[0]);
+ std::chrono::nanoseconds doffset =
+ std::get<1>(timestamps_[1]) - std::get<1>(timestamps_[0]);
+
+ // If we are early in the log file, the filter hasn't had time to get
+ // started. We might only have 2 samples, and the first sample was
+ // incredibly delayed, violating our velocity constraint. In that case,
+ // modify the first sample (rather than remove it) to retain the knowledge
+ // of the velocity, but adhere to the constraints.
+ //
+ // We are doing this here so as points get added in any order, we don't
+ // confuse ourselves about what really happened.
+ if (doffset > dt * kMaxVelocity()) {
+ const aos::monotonic_clock::duration adjusted_initial_time =
+ std::get<1>(timestamps_[1]) -
+ aos::monotonic_clock::duration(
+ static_cast<aos::monotonic_clock::duration::rep>(
+ dt.count() * kMaxVelocity()));
+
+ return std::make_tuple(std::get<0>(timestamps_[0]),
+ adjusted_initial_time);
+ }
+ }
return std::make_tuple(std::get<0>(timestamps_[i]),
std::get<1>(timestamps_[i]));
}
+ // Returns if the timestamp is frozen or not.
+ bool frozen(size_t index) const {
+ return fully_frozen_ || std::get<2>(timestamps_[index]);
+ }
+
size_t timestamps_size() const { return timestamps_.size(); }
void Debug() {
@@ -317,11 +343,6 @@
void SetFirstTime(aos::monotonic_clock::time_point time);
void SetCsvFileName(std::string_view name);
- // Marks the first line segment (the two points used to compute both the
- // offset and slope), as used. Those points can't be removed from the filter
- // going forwards.
- void Freeze();
-
// Marks all line segments up until the provided time on the provided node as
// used.
void FreezeUntil(aos::monotonic_clock::time_point node_monotonic_now);
@@ -377,22 +398,18 @@
private:
// Removes the oldest timestamp.
- void PopFront() {
- MaybeWriteTimestamp(timestamps_.front());
- timestamps_.pop_front();
- if (next_to_consume_ > 0u) {
- next_to_consume_--;
- }
- }
+ void PopFront();
// Writes a timestamp to the file if it is reasonable.
- void MaybeWriteTimestamp(std::tuple<aos::monotonic_clock::time_point,
- std::chrono::nanoseconds, bool>
- timestamp);
+ void MaybeWriteTimestamp(
+ std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>
+ timestamp);
// Writes any saved timestamps to file.
void FlushSavedSamples();
+ const Node *node_;
+
// Timestamp, offest, and then a boolean representing if this sample is frozen
// and can't be modified or not.
// TODO(austin): Actually use and update the bool.
@@ -416,6 +433,8 @@
bool fully_frozen_ = false;
+ bool has_popped_ = false;
+
aos::monotonic_clock::time_point first_time_ = aos::monotonic_clock::min_time;
};
@@ -424,7 +443,7 @@
class NoncausalOffsetEstimator {
public:
NoncausalOffsetEstimator(const Node *node_a, const Node *node_b)
- : node_a_(node_a), node_b_(node_b) {}
+ : a_(node_a), b_(node_b), node_a_(node_a), node_b_(node_b) {}
NoncausalTimestampFilter *GetFilter(const Node *n) {
if (n == node_a_) {
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index a046726..4379dcb 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -76,9 +76,19 @@
EXPECT_EQ(filter.offset(), chrono::microseconds(100500));
}
+class FilterTest : public ::testing::Test {
+ public:
+ FlatbufferDetachedBuffer<Node> node_buffer =
+ JsonToFlatbuffer<Node>("{\"name\": \"test\"}");
+ const Node *node = &node_buffer.message();
+};
+
+using NoncausalTimestampFilterTest = FilterTest;
+using NoncausalTimestampFilterDeathTest = FilterTest;
+
// Tests that 2 samples results in the correct line between them, and the
// correct intermediate as it is being built.
-TEST(NoncausalTimestampFilterTest, PeekPop) {
+TEST_F(NoncausalTimestampFilterTest, PeekPop) {
const monotonic_clock::time_point ta(chrono::nanoseconds(100000));
const chrono::nanoseconds oa(chrono::nanoseconds(1000));
const monotonic_clock::time_point tb(chrono::nanoseconds(200000));
@@ -88,7 +98,7 @@
// Simple case, everything is done in order, nothing is dropped.
{
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, oa);
filter.Sample(tb, ob);
@@ -105,7 +115,7 @@
// Now try again while dropping ta after popping it.
{
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, oa);
filter.Sample(tb, ob);
@@ -125,7 +135,7 @@
// Now try again while dropping ta before popping it.
{
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, oa);
filter.Sample(tb, ob);
@@ -142,20 +152,20 @@
}
// Tests that invalid samples get clipped as expected.
-TEST(NoncausalTimestampFilterTest, ClippedSample) {
+TEST_F(NoncausalTimestampFilterTest, ClippedSample) {
const monotonic_clock::time_point ta(chrono::milliseconds(0));
const monotonic_clock::time_point tb(chrono::milliseconds(1));
const monotonic_clock::time_point tc(chrono::milliseconds(2));
{
// A positive slope of 1 ms/second is properly applied.
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, chrono::microseconds(1));
filter.Debug();
filter.Sample(tb, chrono::microseconds(2));
filter.Debug();
- ASSERT_EQ(filter.Timestamps().size(), 2u);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
EXPECT_EQ(filter.timestamp(0),
std::make_tuple(ta, chrono::microseconds(1)));
@@ -165,13 +175,13 @@
{
// A negative slope of 1 ms/second is properly applied.
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, chrono::microseconds(1));
filter.Debug();
filter.Sample(tb, chrono::microseconds(0));
filter.Debug();
- ASSERT_EQ(filter.Timestamps().size(), 2u);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
EXPECT_EQ(filter.timestamp(0),
std::make_tuple(ta, chrono::microseconds(1)));
@@ -181,59 +191,59 @@
{
// Too much negative is ignored.
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, chrono::microseconds(1));
filter.Debug();
filter.Sample(tb, -chrono::microseconds(1));
filter.Debug();
- ASSERT_EQ(filter.Timestamps().size(), 1u);
+ ASSERT_EQ(filter.timestamps_size(), 1u);
}
{
// Too much positive pulls up the first point.
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, chrono::microseconds(1));
filter.Debug();
filter.Sample(tb, chrono::microseconds(3));
filter.Debug();
- ASSERT_EQ(filter.Timestamps().size(), 2u);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
- EXPECT_EQ(std::get<1>(filter.Timestamps()[0]), chrono::microseconds(2));
- EXPECT_EQ(std::get<1>(filter.Timestamps()[1]), chrono::microseconds(3));
+ EXPECT_EQ(std::get<1>(filter.timestamp(0)), chrono::microseconds(2));
+ EXPECT_EQ(std::get<1>(filter.timestamp(1)), chrono::microseconds(3));
}
{
// Too much positive slope removes points.
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, chrono::microseconds(1));
filter.Debug();
filter.Sample(tb, chrono::microseconds(1));
filter.Debug();
- ASSERT_EQ(filter.Timestamps().size(), 2u);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
// Now add a sample with a slope of 0.002. This should back propagate and
// remove the middle point since it violates our constraints.
filter.Sample(tc, chrono::microseconds(3));
filter.Debug();
- ASSERT_EQ(filter.Timestamps().size(), 2u);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
- EXPECT_EQ(std::get<1>(filter.Timestamps()[0]), chrono::microseconds(1));
- EXPECT_EQ(std::get<1>(filter.Timestamps()[1]), chrono::microseconds(3));
+ EXPECT_EQ(std::get<1>(filter.timestamp(0)), chrono::microseconds(1));
+ EXPECT_EQ(std::get<1>(filter.timestamp(1)), chrono::microseconds(3));
}
}
// Tests that removing points from the filter works as expected.
-TEST(NoncausalTimestampFilterTest, PointRemoval) {
+TEST_F(NoncausalTimestampFilterTest, PointRemoval) {
const monotonic_clock::time_point t_before(-chrono::milliseconds(1));
const monotonic_clock::time_point ta(chrono::milliseconds(0));
const monotonic_clock::time_point tb(chrono::milliseconds(1));
const monotonic_clock::time_point tc(chrono::milliseconds(2));
// A positive slope of 1 ms/second is properly applied.
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, chrono::microseconds(1));
filter.Debug();
@@ -241,45 +251,433 @@
filter.Debug();
filter.Sample(tc, chrono::microseconds(1));
filter.Debug();
- ASSERT_EQ(filter.Timestamps().size(), 3u);
+ ASSERT_EQ(filter.timestamps_size(), 3u);
// Before or in the middle of the first line segment shouldn't change the
// number of points.
EXPECT_FALSE(filter.Pop(t_before));
- ASSERT_EQ(filter.Timestamps().size(), 3u);
+ ASSERT_EQ(filter.timestamps_size(), 3u);
EXPECT_FALSE(filter.Pop(ta));
- ASSERT_EQ(filter.Timestamps().size(), 3u);
+ ASSERT_EQ(filter.timestamps_size(), 3u);
EXPECT_FALSE(filter.Pop(ta + chrono::microseconds(100)));
- ASSERT_EQ(filter.Timestamps().size(), 3u);
+ ASSERT_EQ(filter.timestamps_size(), 3u);
// The second point should trigger a pop, since the offset computed using the
// points won't change when it is used, and any times after (even 1-2 ns
// later) would be wrong.
EXPECT_TRUE(filter.Pop(tb));
- ASSERT_EQ(filter.Timestamps().size(), 2u);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
}
// Tests that inserting duplicate points causes the duplicates to get ignored.
-TEST(NoncausalTimestampFilterTest, DuplicatePoints) {
+TEST_F(NoncausalTimestampFilterTest, DuplicatePoints) {
const monotonic_clock::time_point ta(chrono::milliseconds(0));
const chrono::nanoseconds oa(chrono::microseconds(1));
const monotonic_clock::time_point tb(chrono::milliseconds(1));
const chrono::nanoseconds ob(chrono::microseconds(2));
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(ta, oa);
filter.Sample(tb, ob);
- EXPECT_EQ(filter.Timestamps().size(), 2u);
+ EXPECT_EQ(filter.timestamps_size(), 2u);
filter.Sample(tb, ob);
- EXPECT_EQ(filter.Timestamps().size(), 2u);
+ EXPECT_EQ(filter.timestamps_size(), 2u);
+}
+
+// Tests that inserting points in the middle of the time sequence works for the
+// simple case.
+TEST_F(NoncausalTimestampFilterTest, BackwardsInTimeSimple) {
+ // Start with the simple case. A valid point in the middle.
+ const monotonic_clock::time_point ta(chrono::milliseconds(0));
+ const chrono::nanoseconds oa(chrono::microseconds(1));
+
+ const monotonic_clock::time_point tb(chrono::milliseconds(1));
+ const chrono::nanoseconds ob(chrono::microseconds(0));
+
+ const monotonic_clock::time_point tc(chrono::milliseconds(2));
+ const chrono::nanoseconds oc(chrono::microseconds(1));
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tc, oc);
+ filter.Sample(tb, ob);
+ filter.Debug();
+ EXPECT_EQ(filter.timestamps_size(), 3u);
+
+ EXPECT_EQ(filter.timestamp(0), std::make_tuple(ta, oa));
+ EXPECT_EQ(filter.timestamp(1), std::make_tuple(tb, ob));
+ EXPECT_EQ(filter.timestamp(2), std::make_tuple(tc, oc));
+}
+
+// Tests that inserting a duplicate point at the beginning gets ignored if it is
+// more negative than the original beginning point.
+TEST_F(NoncausalTimestampFilterTest, BackwardsInTimeDuplicateNegative) {
+ const monotonic_clock::time_point ta(chrono::milliseconds(0));
+ const chrono::nanoseconds oa(chrono::microseconds(1));
+
+ const monotonic_clock::time_point tb(chrono::milliseconds(1));
+ const chrono::nanoseconds ob(chrono::microseconds(1));
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tb, ob);
+ filter.Sample(ta, chrono::microseconds(0));
+ filter.Debug();
+ EXPECT_EQ(filter.timestamps_size(), 2u);
+
+ EXPECT_EQ(filter.timestamp(0), std::make_tuple(ta, chrono::microseconds(1)));
+ EXPECT_EQ(filter.timestamp(1), std::make_tuple(tb, ob));
+}
+
+// Tests that inserting a better duplicate point at the beginning gets taken if
+// it is more positive than the original beginning point.
+TEST_F(NoncausalTimestampFilterTest, BackwardsInTimeDuplicatePositive) {
+ const monotonic_clock::time_point ta(chrono::milliseconds(0));
+ const chrono::nanoseconds oa(chrono::microseconds(1));
+
+ const monotonic_clock::time_point tb(chrono::milliseconds(1));
+ const chrono::nanoseconds ob(chrono::microseconds(1));
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tb, ob);
+ filter.Sample(ta, chrono::microseconds(2));
+ filter.Debug();
+ EXPECT_EQ(filter.timestamps_size(), 2u);
+
+ EXPECT_EQ(filter.timestamp(0), std::make_tuple(ta, chrono::microseconds(2)));
+ EXPECT_EQ(filter.timestamp(1), std::make_tuple(tb, ob));
+}
+
+// Tests that inserting a negative duplicate point in the middle is dropped.
+TEST_F(NoncausalTimestampFilterTest, BackwardsInTimeMiddleDuplicateNegative) {
+ const monotonic_clock::time_point ta(chrono::milliseconds(0));
+ const chrono::nanoseconds oa(chrono::microseconds(1));
+
+ const monotonic_clock::time_point tb(chrono::milliseconds(1));
+ const chrono::nanoseconds ob(chrono::microseconds(2));
+
+ const monotonic_clock::time_point tc(chrono::milliseconds(2));
+ const chrono::nanoseconds oc(chrono::microseconds(1));
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tb, ob);
+ filter.Sample(tc, oc);
+ filter.Sample(tb, chrono::microseconds(0));
+ filter.Debug();
+ EXPECT_EQ(filter.timestamps_size(), 3u);
+
+ EXPECT_EQ(filter.timestamp(0), std::make_tuple(ta, oa));
+ EXPECT_EQ(filter.timestamp(1), std::make_tuple(tb, ob));
+ EXPECT_EQ(filter.timestamp(2), std::make_tuple(tc, oc));
+}
+
+// Tests that inserting a positive duplicate point in the middle is taken.
+TEST_F(NoncausalTimestampFilterTest, BackwardsInTimeMiddleDuplicatePositive) {
+ const monotonic_clock::time_point ta(chrono::milliseconds(0));
+ const chrono::nanoseconds oa(chrono::microseconds(1));
+
+ const monotonic_clock::time_point tb(chrono::milliseconds(1));
+ const chrono::nanoseconds ob(chrono::microseconds(0));
+
+ const monotonic_clock::time_point tc(chrono::milliseconds(2));
+ const chrono::nanoseconds oc(chrono::microseconds(1));
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tb, ob);
+ filter.Sample(tc, oc);
+ filter.Sample(tb, chrono::microseconds(2));
+ filter.Debug();
+ EXPECT_EQ(filter.timestamps_size(), 3u);
+
+ EXPECT_EQ(filter.timestamp(0), std::make_tuple(ta, oa));
+ EXPECT_EQ(filter.timestamp(1), std::make_tuple(tb, chrono::microseconds(2)));
+ EXPECT_EQ(filter.timestamp(2), std::make_tuple(tc, oc));
+}
+
+// Tests that a bunch of points added in any order results in the same answer as
+// adding them in order. 4 points should give us enough combos, and try all the
+// orderings of the points too. Given that the in and out of order code
+// essentially re-implements the same logic, this is an awesome consistency
+// check.
+TEST_F(NoncausalTimestampFilterTest, RandomTimeInsertion) {
+ // 2 ms apart with 1 us of resolution on the delta should give us all sorts of
+ // equality constraints for all sorts of orderings.
+ const std::array<monotonic_clock::time_point, 4> t(
+ {monotonic_clock::time_point(chrono::milliseconds(0)),
+ monotonic_clock::time_point(chrono::milliseconds(2)),
+ monotonic_clock::time_point(chrono::milliseconds(4)),
+ monotonic_clock::time_point(chrono::milliseconds(6))});
+
+ for (int i = -10; i < 10; ++i) {
+ for (int j = -10; j < 10; ++j) {
+ for (int k = -10; k < 10; ++k) {
+ for (int l = -10; l < 10; ++l) {
+ std::array<chrono::nanoseconds, 4> o(
+ {chrono::microseconds(i), chrono::microseconds(j),
+ chrono::microseconds(k), chrono::microseconds(l)});
+ NoncausalTimestampFilter forward(node);
+
+ VLOG(1) << "Sorting in order";
+ forward.Sample(t[0], o[0]);
+ forward.Sample(t[1], o[1]);
+ forward.Sample(t[2], o[2]);
+ forward.Sample(t[3], o[3]);
+
+ // Confirm everything is within the velocity bounds.
+ for (size_t i = 1; i < forward.timestamps_size(); ++i) {
+ const chrono::nanoseconds dt =
+ std::get<0>(forward.timestamp(i)) -
+ std::get<0>(forward.timestamp(i - 1));
+ const chrono::nanoseconds doffset =
+ std::get<1>(forward.timestamp(i)) -
+ std::get<1>(forward.timestamp(i - 1));
+ EXPECT_GE(doffset, -dt * kMaxVelocity());
+ EXPECT_LE(doffset, dt * kMaxVelocity());
+ }
+
+ // Now that we have the correct answer, try all the combos to see
+ // what breaks it.
+ std::array<int, 4> indices({0, 1, 2, 3});
+ int r = 0;
+ do {
+ std::array<
+ std::pair<monotonic_clock::time_point, chrono::nanoseconds>, 4>
+ pairs({std::make_pair(t[indices[0]], o[indices[0]]),
+ std::make_pair(t[indices[1]], o[indices[1]]),
+ std::make_pair(t[indices[2]], o[indices[2]]),
+ std::make_pair(t[indices[3]], o[indices[3]])});
+
+ VLOG(1) << "Sorting randomized";
+ NoncausalTimestampFilter random(node);
+ random.Sample(pairs[0].first, pairs[0].second);
+ if (VLOG_IS_ON(1)) {
+ random.Debug();
+ }
+ random.Sample(pairs[1].first, pairs[1].second);
+ if (VLOG_IS_ON(1)) {
+ random.Debug();
+ }
+ random.Sample(pairs[2].first, pairs[2].second);
+ if (VLOG_IS_ON(1)) {
+ random.Debug();
+ }
+ random.Sample(pairs[3].first, pairs[3].second);
+ if (VLOG_IS_ON(1)) {
+ random.Debug();
+ }
+
+ if (forward.timestamps_size() != random.timestamps_size()) {
+ LOG(INFO) << "Iteration i == " << i << " && j == " << j
+ << " && k == " << k << " && l == " << l
+ << " && r == " << r;
+ LOG(INFO) << "Forward";
+ forward.Debug();
+ LOG(INFO) << "Random";
+ for (int i = 0; i < 4; ++i) {
+ LOG(INFO) << "Sample(" << pairs[i].first << ", "
+ << pairs[i].second.count() << ")";
+ }
+ random.Debug();
+ }
+ ASSERT_EQ(forward.timestamps_size(), random.timestamps_size());
+ for (size_t s = 0; s < forward.timestamps_size(); ++s) {
+ EXPECT_EQ(forward.timestamp(s), random.timestamp(s));
+ }
+ ++r;
+ } while (std::next_permutation(indices.begin(), indices.end()));
+ }
+ }
+ }
+ }
+}
+
+// Tests that the right points get frozen when we ask for them to be.
+TEST_F(NoncausalTimestampFilterTest, FrozenTimestamps) {
+ // Start with the simple case. A valid point in the middle.
+ const monotonic_clock::time_point ta(chrono::milliseconds(0));
+ const chrono::nanoseconds oa(chrono::microseconds(1));
+
+ const monotonic_clock::time_point tb(chrono::milliseconds(1));
+ const chrono::nanoseconds ob(chrono::microseconds(0));
+
+ const monotonic_clock::time_point tc(chrono::milliseconds(2));
+ const chrono::nanoseconds oc(chrono::microseconds(1));
+
+ // Test for our node.
+ {
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tc, oc);
+ filter.Sample(tb, ob);
+ ASSERT_EQ(filter.timestamps_size(), 3u);
+
+ filter.FreezeUntil(ta - chrono::microseconds(1));
+ EXPECT_TRUE(filter.frozen(0));
+ EXPECT_FALSE(filter.frozen(1));
+
+ filter.FreezeUntil(ta);
+ EXPECT_TRUE(filter.frozen(0));
+ EXPECT_FALSE(filter.frozen(1));
+
+ filter.FreezeUntil(ta + chrono::microseconds(1));
+ EXPECT_TRUE(filter.frozen(0));
+ EXPECT_TRUE(filter.frozen(1));
+ EXPECT_FALSE(filter.frozen(2));
+
+ filter.FreezeUntil(tc);
+ EXPECT_TRUE(filter.frozen(0));
+ EXPECT_TRUE(filter.frozen(1));
+ EXPECT_TRUE(filter.frozen(2));
+ }
+
+ // Test that fully frozen doesn't apply when there is 1 time and we are before
+ // the start.
+ {
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+
+ filter.FreezeUntil(ta - chrono::microseconds(1));
+ EXPECT_TRUE(filter.frozen(0));
+
+ // New samples aren't frozen until they are explicitly frozen.
+ filter.Sample(tb, ob);
+ EXPECT_FALSE(filter.frozen(1));
+ filter.FreezeUntil(ta + chrono::microseconds(1));
+
+ EXPECT_TRUE(filter.frozen(1));
+ }
+
+ // Test the remote node
+ {
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tc, oc);
+ filter.Sample(tb, ob);
+ ASSERT_EQ(filter.timestamps_size(), 3u);
+
+ filter.FreezeUntilRemote(ta + oa - chrono::microseconds(1));
+ EXPECT_TRUE(filter.frozen(0));
+ EXPECT_FALSE(filter.frozen(1));
+
+ filter.FreezeUntilRemote(ta + oa);
+ EXPECT_TRUE(filter.frozen(0));
+ EXPECT_FALSE(filter.frozen(1));
+
+ filter.FreezeUntilRemote(ta + oa + chrono::microseconds(1));
+ EXPECT_TRUE(filter.frozen(0));
+ EXPECT_TRUE(filter.frozen(1));
+ EXPECT_FALSE(filter.frozen(2));
+
+ filter.FreezeUntilRemote(tc + oc);
+ EXPECT_TRUE(filter.frozen(0));
+ EXPECT_TRUE(filter.frozen(1));
+ EXPECT_TRUE(filter.frozen(2));
+ }
+
+ // Test that fully frozen doesn't apply when there is 1 time and we are before
+ // the start on the remote node.
+ {
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+
+ filter.FreezeUntilRemote(ta + oa - chrono::microseconds(1));
+ EXPECT_TRUE(filter.frozen(0));
+
+ filter.Sample(tb, ob);
+ EXPECT_FALSE(filter.frozen(1));
+ filter.FreezeUntilRemote(ta + oa + chrono::microseconds(1));
+
+ EXPECT_TRUE(filter.frozen(1));
+ }
+}
+
+// Tests that we refuse to modify frozen points in a bunch of different ways.
+TEST_F(NoncausalTimestampFilterDeathTest, FrozenTimestamps) {
+ // Start with the simple case. A valid point in the middle.
+ const monotonic_clock::time_point ta(chrono::milliseconds(0));
+ const chrono::nanoseconds oa(chrono::microseconds(100));
+
+ const monotonic_clock::time_point tb(chrono::milliseconds(100));
+ const chrono::nanoseconds ob(chrono::microseconds(0));
+
+ const monotonic_clock::time_point tc(chrono::milliseconds(200));
+ const chrono::nanoseconds oc(chrono::microseconds(100));
+
+ {
+ // Test that adding before a frozen sample explodes.
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tb, ob);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
+ filter.FreezeUntil(tb);
+
+ EXPECT_DEATH({ filter.Sample(tb, oa); },
+ "Tried to insert 0.100000000sec before 0.100000000sec, which "
+ "is a frozen time");
+ }
+
+ {
+ // Test that if we freeze it all after the end, we refuse any new samples.
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tb, ob);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
+ filter.FreezeUntil(tc);
+
+ EXPECT_DEATH(
+ { filter.Sample(tc, oc); },
+ "Returned a horizontal line previously and then got a new sample at "
+ "0.200000000sec, 0.2 seconds after the last sample at 0.000000000sec");
+ }
+
+ {
+ // Test that if we freeze it all after the end, we refuse any new samples.
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tc, oc);
+ ASSERT_EQ(filter.timestamps_size(), 2u);
+ filter.FreezeUntil(tc);
+
+ EXPECT_DEATH(
+ { filter.Sample(tb, ob); },
+ "Tried to insert 0.100000000sec before 0.200000000sec, which is a frozen time");
+ }
+
+ {
+ // Test that if we freeze, and a point in the middle triggers back
+ // propagation, we refuse.
+ NoncausalTimestampFilter filter(node);
+
+ filter.Sample(ta, oa);
+ filter.Sample(tb, ob);
+ filter.Sample(tc, oc);
+ ASSERT_EQ(filter.timestamps_size(), 3u);
+ filter.FreezeUntil(tb);
+
+ EXPECT_DEATH(
+ { filter.Sample(tb, oa); },
+ "Tried to insert 0.100000000sec before 0.100000000sec, which is a frozen time");
+ EXPECT_DEATH({ filter.Sample(tb + chrono::nanoseconds(1), oa); },
+ "Can't pop an already frozen sample");
+ }
}
// Tests that all variants of InterpolateOffset do reasonable things.
-TEST(NoncausalTimestampFilterTest, InterpolateOffset) {
+TEST_F(NoncausalTimestampFilterTest, InterpolateOffset) {
const monotonic_clock::time_point e = monotonic_clock::epoch();
const monotonic_clock::time_point t1 = e + chrono::nanoseconds(0);
@@ -382,7 +780,7 @@
}
// Tests that FindTimestamps finds timestamps in a sequence.
-TEST(NoncausalTimestampFilterTest, FindTimestamps) {
+TEST_F(NoncausalTimestampFilterTest, FindTimestamps) {
const monotonic_clock::time_point e = monotonic_clock::epoch();
// Note: t1, t2, t3 need to be picked such that the slop is small so filter
// doesn't modify the timestamps.
@@ -393,7 +791,7 @@
const monotonic_clock::time_point t3 = e + chrono::microseconds(2000);
const chrono::nanoseconds o3 = chrono::nanoseconds(50);
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(t1, o1);
filter.Sample(t2, o2);
@@ -452,7 +850,7 @@
// Tests that Offset returns results indicative of it calling InterpolateOffset
// and FindTimestamps correctly.
-TEST(NoncausalTimestampFilterTest, Offset) {
+TEST_F(NoncausalTimestampFilterTest, Offset) {
const monotonic_clock::time_point e = monotonic_clock::epoch();
// Note: t1, t2, t3 need to be picked such that the slop is small so filter
// doesn't modify the timestamps.
@@ -468,7 +866,7 @@
const chrono::nanoseconds o3 = chrono::nanoseconds(50);
const double o3d = static_cast<double>(o3.count());
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(t1, o1);
@@ -503,14 +901,14 @@
// derivatives are consistent. Do this with a massive offset to ensure that we
// are subtracting out nominal offsets correctly to retain numerical precision
// in the result.
-TEST(NoncausalTimestampFilterTest, CostAndSlopeSinglePoint) {
+TEST_F(NoncausalTimestampFilterTest, CostAndSlopeSinglePoint) {
const monotonic_clock::time_point e = monotonic_clock::epoch();
const monotonic_clock::time_point t1 =
e + chrono::nanoseconds(0) + chrono::seconds(10000000000);
const chrono::nanoseconds o1 =
chrono::nanoseconds(1000) - chrono::seconds(10000000000);
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(t1, o1);
@@ -545,7 +943,7 @@
}
}
-TEST(NoncausalTimestampFilterTest, CostAndSlope) {
+TEST_F(NoncausalTimestampFilterTest, CostAndSlope) {
const monotonic_clock::time_point e = monotonic_clock::epoch();
// Note: t1, t2, t3 need to be picked such that the slope is small so filter
// doesn't modify the timestamps.
@@ -564,7 +962,7 @@
const chrono::nanoseconds o3 =
chrono::nanoseconds(500) - chrono::seconds(10000000000);
- NoncausalTimestampFilter filter;
+ NoncausalTimestampFilter filter(node);
filter.Sample(t1, o1);
filter.Sample(t2, o2);