Add a way to get the error to the bounds to NoncausalTimestampFilter
Now that we can compute the worst case offset, we want to be able to use
that worst case when enforcing bounds. Expose it in a function, and add
a test.
Change-Id: Ibc1ff97ce2353f564a59eb7794a3739ab07f2495
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/events/logging/boot_timestamp.h b/aos/events/logging/boot_timestamp.h
index 0d68045..bd9b357 100644
--- a/aos/events/logging/boot_timestamp.h
+++ b/aos/events/logging/boot_timestamp.h
@@ -101,6 +101,11 @@
BootTimestamp operator+(BootDuration d) const {
return {boot, time + d.duration};
}
+
+ BootDuration operator-(BootTimestamp t) const {
+ CHECK_EQ(t.boot, boot);
+ return {boot, time - t.time};
+ }
};
// Structure to hold both a boot and queue index. Queue indices reset after
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 4d6946d..c1c0126 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -976,6 +976,35 @@
points.second.first, points.second.second, ta_base, ta));
}
+std::pair<Pointer, std::pair<chrono::nanoseconds, double>>
+NoncausalTimestampFilter::SingleFilter::BoundsOffset(
+ const SingleFilter *other, Pointer pointer,
+ monotonic_clock::time_point ta_base, double ta) const {
+ CHECK_GT(timestamps_size(), 0u) << node_names_;
+ if (IsOutsideSamples(ta_base, ta)) {
+ // Special case size = 1 or ta_base before first timestamp or
+ // after last timestamp, so we need to extrapolate out
+ std::pair<Pointer,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
+ reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+ return std::make_pair(reference_timestamp.first,
+ NoncausalTimestampFilter::ExtrapolateOffset(
+ reference_timestamp.second, ta_base, ta));
+ }
+
+ std::pair<
+ Pointer,
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>>
+ points = FindTimestamps(other, false, pointer, ta_base, ta);
+ CHECK_LT(std::get<0>(points.second.first), std::get<0>(points.second.second));
+ // Return both the integer and double portion together to save a timestamp
+ // lookup.
+ return std::make_pair(points.first, NoncausalTimestampFilter::BoundOffset(
+ points.second.first,
+ points.second.second, ta_base, ta));
+}
+
std::pair<Pointer, double> NoncausalTimestampFilter::SingleFilter::OffsetError(
const SingleFilter *other, Pointer pointer,
aos::monotonic_clock::time_point ta_base, double ta,
@@ -994,6 +1023,25 @@
((tb - ta) - offset.second.second));
}
+std::pair<Pointer, double>
+NoncausalTimestampFilter::SingleFilter::BoundsOffsetError(
+ const SingleFilter *other, Pointer pointer,
+ aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const {
+ NormalizeTimestamps(&ta_base, &ta);
+ NormalizeTimestamps(&tb_base, &tb);
+
+ const std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>> offset =
+ BoundsOffset(other, pointer, ta_base, ta);
+
+ // Compute the integer portion first, and the double portion second. Subtract
+ // the results of each. This handles large offsets without losing precision.
+ return std::make_pair(
+ offset.first,
+ static_cast<double>(((tb_base - ta_base) - offset.second.first).count()) +
+ ((tb - ta) - offset.second.second));
+}
+
std::string NoncausalTimestampFilter::DebugOffsetError(
const NoncausalTimestampFilter *other, Pointer pointer,
BootTimestamp ta_base, double ta, BootTimestamp tb_base, double tb,
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 1367f4c..92053ea 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -330,6 +330,25 @@
result.first.boot_filter_ = boot_filter;
return result;
}
+
+ // Returns the error between the offset in the provided timestamps, and the
+ // bounds offset at ta. Also returns a pointer to the timestamps used for the
+ // lookup to be passed back in again for a more efficient second lookup.
+ std::pair<Pointer, double> BoundsOffsetError(
+ const NoncausalTimestampFilter *other, Pointer pointer,
+ logger::BootTimestamp ta_base, double ta, logger::BootTimestamp tb_base,
+ double tb) const {
+ const BootFilter *boot_filter = filter(pointer, ta_base.boot, tb_base.boot);
+ const SingleFilter *other_filter =
+ other == nullptr
+ ? nullptr
+ : other->maybe_single_filter(tb_base.boot, ta_base.boot);
+ std::pair<Pointer, double> result = boot_filter->filter.BoundsOffsetError(
+ other_filter, pointer, ta_base.time, ta, tb_base.time, tb);
+ result.first.boot_filter_ = boot_filter;
+ return result;
+ }
+
// Returns the string representation of 2 * OffsetError(ta, tb)
std::string DebugOffsetError(const NoncausalTimestampFilter *other,
Pointer pointer, logger::BootTimestamp ta_base,
@@ -500,36 +519,6 @@
}
// Public for testing.
- // Returns the offset for the point in time, using the timestamps in the deque
- // to form a polyline used to interpolate.
- logger::BootDuration Offset(const NoncausalTimestampFilter *other,
- Pointer pointer, logger::BootTimestamp ta,
- size_t sample_boot) const {
- return {sample_boot,
- filter(ta.boot, sample_boot)
- ->filter
- .Offset(other == nullptr
- ? nullptr
- : &other->filter(sample_boot, ta.boot)->filter,
- pointer, ta.time)
- .second};
- }
-
- std::pair<logger::BootDuration, double> Offset(
- const NoncausalTimestampFilter *other, Pointer pointer,
- logger::BootTimestamp ta_base, double ta, size_t sample_boot) const {
- std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>> result =
- filter(ta_base.boot, sample_boot)
- ->filter.Offset(
- other == nullptr
- ? nullptr
- : &other->filter(sample_boot, ta_base.boot)->filter,
- pointer, ta_base.time, ta);
- return std::make_pair(
- logger::BootDuration{sample_boot, result.second.first},
- result.second.second);
- }
-
// Assuming that there are at least 2 points in timestamps_, finds the 2
// matching points.
std::pair<Pointer,
@@ -589,11 +578,6 @@
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
monotonic_clock::time_point ta_base, double ta);
- static double InterpolateOffsetRemainder(
- std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> /*p0*/,
- std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> /*p1*/,
- monotonic_clock::time_point ta_base, double ta);
-
static std::pair<std::chrono::nanoseconds, double> BoundOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
@@ -653,11 +637,21 @@
std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>> Offset(
const SingleFilter *other, Pointer pointer,
monotonic_clock::time_point ta_base, double ta) const;
+
+ std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>>
+ BoundsOffset(const SingleFilter *other, Pointer pointer,
+ monotonic_clock::time_point ta_base, double ta) const;
+
std::pair<Pointer, double> OffsetError(
const SingleFilter *other, Pointer pointer,
aos::monotonic_clock::time_point ta_base, double ta,
aos::monotonic_clock::time_point tb_base, double tb) const;
+ std::pair<Pointer, double> BoundsOffsetError(
+ const SingleFilter *other, Pointer pointer,
+ aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const;
+
bool has_unobserved_line() const;
monotonic_clock::time_point unobserved_line_end() const;
monotonic_clock::time_point unobserved_line_remote_end() const;
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 4aa5377..e99c061 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -34,6 +34,49 @@
return std::make_tuple(BootTimestamp{0, std::get<0>(result)},
BootDuration{0, std::get<1>(result)});
}
+
+ logger::BootDuration Offset(const TestingNoncausalTimestampFilter *other,
+ Pointer pointer, logger::BootTimestamp ta,
+ size_t sample_boot) const {
+ return {sample_boot,
+ filter(ta.boot, sample_boot)
+ ->filter
+ .Offset(other == nullptr
+ ? nullptr
+ : &other->filter(sample_boot, ta.boot)->filter,
+ pointer, ta.time)
+ .second};
+ }
+
+ std::pair<logger::BootDuration, double> Offset(
+ const TestingNoncausalTimestampFilter *other, Pointer pointer,
+ logger::BootTimestamp ta_base, double ta, size_t sample_boot) const {
+ std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>> result =
+ filter(ta_base.boot, sample_boot)
+ ->filter.Offset(
+ other == nullptr
+ ? nullptr
+ : &other->filter(sample_boot, ta_base.boot)->filter,
+ pointer, ta_base.time, ta);
+ return std::make_pair(
+ logger::BootDuration{sample_boot, result.second.first},
+ result.second.second);
+ }
+
+ std::pair<logger::BootDuration, double> BoundsOffset(
+ const TestingNoncausalTimestampFilter *other, Pointer pointer,
+ logger::BootTimestamp ta_base, double ta, size_t sample_boot) const {
+ std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>> result =
+ filter(ta_base.boot, sample_boot)
+ ->filter.BoundsOffset(
+ other == nullptr
+ ? nullptr
+ : &other->filter(sample_boot, ta_base.boot)->filter,
+ pointer, ta_base.time, ta);
+ return std::make_pair(
+ logger::BootDuration{sample_boot, result.second.first},
+ result.second.second);
+ }
};
void NormalizeTimestamps(monotonic_clock::time_point *ta_base, double *ta) {
@@ -1110,8 +1153,8 @@
filter.FindTimestamps(nullptr, true, result.first,
e - chrono::microseconds(10), 0.9, 0));
- result =
- filter.FindTimestamps(nullptr, true, Pointer(), e + chrono::microseconds(0), 0);
+ result = filter.FindTimestamps(nullptr, true, Pointer(),
+ e + chrono::microseconds(0), 0);
EXPECT_THAT(result.second,
::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
::testing::Eq(std::make_tuple(t2, o2))));
@@ -1415,6 +1458,8 @@
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t1, 0), o1);
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t1, 0.0, 0),
std::make_pair(o1, 0.0));
+ EXPECT_EQ(filter.BoundsOffset(nullptr, Pointer(), t1, 0.0, 0),
+ std::make_pair(o1, 0.0));
// Check if we ask for something away from point that we get an offset
// based on the MaxVelocity allowed
const double offset_pre = -(t1.time - e.time).count() * kMaxVelocity();
@@ -1424,6 +1469,10 @@
filter.Offset(nullptr, Pointer(), e, 0.0, 0),
std::make_pair(o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)),
0.0));
+ EXPECT_EQ(
+ filter.BoundsOffset(nullptr, Pointer(), e, 0.0, 0),
+ std::make_pair(o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)),
+ 0.0));
double offset_post = -(t2.time - t1.time).count() * kMaxVelocity();
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t2, 0),
@@ -1432,13 +1481,23 @@
filter.Offset(nullptr, Pointer(), t2, 0.0, 0),
std::make_pair(
o1 + chrono::nanoseconds(static_cast<int64_t>(offset_post)), 0.0));
+ EXPECT_EQ(
+ filter.BoundsOffset(nullptr, Pointer(), t2, 0.0, 0),
+ std::make_pair(
+ o1 + chrono::nanoseconds(static_cast<int64_t>(offset_post)), 0.0));
filter.Sample(t2, o2);
filter.Sample(t3, o3);
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t1, 0), o1);
+ EXPECT_EQ(filter.BoundsOffset(nullptr, Pointer(), t1, 0.0, 0),
+ std::make_pair(o1, 0.0));
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t2, 0), o2);
+ EXPECT_EQ(filter.BoundsOffset(nullptr, Pointer(), t2, 0.0, 0),
+ std::make_pair(o2, 0.0));
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t3, 0), o3);
+ EXPECT_EQ(filter.BoundsOffset(nullptr, Pointer(), t3, 0.0, 0),
+ std::make_pair(o3, 0.0));
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t1, 0.0, 0),
std::make_pair(o1, 0.0));
@@ -1476,6 +1535,9 @@
filter.Offset(nullptr, Pointer(), t4, 0.0, 0),
std::make_pair(
o3 + chrono::nanoseconds(static_cast<int64_t>(offset_post)), 0.0));
+
+ EXPECT_EQ(filter.BoundsOffset(nullptr, Pointer(), t2 + (t3 - t2) / 2, 0.0, 0),
+ std::make_pair(o2 - chrono::nanoseconds(500), 0.0));
}
// Tests that adding duplicates gets correctly deduplicated.