Add support for freezing samples in NoncausalOffsetEstimator
If a sample changes after it has been used inside timestamp estimation,
that will cause a discontinuity in the time estimation if changed.
Freeze those samples and refuse to modify those.
Change-Id: I65a2035ed74085b862406c3caaa971d9cc5c198f
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 7faf637..5579b9a 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -234,6 +234,14 @@
<< " slope " << std::setprecision(9) << std::fixed
<< slope(node_index);
}
+
+ for (std::pair<const std::tuple<const Node *, const Node *>,
+ message_bridge::NoncausalOffsetEstimator> &filter : filters_) {
+ // TODO(austin): Do we need to freeze up until a time? If we freeze a
+ // single point line segment, we are really assuming that it will never
+ // deviate from horizontal again.
+ filter.second.Freeze();
+ }
}
void MultiNodeNoncausalOffsetEstimator::Initialize(
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 5e18185..edb20a1 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -547,6 +547,9 @@
chrono::duration_cast<chrono::duration<double>>(sample_ns).count());
}
+ CHECK(!fully_frozen_)
+ << ": Returned a horizontal line previously and then got a new sample.";
+
// The first sample is easy. Just do it!
if (timestamps_.size() == 0) {
timestamps_.emplace_back(std::make_tuple(monotonic_now, sample_ns, false));
@@ -569,6 +572,7 @@
// 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();
@@ -576,8 +580,8 @@
doffset = sample_ns - std::get<1>(back);
}
- // TODO(austin): Refuse to modify the 0th element after we have used it.
- timestamps_.emplace_back(std::make_tuple(monotonic_now, sample_ns, true));
+ timestamps_.emplace_back(
+ std::make_tuple(monotonic_now, sample_ns, false));
// 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
@@ -622,8 +626,7 @@
}
}
-bool NoncausalTimestampFilter::Pop(
- aos::monotonic_clock::time_point time) {
+bool NoncausalTimestampFilter::Pop(aos::monotonic_clock::time_point time) {
bool removed = false;
// When the timestamp which is the end of the line is popped, we want to
// drop it off the list. Hence the >=
@@ -634,6 +637,20 @@
return removed;
}
+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.
+ fully_frozen_ = true;
+ } else {
+ std::get<2>(timestamps_[1]) = true;
+ }
+}
+
void NoncausalTimestampFilter::SetFirstTime(
aos::monotonic_clock::time_point time) {
first_time_ = time;
@@ -717,33 +734,36 @@
return false;
}
+void NoncausalOffsetEstimator::Freeze() {
+ a_.Freeze();
+ b_.Freeze();
+}
+
void NoncausalOffsetEstimator::LogFit(std::string_view prefix) {
- const std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- a_timestamps = ATimestamps();
- const std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- b_timestamps = BTimestamps();
+ const std::deque<
+ std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
+ a_timestamps = ATimestamps();
+ const std::deque<
+ std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
+ b_timestamps = BTimestamps();
if (a_timestamps.size() >= 2u) {
- LOG(INFO) << prefix << " " << node_a_->name()->string_view() << " from "
- << node_b_->name()->string_view() << " slope "
- << std::setprecision(20) << fit_.slope() << " offset "
- << fit_.offset().count() << " a [("
- << std::get<0>(a_timestamps[0]) << " -> "
- << std::get<1>(a_timestamps[0]).count() << "ns), ("
- << std::get<0>(a_timestamps[1]) << " -> "
- << std::get<1>(a_timestamps[1]).count()
- << "ns) => {dt: " << std::fixed << std::setprecision(6)
- << std::chrono::duration<double, std::milli>(
- std::get<0>(a_timestamps[1]) -
- std::get<0>(a_timestamps[0]))
- .count()
- << "ms, do: " << std::fixed << std::setprecision(6)
- << std::chrono::duration<double, std::milli>(
- std::get<1>(a_timestamps[1]) -
- std::get<1>(a_timestamps[0]))
- .count()
- << "ms}]";
+ LOG(INFO)
+ << prefix << " " << node_a_->name()->string_view() << " from "
+ << node_b_->name()->string_view() << " slope " << std::setprecision(20)
+ << fit_.slope() << " offset " << fit_.offset().count() << " a [("
+ << std::get<0>(a_timestamps[0]) << " -> "
+ << std::get<1>(a_timestamps[0]).count() << "ns), ("
+ << std::get<0>(a_timestamps[1]) << " -> "
+ << std::get<1>(a_timestamps[1]).count() << "ns) => {dt: " << std::fixed
+ << std::setprecision(6)
+ << std::chrono::duration<double, std::milli>(
+ std::get<0>(a_timestamps[1]) - std::get<0>(a_timestamps[0]))
+ .count()
+ << "ms, do: " << std::fixed << std::setprecision(6)
+ << std::chrono::duration<double, std::milli>(
+ std::get<1>(a_timestamps[1]) - std::get<1>(a_timestamps[0]))
+ .count()
+ << "ms}]";
} else if (a_timestamps.size() == 1u) {
LOG(INFO) << prefix << " " << node_a_->name()->string_view() << " from "
<< node_b_->name()->string_view() << " slope "
@@ -758,25 +778,23 @@
<< fit_.offset().count() << " no samples.";
}
if (b_timestamps.size() >= 2u) {
- LOG(INFO) << prefix << " " << node_a_->name()->string_view() << " from "
- << node_b_->name()->string_view() << " slope "
- << std::setprecision(20) << fit_.slope() << " offset "
- << fit_.offset().count() << " b [("
- << std::get<0>(b_timestamps[0]) << " -> "
- << std::get<1>(b_timestamps[0]).count() << "ns), ("
- << std::get<0>(b_timestamps[1]) << " -> "
- << std::get<1>(b_timestamps[1]).count()
- << "ns) => {dt: " << std::fixed << std::setprecision(6)
- << std::chrono::duration<double, std::milli>(
- std::get<0>(b_timestamps[1]) -
- std::get<0>(b_timestamps[0]))
- .count()
- << "ms, do: " << std::fixed << std::setprecision(6)
- << std::chrono::duration<double, std::milli>(
- std::get<1>(b_timestamps[1]) -
- std::get<1>(b_timestamps[0]))
- .count()
- << "ms}]";
+ LOG(INFO)
+ << prefix << " " << node_a_->name()->string_view() << " from "
+ << node_b_->name()->string_view() << " slope " << std::setprecision(20)
+ << fit_.slope() << " offset " << fit_.offset().count() << " b [("
+ << std::get<0>(b_timestamps[0]) << " -> "
+ << std::get<1>(b_timestamps[0]).count() << "ns), ("
+ << std::get<0>(b_timestamps[1]) << " -> "
+ << std::get<1>(b_timestamps[1]).count() << "ns) => {dt: " << std::fixed
+ << std::setprecision(6)
+ << std::chrono::duration<double, std::milli>(
+ std::get<0>(b_timestamps[1]) - std::get<0>(b_timestamps[0]))
+ .count()
+ << "ms, do: " << std::fixed << std::setprecision(6)
+ << std::chrono::duration<double, std::milli>(
+ std::get<1>(b_timestamps[1]) - std::get<1>(b_timestamps[0]))
+ .count()
+ << "ms}]";
} else if (b_timestamps.size() == 1u) {
LOG(INFO) << prefix << " " << node_b_->name()->string_view() << " from "
<< node_a_->name()->string_view() << " slope "
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 82cc9b8..fa52a6f 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -359,6 +359,11 @@
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();
+
private:
// Removes the oldest timestamp.
void PopFront() {
@@ -382,6 +387,8 @@
FILE *fp_ = nullptr;
FILE *samples_fp_ = nullptr;
+ bool fully_frozen_ = false;
+
aos::monotonic_clock::time_point first_time_ = aos::monotonic_clock::min_time;
};
@@ -403,6 +410,11 @@
bool Pop(const Node *node,
aos::monotonic_clock::time_point node_monotonic_now);
+ // 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();
+
// Returns a line for the oldest segment.
Line fit() const { return fit_; }