Use the new solver to compute time
Now that all the infrastructure exists, hook it up. Track which nodes
are connected, if there are any orphaned nodes, and everything else the
old code used to do.
This doesn't yet handle single directions going and coming.
Change-Id: I658347797384f7608870d231a3ebbb2c05dad1dc
diff --git a/aos/events/BUILD b/aos/events/BUILD
index 646f597..9533cb5 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -303,6 +303,7 @@
":pong_lib",
":simulated_event_loop",
"//aos/network:remote_message_fbs",
+ "//aos/network:testing_time_converter",
"//aos/testing:googletest",
],
)
diff --git a/aos/events/event_scheduler.cc b/aos/events/event_scheduler.cc
index f5d6c92..49b170d 100644
--- a/aos/events/event_scheduler.cc
+++ b/aos/events/event_scheduler.cc
@@ -44,13 +44,12 @@
void EventScheduler::CallOldestEvent() {
CHECK_GT(events_list_.size(), 0u);
auto iter = events_list_.begin();
- monotonic_now_ = iter->first;
- monotonic_now_valid_ = true;
+ CHECK_EQ(monotonic_now(), iter->first)
+ << ": Time is wrong on node " << node_index_;
::std::function<void()> callback = ::std::move(iter->second);
events_list_.erase(iter);
callback();
- monotonic_now_valid_ = false;
}
void EventScheduler::RunOnRun() {
@@ -158,6 +157,10 @@
}
}
+ if (min_scheduler) {
+ VLOG(1) << "Oldest event " << min_event_time << " on scheduler "
+ << min_scheduler->node_index_;
+ }
return std::make_tuple(min_event_time, min_scheduler);
}
diff --git a/aos/events/event_scheduler.h b/aos/events/event_scheduler.h
index 959caea..397b5f0 100644
--- a/aos/events/event_scheduler.h
+++ b/aos/events/event_scheduler.h
@@ -68,6 +68,13 @@
std::multimap<monotonic_clock::time_point, std::function<void()>>;
using Token = ChannelType::iterator;
+ // Sets the time converter in use for this scheduler (and the corresponding
+ // node index)
+ void SetTimeConverter(size_t node_index, TimeConverter *converter) {
+ node_index_ = node_index;
+ converter_ = converter;
+ }
+
// Schedule an event with a callback function
// Returns an iterator to the event
Token Schedule(monotonic_clock::time_point time,
@@ -98,53 +105,25 @@
// measurement.
distributed_clock::time_point ToDistributedClock(
monotonic_clock::time_point time) const {
- return distributed_clock::epoch() +
- std::chrono::duration_cast<std::chrono::nanoseconds>(
- (time.time_since_epoch() - distributed_offset_) /
- distributed_slope_);
+ return converter_->ToDistributedClock(node_index_, time);
}
// Takes the distributed time and converts it to the monotonic clock for this
// node.
monotonic_clock::time_point FromDistributedClock(
distributed_clock::time_point time) const {
- return monotonic_clock::epoch() +
- std::chrono::duration_cast<std::chrono::nanoseconds>(
- time.time_since_epoch() * distributed_slope_) +
- distributed_offset_;
+ return converter_->FromDistributedClock(node_index_, time);
}
// Returns the current monotonic time on this node calculated from the
// distributed clock.
inline monotonic_clock::time_point monotonic_now() const;
- // Sets the offset between the distributed and monotonic clock.
- // monotonic = distributed * slope + offset;
- void SetDistributedOffset(std::chrono::nanoseconds distributed_offset,
- double distributed_slope) {
- // TODO(austin): Use a starting point to improve precision.
- // TODO(austin): Make slope be the slope of the offset, not the input,
- // throught the calculation process.
- distributed_offset_ = distributed_offset;
- distributed_slope_ = distributed_slope;
-
- // Once we update the offset, now isn't going to be valid anymore.
- // TODO(austin): Probably should instead use the piecewise linear function
- // and evaluate it correctly.
- monotonic_now_valid_ = false;
- }
-
private:
friend class EventSchedulerScheduler;
// Current execution time.
- bool monotonic_now_valid_ = false;
monotonic_clock::time_point monotonic_now_ = monotonic_clock::epoch();
- // Offset to the distributed clock.
- // distributed = monotonic + offset;
- std::chrono::nanoseconds distributed_offset_ = std::chrono::seconds(0);
- double distributed_slope_ = 1.0;
-
// List of functions to run (once) when running.
std::vector<std::function<void()>> on_run_;
@@ -154,6 +133,29 @@
// Pointer to the actual scheduler.
EventSchedulerScheduler *scheduler_scheduler_ = nullptr;
+
+ // Node index handle to be handed back to the TimeConverter. This lets the
+ // same time converter be used for all the nodes, and the node index
+ // distinguish which one.
+ size_t node_index_ = 0;
+
+ // Converts time by doing nothing to it.
+ class UnityConverter final : public TimeConverter {
+ public:
+ distributed_clock::time_point ToDistributedClock(
+ size_t /*node_index*/, monotonic_clock::time_point time) override {
+ return distributed_clock::epoch() + time.time_since_epoch();
+ }
+
+ monotonic_clock::time_point FromDistributedClock(
+ size_t /*node_index*/, distributed_clock::time_point time) override {
+ return monotonic_clock::epoch() + time.time_since_epoch();
+ }
+ };
+
+ UnityConverter unity_converter_;
+
+ TimeConverter *converter_ = &unity_converter_;
};
// We need a heap of heaps...
@@ -211,22 +213,7 @@
};
inline monotonic_clock::time_point EventScheduler::monotonic_now() const {
- // Make sure we stay in sync.
- if (monotonic_now_valid_) {
- // We want time to be smooth, so confirm that it doesn't change too much
- // while handling an event.
- //
- // There are 2 sources of error. There are numerical precision and interger
- // rounding problems going from the monotonic clock to the distributed clock
- // and back again. When we update the time function as well to transition
- // line segments, we have a slight jump as well.
- CHECK_NEAR(monotonic_now_,
- FromDistributedClock(scheduler_scheduler_->distributed_now()),
- std::chrono::nanoseconds(2));
- return monotonic_now_;
- } else {
- return FromDistributedClock(scheduler_scheduler_->distributed_now());
- }
+ return FromDistributedClock(scheduler_scheduler_->distributed_now());
}
inline bool EventScheduler::is_running() const {
diff --git a/aos/events/event_scheduler_test.cc b/aos/events/event_scheduler_test.cc
index e2523db..1ad0a84 100644
--- a/aos/events/event_scheduler_test.cc
+++ b/aos/events/event_scheduler_test.cc
@@ -8,10 +8,52 @@
namespace chrono = std::chrono;
+// Legacy time converter for keeping old tests working. Has numerical precision
+// problems.
+class SlopeOffsetTimeConverter final : public TimeConverter {
+ public:
+ SlopeOffsetTimeConverter(size_t nodes_count)
+ : distributed_offset_(nodes_count, std::chrono::seconds(0)),
+ distributed_slope_(nodes_count, 1.0) {}
+
+ // Sets the offset between the distributed and monotonic clock.
+ // monotonic = distributed * slope + offset;
+ void SetDistributedOffset(size_t node_index,
+ std::chrono::nanoseconds distributed_offset,
+ double distributed_slope) {
+ distributed_offset_[node_index] = distributed_offset;
+ distributed_slope_[node_index] = distributed_slope;
+ }
+
+ distributed_clock::time_point ToDistributedClock(
+ size_t node_index, monotonic_clock::time_point time) override {
+ return distributed_clock::epoch() +
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ (time.time_since_epoch() - distributed_offset_[node_index]) /
+ distributed_slope_[node_index]);
+ }
+
+ monotonic_clock::time_point FromDistributedClock(
+ size_t node_index, distributed_clock::time_point time) override {
+ return monotonic_clock::epoch() +
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ time.time_since_epoch() * distributed_slope_[node_index]) +
+ distributed_offset_[node_index];
+ }
+
+ private:
+ // Offset to the distributed clock.
+ // distributed = monotonic + offset;
+ std::vector<std::chrono::nanoseconds> distributed_offset_;
+ std::vector<double> distributed_slope_;
+};
+
// Tests that the default parameters (slope of 1, offest of 0) behave as
// an identity.
TEST(EventSchedulerTest, IdentityTimeConversion) {
+ SlopeOffsetTimeConverter time(1);
EventScheduler s;
+ s.SetTimeConverter(0u, &time);
EXPECT_EQ(s.FromDistributedClock(distributed_clock::epoch()),
monotonic_clock::epoch());
@@ -22,15 +64,16 @@
EXPECT_EQ(s.ToDistributedClock(monotonic_clock::epoch()),
distributed_clock::epoch());
- EXPECT_EQ(
- s.ToDistributedClock(monotonic_clock::epoch() + chrono::seconds(1)),
- distributed_clock::epoch() + chrono::seconds(1));
+ EXPECT_EQ(s.ToDistributedClock(monotonic_clock::epoch() + chrono::seconds(1)),
+ distributed_clock::epoch() + chrono::seconds(1));
}
// Tests that a non-unity slope is computed correctly.
TEST(EventSchedulerTest, DoubleTimeConversion) {
+ SlopeOffsetTimeConverter time(1);
EventScheduler s;
- s.SetDistributedOffset(std::chrono::seconds(7), 2.0);
+ s.SetTimeConverter(0u, &time);
+ time.SetDistributedOffset(0u, std::chrono::seconds(7), 2.0);
EXPECT_EQ(s.FromDistributedClock(distributed_clock::epoch()),
monotonic_clock::epoch() + chrono::seconds(7));
@@ -42,9 +85,8 @@
EXPECT_EQ(s.ToDistributedClock(monotonic_clock::epoch() + chrono::seconds(7)),
distributed_clock::epoch());
- EXPECT_EQ(
- s.ToDistributedClock(monotonic_clock::epoch() + chrono::seconds(9)),
- distributed_clock::epoch() + chrono::seconds(1));
+ EXPECT_EQ(s.ToDistributedClock(monotonic_clock::epoch() + chrono::seconds(9)),
+ distributed_clock::epoch() + chrono::seconds(1));
}
} // namespace aos
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index 5d21ae4..0d8fe0b 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -307,6 +307,7 @@
"//aos/events:ping_lib",
"//aos/events:pong_lib",
"//aos/events:simulated_event_loop",
+ "//aos/network:testing_time_converter",
"//aos/testing:googletest",
"//aos/testing:tmpdir",
],
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 00322e1..2fe5473 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -766,8 +766,8 @@
// Only set it if this node delivers to the peer timestamp_mapper. Otherwise
// we could needlessly save data.
if (node_data->any_delivered) {
- LOG(INFO) << "Registering on node " << node() << " for peer node "
- << timestamp_mapper->node();
+ VLOG(1) << "Registering on node " << node() << " for peer node "
+ << timestamp_mapper->node();
CHECK(timestamp_mapper->nodes_data_[node()].peer == nullptr);
timestamp_mapper->nodes_data_[node()].peer = this;
diff --git a/aos/events/logging/logger.cc b/aos/events/logging/logger.cc
index d4f4abd..2e0f5db 100644
--- a/aos/events/logging/logger.cc
+++ b/aos/events/logging/logger.cc
@@ -1101,7 +1101,8 @@
remapped_configuration_ = event_loop_factory_->configuration();
filters_ =
std::make_unique<message_bridge::MultiNodeNoncausalOffsetEstimator>(
- event_loop_factory_);
+ event_loop_factory_, logged_configuration(),
+ FLAGS_skip_order_validation);
for (const Node *node : configuration::GetNodes(configuration())) {
const size_t node_index =
@@ -1130,6 +1131,7 @@
state->SetChannelCount(logged_configuration()->channels()->size());
}
+ event_loop_factory_->SetTimeConverter(filters_.get());
for (const Node *node : configuration::GetNodes(configuration())) {
const size_t node_index =
@@ -1161,15 +1163,12 @@
"you sure that the replay config matches the original config?";
}
- filters_->Initialize(logged_configuration());
+ filters_->CheckGraph();
for (std::unique_ptr<State> &state : states_) {
state->SeedSortedMessages();
}
- // And solve.
- UpdateOffsets();
-
// We want to start the log file at the last start time of the log files
// from all the nodes. Compute how long each node's simulation needs to run
// to move time to this point.
@@ -1191,6 +1190,9 @@
start_time, state->ToDistributedClock(state->monotonic_start_time()));
}
+ // TODO(austin): If a node doesn't have a start time, we might not queue
+ // enough. If this happens, we'll explode with a frozen error eventually.
+
CHECK_GE(start_time, distributed_clock::epoch())
<< ": Hmm, we have a node starting before the start of time. Offset "
"everything.";
@@ -1255,14 +1257,6 @@
}
}
-void LogReader::UpdateOffsets() {
- filters_->UpdateOffsets();
-
- if (VLOG_IS_ON(1)) {
- filters_->LogFit("Offset is");
- }
-}
-
message_bridge::NoncausalOffsetEstimator *LogReader::GetFilter(
const Node *node_a, const Node *node_b) {
if (filters_) {
@@ -1344,9 +1338,6 @@
}
return;
}
- if (VLOG_IS_ON(1)) {
- filters_->LogFit("Offset was");
- }
bool update_time;
TimestampedMessage timestamped_message = state->PopOldest(&update_time);
@@ -1388,8 +1379,13 @@
// Confirm that the message was sent on the sending node before the
// destination node (this node). As a proxy, do this by making sure
// that time on the source node is past when the message was sent.
+ //
+ // TODO(austin): <= means that the cause message (which we know) could
+ // happen after the effect even though we know they are at the same
+ // time. I doubt anyone will notice for a bit, but we should really
+ // fix that.
if (!FLAGS_skip_order_validation) {
- CHECK_LT(
+ CHECK_LE(
timestamped_message.monotonic_remote_time,
state->monotonic_remote_now(timestamped_message.channel_index))
<< state->event_loop()->node()->name()->string_view() << " to "
@@ -1401,7 +1397,7 @@
logged_configuration()->channels()->Get(
timestamped_message.channel_index))
<< " " << state->DebugString();
- } else if (timestamped_message.monotonic_remote_time >=
+ } else if (timestamped_message.monotonic_remote_time >
state->monotonic_remote_now(
timestamped_message.channel_index)) {
LOG(WARNING)
@@ -1492,7 +1488,6 @@
return state->monotonic_now();
});
- UpdateOffsets();
VLOG(1) << MaybeNodeName(state->event_loop()->node()) << "Now is now "
<< state->monotonic_now();
diff --git a/aos/events/logging/logger.h b/aos/events/logging/logger.h
index e3f5380..4213066 100644
--- a/aos/events/logging/logger.h
+++ b/aos/events/logging/logger.h
@@ -741,10 +741,6 @@
// less than the second node.
std::unique_ptr<message_bridge::MultiNodeNoncausalOffsetEstimator> filters_;
- // Updates the offset matrix solution and sets the per-node distributed
- // offsets in the factory.
- void UpdateOffsets();
-
std::unique_ptr<FlatbufferDetachedBuffer<Configuration>>
remapped_configuration_buffer_;
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 05bafe4..16fb2ab 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -7,6 +7,7 @@
#include "aos/events/pong_lib.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/network/remote_message_generated.h"
+#include "aos/network/testing_time_converter.h"
#include "aos/network/timestamp_generated.h"
#include "aos/testing/tmpdir.h"
#include "aos/util/file.h"
@@ -427,11 +428,16 @@
MultinodeLoggerTest()
: config_(aos::configuration::ReadConfig(
"aos/events/logging/multinode_pingpong_config.json")),
+ time_converter_(configuration::NodesCount(&config_.message())),
event_loop_factory_(&config_.message()),
pi1_(
configuration::GetNode(event_loop_factory_.configuration(), "pi1")),
+ pi1_index_(configuration::GetNodeIndex(
+ event_loop_factory_.configuration(), pi1_)),
pi2_(
configuration::GetNode(event_loop_factory_.configuration(), "pi2")),
+ pi2_index_(configuration::GetNodeIndex(
+ event_loop_factory_.configuration(), pi2_)),
tmp_dir_(aos::testing::TestTmpDir()),
logfile_base1_(tmp_dir_ + "/multi_logfile1"),
logfile_base2_(tmp_dir_ + "/multi_logfile2"),
@@ -474,6 +480,8 @@
ping_(ping_event_loop_.get()),
pong_event_loop_(event_loop_factory_.MakeEventLoop("pong", pi2_)),
pong_(pong_event_loop_.get()) {
+ event_loop_factory_.SetTimeConverter(&time_converter_);
+
// Go through and remove the logfiles if they already exist.
for (const auto file : logfiles_) {
unlink(file.c_str());
@@ -637,10 +645,13 @@
// Config and factory.
aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ message_bridge::TestingTimeConverter time_converter_;
SimulatedEventLoopFactory event_loop_factory_;
- const Node *pi1_;
- const Node *pi2_;
+ const Node *const pi1_;
+ const size_t pi1_index_;
+ const Node *const pi2_;
+ const size_t pi2_index_;
std::string tmp_dir_;
std::string logfile_base1_;
@@ -723,6 +734,7 @@
// Tests that we can write and read simple multi-node log files.
TEST_F(MultinodeLoggerTest, SimpleMultiNode) {
+ time_converter_.StartEqual();
{
LoggerState pi1_logger = MakeLogger(pi1_);
LoggerState pi2_logger = MakeLogger(pi2_);
@@ -1054,6 +1066,7 @@
// Test that if we feed the replay with a mismatched node list that we die on
// the LogReader constructor.
TEST_F(MultinodeLoggerDeathTest, MultiNodeBadReplayConfig) {
+ time_converter_.StartEqual();
{
LoggerState pi1_logger = MakeLogger(pi1_);
LoggerState pi2_logger = MakeLogger(pi2_);
@@ -1086,6 +1099,7 @@
// Tests that we can read log files where they don't start at the same monotonic
// time.
TEST_F(MultinodeLoggerTest, StaggeredStart) {
+ time_converter_.StartEqual();
{
LoggerState pi1_logger = MakeLogger(pi1_);
LoggerState pi2_logger = MakeLogger(pi2_);
@@ -1187,58 +1201,78 @@
// match correctly. While we are here, also test that different ending times
// also is readable.
TEST_F(MultinodeLoggerTest, MismatchedClocks) {
+ // TODO(austin): Negate...
+ const chrono::nanoseconds initial_pi2_offset = chrono::seconds(1000);
+
+ time_converter_.AddMonotonic({monotonic_clock::epoch(),
+ monotonic_clock::epoch() + initial_pi2_offset});
+ // Wait for 95 ms, (~0.1 seconds - 1/2 of the ping/pong period), and set the
+ // skew to be 200 uS/s
+ const chrono::nanoseconds startup_sleep1 = time_converter_.AddMonotonic(
+ {chrono::milliseconds(95),
+ chrono::milliseconds(95) - chrono::nanoseconds(200) * 95});
+ // Run another 200 ms to have one logger start first.
+ const chrono::nanoseconds startup_sleep2 = time_converter_.AddMonotonic(
+ {chrono::milliseconds(200), chrono::milliseconds(200)});
+ // Slew one way then the other at the same 200 uS/S slew rate. Make sure we
+ // go far enough to cause problems if this isn't accounted for.
+ const chrono::nanoseconds logger_run1 = time_converter_.AddMonotonic(
+ {chrono::milliseconds(20000),
+ chrono::milliseconds(20000) - chrono::nanoseconds(200) * 20000});
+ const chrono::nanoseconds logger_run2 = time_converter_.AddMonotonic(
+ {chrono::milliseconds(40000),
+ chrono::milliseconds(40000) + chrono::nanoseconds(200) * 40000});
+ const chrono::nanoseconds logger_run3 = time_converter_.AddMonotonic(
+ {chrono::milliseconds(400), chrono::milliseconds(400)});
+
{
LoggerState pi2_logger = MakeLogger(pi2_);
+ NodeEventLoopFactory *pi1 =
+ event_loop_factory_.GetNodeEventLoopFactory(pi1_);
NodeEventLoopFactory *pi2 =
event_loop_factory_.GetNodeEventLoopFactory(pi2_);
LOG(INFO) << "pi2 times: " << pi2->monotonic_now() << " "
<< pi2->realtime_now() << " distributed "
<< pi2->ToDistributedClock(pi2->monotonic_now());
- const chrono::nanoseconds initial_pi2_offset = -chrono::seconds(1000);
- chrono::nanoseconds pi2_offset = initial_pi2_offset;
-
- pi2->SetDistributedOffset(-pi2_offset, 1.0);
LOG(INFO) << "pi2 times: " << pi2->monotonic_now() << " "
<< pi2->realtime_now() << " distributed "
<< pi2->ToDistributedClock(pi2->monotonic_now());
- for (int i = 0; i < 95; ++i) {
- pi2_offset += chrono::nanoseconds(200);
- pi2->SetDistributedOffset(-pi2_offset, 1.0);
- event_loop_factory_.RunFor(chrono::milliseconds(1));
- }
+ event_loop_factory_.RunFor(startup_sleep1);
StartLogger(&pi2_logger);
- event_loop_factory_.RunFor(chrono::milliseconds(200));
+ event_loop_factory_.RunFor(startup_sleep2);
{
// Run pi1's logger for only part of the time.
LoggerState pi1_logger = MakeLogger(pi1_);
StartLogger(&pi1_logger);
+ event_loop_factory_.RunFor(logger_run1);
- for (int i = 0; i < 20000; ++i) {
- pi2_offset += chrono::nanoseconds(200);
- pi2->SetDistributedOffset(-pi2_offset, 1.0);
- event_loop_factory_.RunFor(chrono::milliseconds(1));
- }
+ // Make sure we slewed time far enough so that the difference is greater
+ // than the network delay. This confirms that if we sort incorrectly, it
+ // would show in the results.
+ EXPECT_LT(
+ (pi2->monotonic_now() - pi1->monotonic_now()) - initial_pi2_offset,
+ -event_loop_factory_.send_delay() -
+ event_loop_factory_.network_delay());
- EXPECT_GT(pi2_offset - initial_pi2_offset,
- event_loop_factory_.send_delay() +
- event_loop_factory_.network_delay());
+ event_loop_factory_.RunFor(logger_run2);
- for (int i = 0; i < 40000; ++i) {
- pi2_offset -= chrono::nanoseconds(200);
- pi2->SetDistributedOffset(-pi2_offset, 1.0);
- event_loop_factory_.RunFor(chrono::milliseconds(1));
- }
+ // And now check that we went far enough the other way to make sure we
+ // cover both problems.
+ EXPECT_GT(
+ (pi2->monotonic_now() - pi1->monotonic_now()) - initial_pi2_offset,
+ event_loop_factory_.send_delay() +
+ event_loop_factory_.network_delay());
}
// And log a bit more on pi2.
- event_loop_factory_.RunFor(chrono::milliseconds(400));
+ event_loop_factory_.RunFor(logger_run3);
}
LogReader reader(SortParts(logfiles_));
@@ -1341,6 +1375,7 @@
// Tests that we can sort a bunch of parts into the pre-determined sorted parts.
TEST_F(MultinodeLoggerTest, SortParts) {
+ time_converter_.StartEqual();
// Make a bunch of parts.
{
LoggerState pi1_logger = MakeLogger(pi1_);
@@ -1361,6 +1396,7 @@
// Tests that we can sort a bunch of parts with an empty part. We should ignore
// it and remove it from the sorted list.
TEST_F(MultinodeLoggerTest, SortEmptyParts) {
+ time_converter_.StartEqual();
// Make a bunch of parts.
{
LoggerState pi1_logger = MakeLogger(pi1_);
@@ -1388,6 +1424,7 @@
// Tests that we can sort a bunch of parts with an empty .xz file in there. The
// empty file should be ignored.
TEST_F(MultinodeLoggerTest, SortEmptyCompressedParts) {
+ time_converter_.StartEqual();
// Make a bunch of parts.
{
LoggerState pi1_logger = MakeLogger(pi1_);
@@ -1416,6 +1453,7 @@
// Tests that we can sort a bunch of parts with the end missing off a compressed
// file. We should use the part we can read.
TEST_F(MultinodeLoggerTest, SortTruncatedCompressedParts) {
+ time_converter_.StartEqual();
// Make a bunch of parts.
{
LoggerState pi1_logger = MakeLogger(pi1_);
@@ -1447,6 +1485,7 @@
// Tests that if we remap a remapped channel, it shows up correctly.
TEST_F(MultinodeLoggerTest, RemapLoggedChannel) {
+ time_converter_.StartEqual();
{
LoggerState pi1_logger = MakeLogger(pi1_);
LoggerState pi2_logger = MakeLogger(pi2_);
@@ -1513,6 +1552,7 @@
// This should be enough that we can then re-run the logger and get a valid log
// back.
TEST_F(MultinodeLoggerTest, MessageHeader) {
+ time_converter_.StartEqual();
{
LoggerState pi1_logger = MakeLogger(pi1_);
LoggerState pi2_logger = MakeLogger(pi2_);
@@ -1741,15 +1781,13 @@
// Tests that we properly populate and extract the logger_start time by setting
// up a clock difference between 2 nodes and looking at the resulting parts.
TEST_F(MultinodeLoggerTest, LoggerStartTime) {
+ time_converter_.AddMonotonic(
+ {monotonic_clock::epoch(),
+ monotonic_clock::epoch() + chrono::seconds(1000)});
{
LoggerState pi1_logger = MakeLogger(pi1_);
LoggerState pi2_logger = MakeLogger(pi2_);
- NodeEventLoopFactory *pi2 =
- event_loop_factory_.GetNodeEventLoopFactory(pi2_);
-
- pi2->SetDistributedOffset(chrono::seconds(1000), 1.0);
-
StartLogger(&pi1_logger);
StartLogger(&pi2_logger);
@@ -1785,6 +1823,7 @@
// This should be enough that we can then re-run the logger and get a valid log
// back.
TEST_F(MultinodeLoggerDeathTest, RemoteReboot) {
+ time_converter_.StartEqual();
std::string pi2_boot1;
std::string pi2_boot2;
{
@@ -1828,8 +1867,13 @@
// unavailable.
TEST_F(MultinodeLoggerTest, OneDirectionWithNegativeSlope) {
event_loop_factory_.GetNodeEventLoopFactory(pi1_)->Disconnect(pi2_);
- event_loop_factory_.GetNodeEventLoopFactory(pi2_)->SetDistributedOffset(
- chrono::seconds(1000), 0.99999);
+ time_converter_.AddMonotonic(
+ {monotonic_clock::epoch(),
+ monotonic_clock::epoch() + chrono::seconds(1000)});
+
+ time_converter_.AddMonotonic(
+ {chrono::milliseconds(10000),
+ chrono::milliseconds(10000) - chrono::milliseconds(1)});
{
LoggerState pi1_logger = MakeLogger(pi1_);
@@ -1849,8 +1893,36 @@
// unavailable.
TEST_F(MultinodeLoggerTest, OneDirectionWithPositiveSlope) {
event_loop_factory_.GetNodeEventLoopFactory(pi1_)->Disconnect(pi2_);
- event_loop_factory_.GetNodeEventLoopFactory(pi2_)->SetDistributedOffset(
- chrono::seconds(500), 1.00001);
+ time_converter_.AddMonotonic(
+ {monotonic_clock::epoch(),
+ monotonic_clock::epoch() + chrono::seconds(500)});
+
+ time_converter_.AddMonotonic(
+ {chrono::milliseconds(10000),
+ chrono::milliseconds(10000) + chrono::milliseconds(1)});
+ {
+ LoggerState pi1_logger = MakeLogger(pi1_);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ StartLogger(&pi1_logger);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(10000));
+ }
+
+ // Confirm that we can parse the result. LogReader has enough internal CHECKs
+ // to confirm the right thing happened.
+ ConfirmReadable(pi1_single_direction_logfiles_);
+}
+
+// Tests that we properly handle a dead node. Do this by just disconnecting it
+// and only using one nodes of logs.
+TEST_F(MultinodeLoggerTest, DeadNode) {
+ event_loop_factory_.GetNodeEventLoopFactory(pi1_)->Disconnect(pi2_);
+ event_loop_factory_.GetNodeEventLoopFactory(pi2_)->Disconnect(pi1_);
+ time_converter_.AddMonotonic(
+ {monotonic_clock::epoch(),
+ monotonic_clock::epoch() + chrono::seconds(1000)});
{
LoggerState pi1_logger = MakeLogger(pi1_);
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 8621851..750df39 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -1034,6 +1034,13 @@
return result->get();
}
+void SimulatedEventLoopFactory::SetTimeConverter(
+ TimeConverter *time_converter) {
+ for (std::unique_ptr<NodeEventLoopFactory> &factory : node_factories_) {
+ factory->SetTimeConverter(time_converter);
+ }
+}
+
::std::unique_ptr<EventLoop> SimulatedEventLoopFactory::MakeEventLoop(
std::string_view name, const Node *node) {
if (node == nullptr) {
@@ -1092,9 +1099,7 @@
}
}
-void SimulatedEventLoopFactory::Exit() {
- scheduler_scheduler_.Exit();
-}
+void SimulatedEventLoopFactory::Exit() { scheduler_scheduler_.Exit(); }
void SimulatedEventLoopFactory::DisableForwarding(const Channel *channel) {
CHECK(bridge_) << ": Can't disable forwarding without a message bridge.";
diff --git a/aos/events/simulated_event_loop.h b/aos/events/simulated_event_loop.h
index 8494593..4a2b289 100644
--- a/aos/events/simulated_event_loop.h
+++ b/aos/events/simulated_event_loop.h
@@ -73,6 +73,9 @@
// lifetime identical to the factory.
NodeEventLoopFactory *GetNodeEventLoopFactory(const Node *node);
+ // Sets the time converter for all nodes.
+ void SetTimeConverter(TimeConverter *time_converter);
+
// Starts executing the event loops unconditionally.
void Run();
// Executes the event loops for a duration.
@@ -166,16 +169,19 @@
// Converts a time to the distributed clock for scheduling and cross-node time
// measurement.
+ // Note: converting time too far in the future can cause problems when
+ // replaying logs. Only convert times in the present or near past.
inline distributed_clock::time_point ToDistributedClock(
monotonic_clock::time_point time) const;
inline monotonic_clock::time_point FromDistributedClock(
distributed_clock::time_point time) const;
- // Sets the offset between the monotonic clock and the central distributed
- // clock. distributed_clock = monotonic_clock + offset.
- void SetDistributedOffset(std::chrono::nanoseconds monotonic_offset,
- double monotonic_slope) {
- scheduler_.SetDistributedOffset(monotonic_offset, monotonic_slope);
+ // Sets the class used to convert time. This pointer must out-live the
+ // SimulatedEventLoopFactory.
+ void SetTimeConverter(TimeConverter *time_converter) {
+ scheduler_.SetTimeConverter(
+ configuration::GetNodeIndex(factory_->configuration(), node_),
+ time_converter);
}
// Returns the boot UUID for this node.
diff --git a/aos/events/simulated_event_loop_test.cc b/aos/events/simulated_event_loop_test.cc
index 18e7cc9..0fb757b 100644
--- a/aos/events/simulated_event_loop_test.cc
+++ b/aos/events/simulated_event_loop_test.cc
@@ -11,6 +11,7 @@
#include "aos/network/message_bridge_client_generated.h"
#include "aos/network/message_bridge_server_generated.h"
#include "aos/network/remote_message_generated.h"
+#include "aos/network/testing_time_converter.h"
#include "aos/network/timestamp_generated.h"
#include "gtest/gtest.h"
@@ -690,15 +691,27 @@
aos::configuration::ReadConfig(ConfigPrefix() +
"events/multinode_pingpong_config.json");
const Node *pi1 = configuration::GetNode(&config.message(), "pi1");
+ const size_t pi1_index = configuration::GetNodeIndex(&config.message(), pi1);
+ ASSERT_EQ(pi1_index, 0u);
const Node *pi2 = configuration::GetNode(&config.message(), "pi2");
+ const size_t pi2_index = configuration::GetNodeIndex(&config.message(), pi2);
+ ASSERT_EQ(pi2_index, 1u);
const Node *pi3 = configuration::GetNode(&config.message(), "pi3");
+ const size_t pi3_index = configuration::GetNodeIndex(&config.message(), pi3);
+ ASSERT_EQ(pi3_index, 2u);
+ message_bridge::TestingTimeConverter time(
+ configuration::NodesCount(&config.message()));
SimulatedEventLoopFactory simulated_event_loop_factory(&config.message());
NodeEventLoopFactory *pi2_factory =
simulated_event_loop_factory.GetNodeEventLoopFactory(pi2);
+ pi2_factory->SetTimeConverter(&time);
constexpr chrono::milliseconds kOffset{1501};
- pi2_factory->SetDistributedOffset(kOffset, 1.0);
+ time.AddNextTimestamp(
+ distributed_clock::epoch(),
+ {monotonic_clock::epoch(), monotonic_clock::epoch() + kOffset,
+ monotonic_clock::epoch()});
std::unique_ptr<EventLoop> ping_event_loop =
simulated_event_loop_factory.MakeEventLoop("ping", pi1);
@@ -711,15 +724,15 @@
// Wait to let timestamp estimation start up before looking for the results.
simulated_event_loop_factory.RunFor(chrono::milliseconds(500));
+ std::unique_ptr<EventLoop> pi1_pong_counter_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("pi1_pong_counter", pi1);
+
std::unique_ptr<EventLoop> pi2_pong_counter_event_loop =
simulated_event_loop_factory.MakeEventLoop("pi2_pong_counter", pi2);
std::unique_ptr<EventLoop> pi3_pong_counter_event_loop =
simulated_event_loop_factory.MakeEventLoop("pi3_pong_counter", pi3);
- std::unique_ptr<EventLoop> pi1_pong_counter_event_loop =
- simulated_event_loop_factory.MakeEventLoop("pi1_pong_counter", pi1);
-
// Confirm the offsets are being recovered correctly.
int pi1_server_statistics_count = 0;
pi1_pong_counter_event_loop->MakeWatcher(
@@ -1188,16 +1201,32 @@
aos::configuration::ReadConfig(ConfigPrefix() +
"events/multinode_pingpong_config.json");
const Node *pi1 = configuration::GetNode(&config.message(), "pi1");
+ const size_t pi1_index = configuration::GetNodeIndex(&config.message(), pi1);
+ ASSERT_EQ(pi1_index, 0u);
const Node *pi2 = configuration::GetNode(&config.message(), "pi2");
+ const size_t pi2_index = configuration::GetNodeIndex(&config.message(), pi2);
+ ASSERT_EQ(pi2_index, 1u);
+ const Node *pi3 = configuration::GetNode(&config.message(), "pi3");
+ const size_t pi3_index = configuration::GetNodeIndex(&config.message(), pi3);
+ ASSERT_EQ(pi3_index, 2u);
+ message_bridge::TestingTimeConverter time(
+ configuration::NodesCount(&config.message()));
SimulatedEventLoopFactory simulated_event_loop_factory(&config.message());
NodeEventLoopFactory *pi2_factory =
simulated_event_loop_factory.GetNodeEventLoopFactory(pi2);
+ pi2_factory->SetTimeConverter(&time);
- // Move the pi far into the future so the slope is significant. And set it to
- // something reasonable.
constexpr chrono::milliseconds kOffset{150100};
- pi2_factory->SetDistributedOffset(kOffset, 1.0001);
+ time.AddNextTimestamp(
+ distributed_clock::epoch(),
+ {monotonic_clock::epoch(), monotonic_clock::epoch() + kOffset,
+ monotonic_clock::epoch()});
+ time.AddNextTimestamp(
+ distributed_clock::epoch() + chrono::seconds(10),
+ {monotonic_clock::epoch() + chrono::milliseconds(9999),
+ monotonic_clock::epoch() + kOffset + chrono::seconds(10),
+ monotonic_clock::epoch() + chrono::milliseconds(9999)});
std::unique_ptr<EventLoop> ping_event_loop =
simulated_event_loop_factory.MakeEventLoop("ping", pi1);
diff --git a/aos/network/BUILD b/aos/network/BUILD
index d8fc474..4ceab73 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -460,7 +460,6 @@
deps = [
"//aos:configuration",
"//aos/time",
- "//third_party/gmp",
"@com_google_absl//absl/strings",
],
)
@@ -475,7 +474,6 @@
"//aos:configuration",
"//aos/events:simulated_event_loop",
"//aos/time",
- "//third_party/gmp",
"@com_github_stevengj_nlopt//:nlopt",
"@org_tuxfamily_eigen//:eigen",
],
@@ -517,6 +515,7 @@
":testing_time_converter",
":timestamp_filter",
"//aos/testing:googletest",
+ "//third_party/gmp",
"@com_github_stevengj_nlopt//:nlopt",
],
)
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index cfa74d4..8d39b9d 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -21,62 +21,26 @@
namespace message_bridge {
namespace {
namespace chrono = std::chrono;
-Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> ToDouble(
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> in) {
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> result =
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(in.rows(),
- in.cols());
- for (int i = 0; i < in.rows(); ++i) {
- for (int j = 0; j < in.cols(); ++j) {
- result(i, j) = in(i, j).get_d();
- }
- }
- return result;
}
-std::tuple<Eigen::Matrix<double, Eigen::Dynamic, 1>,
- Eigen::Matrix<double, Eigen::Dynamic, 1>>
-Solve(const Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> &mpq_map,
- const Eigen::Matrix<mpq_class, Eigen::Dynamic, 1> &mpq_offsets) {
- aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
- // Least squares solve for the slopes and offsets.
- const Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> inv =
- (mpq_map.transpose() * mpq_map).inverse() * mpq_map.transpose();
- aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
-
- VLOG(3) << "Took "
- << std::chrono::duration<double>(end_time - start_time).count()
- << " seconds to invert";
-
- Eigen::Matrix<mpq_class, Eigen::Dynamic, 1> mpq_solution_slope =
- inv.block(0, 0, inv.rows(), 1);
- Eigen::Matrix<mpq_class, Eigen::Dynamic, 1> mpq_solution_offset =
- inv.block(0, 1, inv.rows(), inv.cols() - 1) *
- mpq_offsets.block(1, 0, inv.rows() - 1, 1);
-
- mpq_solution_offset *= mpq_class(1, 1000000000);
-
- return std::make_tuple(ToDouble(mpq_solution_slope),
- ToDouble(mpq_solution_offset));
-}
-} // namespace
-
TimestampProblem::TimestampProblem(size_t count) {
CHECK_GT(count, 1u);
filters_.resize(count);
base_clock_.resize(count);
+ live_.resize(count, true);
+ node_mapping_.resize(count, 0);
}
-// TODO(austin): Adjust simulated event loop factory to take lists of points
-// on all clocks and interpolate.
-//
// TODO(austin): Add linear inequality constraints too.
//
// TODO(austin): Add a rate of change constraint from the last sample. 1
// ms/s. Figure out how to define it. Do this last. This lets us handle
// constraints going away, and constraints close in time.
+//
+// TODO(austin): Use the timestamp of the remote timestamp as more data.
std::vector<double> TimestampProblem::SolveDouble() {
+ MaybeUpdateNodeMapping();
// TODO(austin): Add constraints for relevant segments.
const size_t n = filters_.size() - 1u;
// NLOPT_LD_MMA and NLOPT_LD_LBFGS are alternative solvers, but SLSQP is a
@@ -88,12 +52,12 @@
// precise.
nlopt_set_xtol_rel(opt, 1e-5);
- count_ = 0;
+ cost_call_count_ = 0;
std::vector<double> result(n, 0.0);
double minf = 0.0;
nlopt_result status = nlopt_optimize(opt, result.data(), &minf);
- if (status < 0) {
+ if (status < 0) {
if (status == NLOPT_ROUNDOFF_LIMITED) {
constexpr double kTolerance = 1e-9;
std::vector<double> gradient(n, 0.0);
@@ -105,6 +69,8 @@
// thing.
std::vector<monotonic_clock::time_point> new_base =
DoubleToMonotonic(result.data());
+ // Put the result into base_clock_ so Debug prints out something
+ // useful.
base_clock_ = std::move(new_base);
Debug();
CHECK_LE(std::abs(g), kTolerance)
@@ -132,9 +98,9 @@
};
VLOG(1) << std::setprecision(12) << std::fixed << "Found minimum at f("
- << result[0] << ") -> " << minf << " grad ["
+ << absl::StrJoin(result, ", ") << ") -> " << minf << " grad ["
<< absl::StrJoin(gradient, ", ", MyFormatter()) << "] after "
- << count_ << " cycles for node " << solution_node_ << ".";
+ << cost_call_count_ << " cycles for node " << solution_node_ << ".";
}
nlopt_destroy(opt);
return result;
@@ -144,9 +110,12 @@
const double *r) const {
std::vector<monotonic_clock::time_point> result(filters_.size());
for (size_t i = 0; i < result.size(); ++i) {
- result[i] =
- base_clock(i) +
- std::chrono::nanoseconds(static_cast<int64_t>(std::floor(get_t(r, i))));
+ if (live(i)) {
+ result[i] = base_clock(i) + std::chrono::nanoseconds(static_cast<int64_t>(
+ std::floor(get_t(r, i))));
+ } else {
+ result[i] = monotonic_clock::min_time;
+ }
}
return result;
@@ -157,8 +126,8 @@
return DoubleToMonotonic(solution.data());
}
-double TimestampProblem::Cost(const double *x, double *grad) {
- ++count_;
+double TimestampProblem::Cost(const double *time_offsets, double *grad) {
+ ++cost_call_count_;
if (grad != nullptr) {
for (size_t i = 0; i < filters_.size() - 1u; ++i) {
@@ -169,13 +138,13 @@
for (const struct FilterPair &filter : filters_[i]) {
if (i != solution_node_) {
grad[NodeToSolutionIndex(i)] += filter.filter->DCostDta(
- base_clock_[i], get_t(x, i), base_clock_[filter.b_index],
- get_t(x, filter.b_index));
+ base_clock_[i], get_t(time_offsets, i),
+ base_clock_[filter.b_index], get_t(time_offsets, filter.b_index));
}
if (filter.b_index != solution_node_) {
grad[NodeToSolutionIndex(filter.b_index)] += filter.filter->DCostDtb(
- base_clock_[i], get_t(x, i), base_clock_[filter.b_index],
- get_t(x, filter.b_index));
+ base_clock_[i], get_t(time_offsets, i),
+ base_clock_[filter.b_index], get_t(time_offsets, filter.b_index));
}
}
}
@@ -184,9 +153,9 @@
double cost = 0;
for (size_t i = 0u; i < filters_.size(); ++i) {
for (const struct FilterPair &filter : filters_[i]) {
- cost += filter.filter->Cost(base_clock_[i], get_t(x, i),
+ cost += filter.filter->Cost(base_clock_[i], get_t(time_offsets, i),
base_clock_[filter.b_index],
- get_t(x, filter.b_index));
+ get_t(time_offsets, filter.b_index));
}
}
@@ -216,14 +185,16 @@
LOG(INFO) << std::setprecision(12) << std::fixed
<< "Evaluated minimum at f("
- << absl::StrJoin(DoubleToMonotonic(x), ", ", MyFormatter())
- << ") -> " << cost << gradient << " after " << count_
+ << absl::StrJoin(DoubleToMonotonic(time_offsets), ", ",
+ MyFormatter())
+ << ") -> " << cost << gradient << " after " << cost_call_count_
<< " cycles.";
}
return cost;
}
void TimestampProblem::Debug() {
+ MaybeUpdateNodeMapping();
LOG(INFO) << "Solving for node " << solution_node_ << " at "
<< base_clock_[solution_node_];
@@ -241,13 +212,13 @@
for (size_t i = 0u; i < filters_.size(); ++i) {
std::string gradient = "0.0";
for (const struct FilterPair &filter : filters_[i]) {
- if (i != solution_node_) {
+ if (i != solution_node_ && live(i)) {
gradients[NodeToSolutionIndex(i)].emplace_back(
filter.filter->DebugDCostDta(base_clock_[i], 0.0,
base_clock_[filter.b_index], 0.0, i,
filter.b_index));
}
- if (filter.b_index != solution_node_) {
+ if (filter.b_index != solution_node_ && live(filter.b_index)) {
gradients[NodeToSolutionIndex(filter.b_index)].emplace_back(
filter.filter->DebugDCostDtb(base_clock_[i], 0.0,
base_clock_[filter.b_index], 0.0, i,
@@ -257,13 +228,14 @@
}
for (size_t i = 0u; i < filters_.size(); ++i) {
- LOG(INFO) << "Grad[" << i << "] = "
+ LOG(INFO) << (live(i) ? "live" : "dead") << " Grad[" << i << "] = "
<< (gradients[i].empty() ? std::string("0.0")
: absl::StrJoin(gradients[i], " + "));
}
for (size_t i = 0u; i < filters_.size(); ++i) {
- LOG(INFO) << "base_clock[" << i << "] = " << base_clock_[i];
+ LOG(INFO) << (live(i) ? "live" : "dead") << " base_clock[" << i
+ << "] = " << base_clock_[i];
}
}
@@ -277,11 +249,12 @@
std::vector<monotonic_clock::time_point>>>
next_time = NextTimestamp();
if (!next_time) {
- VLOG(1) << "Last timestamp, calling it quits";
+ LOG(INFO) << "Last timestamp, calling it quits";
at_end_ = true;
break;
}
- VLOG(1) << "Fetched next timestamp while solving.";
+ VLOG(1) << "Fetched next timestamp while solving: "
+ << std::get<0>(*next_time) << " ->";
for (monotonic_clock::time_point t : std::get<1>(*next_time)) {
VLOG(1) << " " << t;
}
@@ -447,6 +420,54 @@
<< result;
return result;
}
+MultiNodeNoncausalOffsetEstimator::MultiNodeNoncausalOffsetEstimator(
+ SimulatedEventLoopFactory *event_loop_factory,
+ const Configuration *logged_configuration, bool skip_order_validation)
+ : InterpolatedTimeConverter(!configuration::MultiNode(logged_configuration)
+ ? 1u
+ : logged_configuration->nodes()->size()),
+ event_loop_factory_(event_loop_factory),
+ logged_configuration_(logged_configuration),
+ skip_order_validation_(skip_order_validation) {
+ filters_per_node_.resize(NodesCount());
+ last_monotonics_.resize(NodesCount(), aos::monotonic_clock::epoch());
+ if (FLAGS_timestamps_to_csv &&
+ configuration::MultiNode(logged_configuration)) {
+ fp_ = fopen("/tmp/timestamp_noncausal_offsets.csv", "w");
+ fprintf(fp_, "# distributed");
+ for (const Node *node : configuration::GetNodes(logged_configuration)) {
+ fprintf(fp_, ", %s", node->name()->c_str());
+ }
+ fprintf(fp_, "\n");
+ }
+}
+
+MultiNodeNoncausalOffsetEstimator::~MultiNodeNoncausalOffsetEstimator() {
+ if (fp_) {
+ fclose(fp_);
+ fp_ = NULL;
+ }
+ if (all_done_) {
+ size_t node_a_index = 0;
+ for (const auto &filters : filters_per_node_) {
+ for (const auto &filter : filters) {
+ std::optional<
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
+ next = filter.filter->Consume();
+ if (next) {
+ skip_order_validation_
+ ? LOG(WARNING)
+ : LOG(FATAL) << "MultiNodeNoncausalOffsetEstimator reported all "
+ "done, but "
+ << node_a_index << " -> " << filter.b_index
+ << " found more data at time " << std::get<0>(*next)
+ << ". Time estimation was silently wrong.";
+ }
+ }
+ ++node_a_index;
+ }
+ }
+}
void MultiNodeNoncausalOffsetEstimator::Start(
SimulatedEventLoopFactory *factory) {
@@ -483,6 +504,18 @@
tuple,
message_bridge::NoncausalOffsetEstimator(node_a, node_b)))
.first->second;
+
+ const size_t node_a_index =
+ configuration::GetNodeIndex(logged_configuration_, node_a);
+ const size_t node_b_index =
+ configuration::GetNodeIndex(logged_configuration_, node_b);
+
+ // TODO(austin): Do a better job documenting which node is which here.
+ filters_per_node_[node_a_index].emplace_back(x.GetFilter(node_a),
+ node_b_index);
+ filters_per_node_[node_b_index].emplace_back(x.GetFilter(node_b),
+ node_a_index);
+
if (FLAGS_timestamps_to_csv) {
x.SetFwdCsvFileName(absl::StrCat("/tmp/timestamp_noncausal_",
node_a->name()->string_view(), "_",
@@ -498,420 +531,498 @@
}
}
-void MultiNodeNoncausalOffsetEstimator::LogFit(std::string_view prefix) {
- for (size_t node_index = 0; node_index < nodes_count(); ++node_index) {
- const Node *node = configuration::GetNode(configuration(), node_index);
- VLOG(1)
- << node->name()->string_view() << " now "
- << event_loop_factory_->GetNodeEventLoopFactory(node)->monotonic_now()
- << " distributed " << event_loop_factory_->distributed_now();
+TimeComparison CompareTimes(const std::vector<monotonic_clock::time_point> &ta,
+ const std::vector<monotonic_clock::time_point> &tb,
+ bool ignore_min_time) {
+ if (ta.size() != tb.size() || ta.empty()) {
+ return TimeComparison::kInvalid;
+ }
+ bool is_less = false;
+ bool is_greater = false;
+ bool is_eq = true;
+ for (size_t i = 0; i < ta.size(); ++i) {
+ if (ignore_min_time && tb[i] == monotonic_clock::min_time) {
+ continue;
+ }
+ if (ta[i] < tb[i]) {
+ is_less = true;
+ is_eq = false;
+ } else if (ta[i] > tb[i]) {
+ is_greater = true;
+ is_eq = false;
+ }
}
- for (std::pair<const std::tuple<const Node *, const Node *>,
- message_bridge::NoncausalOffsetEstimator> &filter : filters_) {
- message_bridge::NoncausalOffsetEstimator *estimator = &filter.second;
+ if (is_eq) {
+ return TimeComparison::kEq;
+ }
+ if (is_less && !is_greater) {
+ return TimeComparison::kBefore;
+ }
+ if (!is_less && is_greater) {
+ return TimeComparison::kAfter;
+ } else {
+ // If we have elements which are both < and >, that is a problem. We are
+ // trying to order timestamps with this code, and equality doesn't provide
+ // an ordering.
+ return TimeComparison::kInvalid;
+ }
+}
- const std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- a_timestamps = estimator->ATimestamps();
- const std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- b_timestamps = estimator->BTimestamps();
-
- if (a_timestamps.size() == 0 && b_timestamps.size() == 0) {
+chrono::nanoseconds MaxElapsedTime(
+ const std::vector<monotonic_clock::time_point> &ta,
+ const std::vector<monotonic_clock::time_point> &tb) {
+ CHECK_EQ(ta.size(), tb.size());
+ CHECK(!ta.empty());
+ bool first = true;
+ chrono::nanoseconds dt;
+ for (size_t i = 0; i < ta.size(); ++i) {
+ // Skip any invalid timestamps.
+ if (tb[i] == monotonic_clock::min_time) {
continue;
}
- if (VLOG_IS_ON(1)) {
- estimator->LogFit(prefix);
+ const chrono::nanoseconds dti = tb[i] - ta[i];
+ if (first || dti > dt) {
+ dt = dti;
+ }
+ first = false;
+ }
+ return dt;
+}
+
+// Class to efficiently track up to 64 bit sets. It uses a uint64 as the
+// backing store, and ffs() to find the first bit set efficiently so we can
+// iterate through the set.
+class BitSet64 {
+ public:
+ BitSet64(size_t size) : size_(size) {
+ // Cheat a bit... Some of the math below assumes that 1 << size fits in a
+ // 64 bit number, and it isn't worth fixing this assumption.
+ CHECK_LE(size, (sizeof(uint64_t) * 8) - 1);
+ }
+
+ size_t size() const { return size_; }
+
+ void Set(size_t index, bool val) {
+ if (val) {
+ data_ |= (1 << index);
+ } else {
+ data_ &= ~(1 << index);
+ }
+ }
+ bool Get(size_t index) const { return (data_ & (1 << index)) != 0; }
+
+ bool operator==(const BitSet64 &other) const {
+ return size_ == other.size_ && data_ == other.data_;
+ }
+
+ bool operator!=(const BitSet64 &other) const {
+ return size_ != other.size_ || data_ != other.data_;
+ }
+
+ // Flips all the indices in the set.
+ BitSet64 operator~() const {
+ // Only flip the bits in the set to keep == working above.
+ BitSet64 result(*this);
+ result.data_ = data_ ^ ((1u << size_) - 1u);
+ return result;
+ }
+
+ // Returns a set for which only the bits which are set in both inputs sets are
+ // set.
+ BitSet64 operator&(const BitSet64 &other) const {
+ BitSet64 result(*this);
+ result.data_ = data_ & other.data_;
+ return result;
+ }
+
+ // Returns the first bit set at or after start. Returns size() if no more
+ // bits are set.
+ size_t FirstBitSet(size_t start) const {
+ static_assert(sizeof(uintmax_t) == sizeof(int64_t),
+ "ffsimax is the wrong size");
+ const int ffs = __builtin_ffsll(data_ & (~((1u << start) - 1u)));
+ if (ffs == 0) {
+ return size_;
}
- const Node *const node_a = std::get<0>(filter.first);
- const Node *const node_b = std::get<1>(filter.first);
+ return ffs - 1;
+ }
- const size_t node_a_index =
- configuration::GetNodeIndex(configuration(), node_a);
- const size_t node_b_index =
- configuration::GetNodeIndex(configuration(), node_b);
+ private:
+ size_t size_;
+ uint64_t data_ = 0;
+};
- NodeEventLoopFactory *node_a_factory =
- event_loop_factory_->GetNodeEventLoopFactory(node_a);
- NodeEventLoopFactory *node_b_factory =
- event_loop_factory_->GetNodeEventLoopFactory(node_b);
+void MultiNodeNoncausalOffsetEstimator::CheckGraph() {
+ // Question doesn't make sense.
+ if (NodesCount() == 1) {
+ return;
+ }
- const double recovered_slope =
- slope(node_b_index) / slope(node_a_index) - 1.0;
- const int64_t recovered_offset =
- offset(node_b_index).count() - offset(node_a_index).count() *
- slope(node_b_index) /
- slope(node_a_index);
+ // Find a starting point.
+ BitSet64 all_nodes(NodesCount());
+ size_t node_a_index = 0;
+ bool found_start = false;
+ for (const auto &filters : filters_per_node_) {
+ for (const auto &filter : filters) {
+ // Pick an initial seed.
+ all_nodes.Set(filter.b_index, true);
+ all_nodes.Set(node_a_index, true);
+ found_start = true;
+ break;
+ }
+ ++node_a_index;
+ }
- VLOG(2) << "Recovered slope " << std::setprecision(20) << recovered_slope
- << " (error " << recovered_slope - estimator->fit().slope() << ") "
- << " offset " << std::setprecision(20) << recovered_offset
- << " (error "
- << recovered_offset - estimator->fit().offset().count() << ")";
+ CHECK(found_start) << ": Failed to find any connected nodes in a graph of "
+ << NodesCount();
- const aos::distributed_clock::time_point a0 =
- node_a_factory->ToDistributedClock(std::get<0>(a_timestamps[0]));
- const aos::distributed_clock::time_point a1 =
- node_a_factory->ToDistributedClock(std::get<0>(a_timestamps[1]));
+ // The set of nodes we have visited.
+ BitSet64 visited_set(all_nodes.size());
+ while (true) {
+ // Compute the set of all nodes which are new and haven't been visited to
+ // track down dependencies.
+ const BitSet64 new_nodes = all_nodes & ~visited_set;
- VLOG(2) << node_a->name()->string_view()
- << " timestamps()[0] = " << std::get<0>(a_timestamps[0]) << " -> "
- << a0 << " distributed -> " << node_b->name()->string_view() << " "
- << node_b_factory->FromDistributedClock(a0) << " should be "
- << aos::monotonic_clock::time_point(
- std::chrono::nanoseconds(static_cast<int64_t>(
- std::get<0>(a_timestamps[0]).time_since_epoch().count() *
- (1.0 + estimator->fit().slope()))) +
- estimator->fit().offset())
- << ((a0 <= event_loop_factory_->distributed_now())
- ? ""
- : " After now, investigate");
- VLOG(2) << node_a->name()->string_view()
- << " timestamps()[1] = " << std::get<0>(a_timestamps[1]) << " -> "
- << a1 << " distributed -> " << node_b->name()->string_view() << " "
- << node_b_factory->FromDistributedClock(a1) << " should be "
- << aos::monotonic_clock::time_point(
- std::chrono::nanoseconds(static_cast<int64_t>(
- std::get<0>(a_timestamps[1]).time_since_epoch().count() *
- (1.0 + estimator->fit().slope()))) +
- estimator->fit().offset())
- << ((event_loop_factory_->distributed_now() <= a1)
- ? ""
- : " Before now, investigate");
+ // And then, before changing it, record the list of nodes we've traversed as
+ // now already visited.
+ visited_set = all_nodes;
- const aos::distributed_clock::time_point b0 =
- node_b_factory->ToDistributedClock(std::get<0>(b_timestamps[0]));
- const aos::distributed_clock::time_point b1 =
- node_b_factory->ToDistributedClock(std::get<0>(b_timestamps[1]));
+ for (size_t i = new_nodes.FirstBitSet(0); i < new_nodes.size();
+ i = new_nodes.FirstBitSet(i + 1)) {
+ for (const auto &filter : filters_per_node_[i]) {
+ all_nodes.Set(i, true);
+ all_nodes.Set(filter.b_index, true);
+ }
+ }
- VLOG(2) << node_b->name()->string_view()
- << " timestamps()[0] = " << std::get<0>(b_timestamps[0]) << " -> "
- << b0 << " distributed -> " << node_a->name()->string_view() << " "
- << node_a_factory->FromDistributedClock(b0)
- << ((b0 <= event_loop_factory_->distributed_now())
- ? ""
- : " After now, investigate");
- VLOG(2) << node_b->name()->string_view()
- << " timestamps()[1] = " << std::get<0>(b_timestamps[1]) << " -> "
- << b1 << " distributed -> " << node_a->name()->string_view() << " "
- << node_a_factory->FromDistributedClock(b1)
- << ((event_loop_factory_->distributed_now() <= b1)
- ? ""
- : " Before now, investigate");
+ if (visited_set == all_nodes) {
+ // No change, abort.
+ break;
+ }
+ }
+
+ // Now, see if we found them all.
+ const BitSet64 full_set = ~BitSet64(all_nodes.size());
+ if (all_nodes != full_set) {
+ // Nope, print them out and explode.
+ const BitSet64 orphaned_nodes = (~all_nodes) & full_set;
+ for (size_t i = orphaned_nodes.FirstBitSet(0); i < orphaned_nodes.size();
+ i = orphaned_nodes.FirstBitSet(i + 1)) {
+ LOG(ERROR) << "Node " << i << " is orphaned";
+ }
+
+ LOG(FATAL) << "Found orphaned nodes";
}
}
-void MultiNodeNoncausalOffsetEstimator::UpdateOffsets() {
- for (size_t node_index = 0; node_index < nodes_count(); ++node_index) {
- const Node *node = configuration::GetNode(configuration(), node_index);
- VLOG(1)
- << node->name()->string_view() << " before "
- << event_loop_factory_->GetNodeEventLoopFactory(node)->monotonic_now();
- }
- VLOG(1) << "Distributed " << event_loop_factory_->distributed_now();
+TimestampProblem MultiNodeNoncausalOffsetEstimator::MakeProblem() {
+ // Build up the problem for all valid timestamps.
+ TimestampProblem problem(NodesCount());
- std::tie(time_slope_matrix_, time_offset_matrix_) = SolveOffsets();
- Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[",
- "]");
-
- for (size_t node_index = 0; node_index < nodes_count(); ++node_index) {
- const Node *node = configuration::GetNode(configuration(), node_index);
- event_loop_factory_->GetNodeEventLoopFactory(node)->SetDistributedOffset(
- offset(node_index), slope(node_index));
-
- VLOG(1) << "Offset for node " << node_index << " "
- << node->name()->string_view() << " is "
- << aos::distributed_clock::time_point(offset(node_index))
- << " 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();
- }
-
- for (size_t node_index = 0; node_index < nodes_count(); ++node_index) {
- const Node *node = configuration::GetNode(configuration(), node_index);
- VLOG(1)
- << node->name()->string_view() << " after "
- << event_loop_factory_->GetNodeEventLoopFactory(node)->monotonic_now();
- }
-}
-
-void MultiNodeNoncausalOffsetEstimator::Initialize(
- const Configuration *logged_configuration) {
- logged_configuration_ = logged_configuration;
- // We need to now seed our per-node time offsets and get everything set up
- // to run.
- const size_t num_nodes = nodes_count();
-
- // It is easiest to solve for per node offsets with a matrix rather than
- // trying to solve the equations by hand. So let's get after it.
- //
- // Now, build up the map matrix.
- //
- // offset_matrix_ = (map_matrix_ + slope_matrix_) * [ta; tb; tc]
- map_matrix_ = Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>::Zero(
- filters_.size() + 1, num_nodes);
- slope_matrix_ =
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>::Zero(
- filters_.size() + 1, num_nodes);
-
- offset_matrix_ =
- Eigen::Matrix<mpq_class, Eigen::Dynamic, 1>::Zero(filters_.size() + 1);
- valid_matrix_ =
- Eigen::Matrix<bool, Eigen::Dynamic, 1>::Zero(filters_.size() + 1);
- last_valid_matrix_ =
- Eigen::Matrix<bool, Eigen::Dynamic, 1>::Zero(filters_.size() + 1);
-
- time_offset_matrix_ = Eigen::VectorXd::Zero(num_nodes);
- time_slope_matrix_ = Eigen::VectorXd::Zero(num_nodes);
-
- // All times should average out to the distributed clock.
- for (int i = 0; i < map_matrix_.cols(); ++i) {
- // 1/num_nodes.
- map_matrix_(0, i) = mpq_class(1, num_nodes);
- }
- valid_matrix_(0) = true;
+ bool found_start = false;
+ BitSet64 traversed_nodes(problem.size());
+ BitSet64 all_live_nodes(problem.size());
+ const BitSet64 all_nodes = ~BitSet64(problem.size());
{
- // Now, add the a - b -> sample elements.
- size_t i = 1;
- for (std::pair<const std::tuple<const Node *, const Node *>,
- message_bridge::NoncausalOffsetEstimator> &filter :
- filters_) {
- const Node *const node_a = std::get<0>(filter.first);
- const Node *const node_b = std::get<1>(filter.first);
-
- const size_t node_a_index =
- configuration::GetNodeIndex(configuration(), node_a);
- const size_t node_b_index =
- configuration::GetNodeIndex(configuration(), node_b);
-
- // -a
- map_matrix_(i, node_a_index) = mpq_class(-1);
- // +b
- map_matrix_(i, node_b_index) = mpq_class(1);
-
- // -> sample
- filter.second.set_slope_pointer(&slope_matrix_(i, node_a_index));
- filter.second.set_offset_pointer(&offset_matrix_(i, 0));
-
- valid_matrix_(i) = false;
- filter.second.set_valid_pointer(&valid_matrix_(i));
-
- ++i;
+ size_t node_a_index = 0;
+ for (const auto &filters : filters_per_node_) {
+ for (const auto &filter : filters) {
+ if (filter.filter->timestamps_size() > 0u) {
+ if (!found_start) {
+ // Pick an initial seed.
+ traversed_nodes.Set(node_a_index, true);
+ traversed_nodes.Set(filter.b_index, true);
+ found_start = true;
+ }
+ all_live_nodes.Set(node_a_index, true);
+ all_live_nodes.Set(filter.b_index, true);
+ problem.add_filter(node_a_index, filter.filter, filter.b_index);
+ }
+ }
+ ++node_a_index;
}
}
- const size_t connected_nodes = ConnectedNodes();
+ // Djikstra's algorithm with bit sets :)
+ // The visited set.
+ BitSet64 visited_set(problem.size());
+ while (true) {
+ // Compute the set of all nodes which are new and haven't been visited to
+ // track down dependencies.
+ BitSet64 new_nodes = traversed_nodes & ~visited_set;
- // We don't need to support isolated nodes until someone has a real use
- // case.
- CHECK_EQ(connected_nodes, num_nodes)
- << ": There is a node which isn't communicating with the rest.";
+ // And then, before changing it, record the list of nodes we've traversed as
+ // now already visited.
+ visited_set = traversed_nodes;
+
+ size_t count = 0;
+ for (size_t i = new_nodes.FirstBitSet(0); i < new_nodes.size();
+ i = new_nodes.FirstBitSet(i + 1)) {
+ for (const auto &filter : filters_per_node_[i]) {
+ if (filter.filter->timestamps_size() > 0u) {
+ traversed_nodes.Set(i, true);
+ traversed_nodes.Set(filter.b_index, true);
+ }
+ }
+ ++count;
+ }
+
+ if (count == 0) {
+ // Finished walking the graph, see how we did.
+ break;
+ }
+ }
+
+ CHECK(traversed_nodes == all_live_nodes)
+ << ": Found a subset of the graph which is disconnected. This isn't "
+ "solvable today, but could be with valid use case.";
+
+ // Set any dead nodes to invalid.
+ if (all_nodes != all_live_nodes) {
+ const BitSet64 dead_nodes = all_nodes & (~traversed_nodes);
+ for (size_t i = dead_nodes.FirstBitSet(0); i < dead_nodes.size();
+ i = dead_nodes.FirstBitSet(i + 1)) {
+ problem.set_live(i, false);
+ VLOG(1) << "Node " << i << " is dead";
+ }
+ if (VLOG_IS_ON(2)) {
+ problem.Debug();
+ }
+ }
+
+ return problem;
}
-std::tuple<Eigen::Matrix<double, Eigen::Dynamic, 1>,
- Eigen::Matrix<double, Eigen::Dynamic, 1>>
-MultiNodeNoncausalOffsetEstimator::SolveOffsets() {
- // TODO(austin): Split this out and unit tests a bit better. When we do
- // partial node subsets and also try to optimize this again would be a good
- // time.
- //
- // TODO(austin): CHECK that the number doesn't change over time. We can freak
- // out if that happens.
+std::tuple<NoncausalTimestampFilter *,
+ std::vector<aos::monotonic_clock::time_point>>
+MultiNodeNoncausalOffsetEstimator::NextSolution(
+ TimestampProblem *problem,
+ const std::vector<aos::monotonic_clock::time_point> &base_times) {
+ // Ok, now solve for the minimum time on each channel.
+ std::vector<aos::monotonic_clock::time_point> times;
+ NoncausalTimestampFilter *next_filter = nullptr;
+ {
+ size_t node_a_index = 0;
+ for (const auto &filters : filters_per_node_) {
+ monotonic_clock::time_point next_node_time = monotonic_clock::max_time;
+ NoncausalTimestampFilter *next_node_filter = nullptr;
+ // Find the oldest time for each node in each filter, and solve for that
+ // time. That gives us the next timestamp for this node.
+ for (const auto &filter : filters) {
+ std::optional<
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
+ candidate = filter.filter->Observe();
- // Start by counting how many node pairs we have an offset estimated for.
- int nonzero_offset_count = 1;
- for (int i = 1; i < valid_matrix_.rows(); ++i) {
- if (valid_matrix_(i) != 0) {
- ++nonzero_offset_count;
+ if (candidate) {
+ if (std::get<0>(*candidate) < next_node_time) {
+ next_node_time = std::get<0>(*candidate);
+ next_node_filter = filter.filter;
+ }
+ }
+ }
+
+ // Found no active filters. Either this node is off, or disconnected, or
+ // we are before the log file starts or after the log file ends.
+ if (next_node_time == monotonic_clock::max_time) {
+ ++node_a_index;
+ continue;
+ }
+
+ // Optimize, and save the time into times if earlier than time.
+ for (size_t node_index = 0; node_index < base_times.size();
+ ++node_index) {
+ // Offset everything based on the elapsed time since the last solution
+ // on the node we are solving for. The rate that time elapses should be
+ // ~1.
+ problem->set_base_clock(
+ node_index,
+ base_times[node_a_index] +
+ (next_node_time - base_times[problem->solution_node()]));
+ }
+
+ problem->set_solution_node(node_a_index);
+ problem->set_base_clock(problem->solution_node(), next_node_time);
+ if (VLOG_IS_ON(1)) {
+ problem->Debug();
+ }
+ // TODO(austin): Can we cache? Solving is expensive.
+ std::vector<monotonic_clock::time_point> solution = problem->Solve();
+
+ if (times.empty()) {
+ times = std::move(solution);
+ next_filter = next_node_filter;
+ ++node_a_index;
+ continue;
+ }
+
+ switch (CompareTimes(times, solution, false)) {
+ // The old solution is before or at the new solution. This means that
+ // the old solution is a better result, so ignore this one.
+ case TimeComparison::kBefore:
+ case TimeComparison::kEq:
+ break;
+ case TimeComparison::kAfter:
+ // The new solution is better! Save it.
+ times = std::move(solution);
+ next_filter = next_node_filter;
+ break;
+ case TimeComparison::kInvalid:
+ // Somehow the new solution is better *and* worse than the old
+ // solution... This is an internal failure because that means time
+ // goes backwards on a node.
+ CHECK_EQ(times.size(), solution.size());
+ LOG(INFO) << "Times can't be compared.";
+ for (size_t i = 0; i < times.size(); ++i) {
+ LOG(INFO) << " " << times[i] << " vs " << solution[i] << " -> "
+ << (times[i] - solution[i]).count() << "ns";
+ }
+ LOG(FATAL) << "Please investigate.";
+ break;
+ }
+ ++node_a_index;
}
}
+ return std::make_tuple(next_filter, std::move(times));
+}
- Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[",
- "]");
+std::optional<std::tuple<distributed_clock::time_point,
+ std::vector<monotonic_clock::time_point>>>
+MultiNodeNoncausalOffsetEstimator::NextTimestamp() {
+ // TODO(austin): Detect and handle there being fewer nodes in the log file
+ // than in replay, or them being in a different order.
+ TimestampProblem problem = MakeProblem();
- // If there are missing rows, we can't solve the original problem and instead
- // need to filter the matrix to remove the missing rows and solve a simplified
- // problem. What this means practically is that we might have pairs of nodes
- // which are communicating, but we don't have timestamps between. But we can
- // have multiple paths in our graph between 2 nodes, so we can still solve
- // time without the missing timestamp.
- //
- // In the following example, we can drop any of the last 3 rows, and still
- // solve.
- //
- // [1/3 1/3 1/3 ] [ta] [t_distributed]
- // [ 1 -1-m1 0 ] [tb] = [oab]
- // [ 1 0 -1-m2 ] [tc] [oac]
- // [ 0 1 -1-m2 ] [obc]
- if (nonzero_offset_count != offset_matrix_.rows()) {
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> mpq_map =
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>::Zero(
- nonzero_offset_count, map_matrix_.cols());
- Eigen::Matrix<mpq_class, Eigen::Dynamic, 1> mpq_offsets =
- Eigen::Matrix<mpq_class, Eigen::Dynamic, 1>::Zero(nonzero_offset_count);
+ // Ok, now solve for the minimum time on each channel.
+ std::vector<aos::monotonic_clock::time_point> times;
+ NoncausalTimestampFilter *next_filter = nullptr;
+ std::tie(next_filter, times) = NextSolution(&problem, last_monotonics_);
- std::vector<bool> valid_nodes(nodes_count(), false);
+ CHECK(!all_done_);
- size_t destination_row = 0;
- for (int j = 0; j < map_matrix_.cols(); ++j) {
- mpq_map(0, j) = mpq_class(1, map_matrix_.cols());
- }
- mpq_offsets(0) = mpq_class(0);
- ++destination_row;
-
- for (int i = 1; i < offset_matrix_.rows(); ++i) {
- // Copy over the first row, i.e. the row which says that all times average
- // to the distributed time. And then copy over all valid rows.
- if (valid_matrix_(i)) {
- mpq_offsets(destination_row) = mpq_class(offset_matrix_(i));
-
- for (int j = 0; j < map_matrix_.cols(); ++j) {
- mpq_map(destination_row, j) = map_matrix_(i, j) + slope_matrix_(i, j);
- if (mpq_map(destination_row, j) != 0) {
- valid_nodes[j] = true;
- }
+ // All done.
+ if (next_filter == nullptr) {
+ if (first_solution_) {
+ // If this is our first time, there is no solution. Instead of giving up
+ // completely, (and providing no estimate of time at all), just say that
+ // everything is on the distributed clock. This will then get used as a
+ // 1:1 mapping.
+ first_solution_ = false;
+ if (fp_) {
+ fprintf(fp_, "0.000000000");
+ for (size_t i = 0; i < NodesCount(); ++i) {
+ fprintf(fp_, ", 0.000000000");
}
-
- ++destination_row;
+ fprintf(fp_, "\n");
+ }
+ return std::make_tuple(distributed_clock::epoch(),
+ std::vector<monotonic_clock::time_point>(
+ NodesCount(), monotonic_clock::epoch()));
+ }
+ if (VLOG_IS_ON(1)) {
+ LOG(INFO) << "Found no more timestamps.";
+ for (const auto &filters : filters_per_node_) {
+ for (const auto &filter : filters) {
+ filter.filter->Debug();
+ }
}
}
+ all_done_ = true;
- VLOG(1) << "Filtered map " << ToDouble(mpq_map).format(HeavyFmt);
- VLOG(1) << "Filtered offsets " << ToDouble(mpq_offsets).format(HeavyFmt);
-
- // Compute (and cache) the current connectivity. If we have N nodes
- // configured, but logs only from one of them, we want to assume that the
- // rest of the nodes match the distributed clock exactly.
- //
- // If data shows up later for them, we will CHECK when time jumps.
- //
- // TODO(austin): Once we have more info on what cases are reasonable, we can
- // open up the restrictions.
- if (valid_matrix_ != last_valid_matrix_) {
- Eigen::FullPivLU<Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>>
- full_piv(mpq_map);
- const size_t connected_nodes = full_piv.rank();
-
- size_t valid_node_count = 0;
- for (size_t i = 0; i < valid_nodes.size(); ++i) {
- const bool valid_node = valid_nodes[i];
- if (valid_node) {
- ++valid_node_count;
- } else {
- LOG(WARNING)
- << "Node "
- << logged_configuration()->nodes()->Get(i)->name()->string_view()
- << " has no observations, setting to distributed clock.";
- }
- }
-
- // Confirm that the set of nodes we have connected matches the rank.
- // Otherwise a<->b and c<->d would count as 4 but really is 3.
- CHECK_EQ(std::max(static_cast<size_t>(1u), valid_node_count),
- connected_nodes)
- << ": Ambiguous nodes.";
-
- last_valid_matrix_ = valid_matrix_;
- cached_valid_node_count_ = valid_node_count;
+ // TODO(austin): Instead of giving up forever, give up as far as we can look
+ // into the future. This would let nodes start up unknown and converge to
+ // something useful when they connect.
+ if (fp_) {
+ fflush(fp_);
}
+ return std::nullopt;
+ }
- // There are 2 cases. Either all the nodes are connected with each other by
- // actual data, or we have isolated nodes. We want to force the isolated
- // nodes to match the distributed clock exactly, and to solve for the other
- // nodes.
- if (cached_valid_node_count_ == 0) {
- // Cheat. If there are no valid nodes, the slopes are 1, and offset is 0,
- // ie, just be the distributed clock.
- return std::make_tuple(
- Eigen::Matrix<double, Eigen::Dynamic, 1>::Ones(nodes_count()),
- Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(nodes_count()));
- } else if (cached_valid_node_count_ == nodes_count()) {
- return Solve(mpq_map, mpq_offsets);
- } else {
- // Strip out any columns (nodes) which aren't relevant. Solve the
- // simplified problem, then set any nodes which were missing back to slope
- // 1, offset 0 (ie the distributed clock).
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>
- valid_node_mpq_map =
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>::Zero(
- nonzero_offset_count, cached_valid_node_count_);
+ if (first_solution_) {
+ std::vector<aos::monotonic_clock::time_point> resolved_times;
+ NoncausalTimestampFilter *resolved_next_filter = nullptr;
- {
- // Only copy over the columns with valid nodes in them.
- size_t column = 0;
- for (size_t i = 0; i < valid_nodes.size(); ++i) {
- if (valid_nodes[i]) {
- valid_node_mpq_map.col(column) = mpq_map.col(i);
+ std::tie(resolved_next_filter, resolved_times) =
+ NextSolution(&problem, times);
- ++column;
- }
- }
- // The 1/n needs to be based on the number of nodes being solved.
- // Recreate it here.
- for (int j = 0; j < valid_node_mpq_map.cols(); ++j) {
- valid_node_mpq_map(0, j) = mpq_class(1, cached_valid_node_count_);
- }
+ first_solution_ = false;
+ next_filter = resolved_next_filter;
+
+ // Force any unknown nodes to track the distributed clock (which starts at 0
+ // too).
+ for (monotonic_clock::time_point &time : times) {
+ if (time == monotonic_clock::min_time) {
+ time = monotonic_clock::epoch();
}
-
- VLOG(1) << "Reduced node filtered map "
- << ToDouble(valid_node_mpq_map).format(HeavyFmt);
- VLOG(1) << "Reduced node filtered offsets "
- << ToDouble(mpq_offsets).format(HeavyFmt);
-
- // Solve the simplified problem now.
- std::tuple<Eigen::Matrix<double, Eigen::Dynamic, 1>,
- Eigen::Matrix<double, Eigen::Dynamic, 1>>
- valid_result = Solve(valid_node_mpq_map, mpq_offsets);
-
- // And expand the results back into a solution matrix.
- std::tuple<Eigen::Matrix<double, Eigen::Dynamic, 1>,
- Eigen::Matrix<double, Eigen::Dynamic, 1>>
- result = std::make_tuple(
- Eigen::Matrix<double, Eigen::Dynamic, 1>::Ones(nodes_count()),
- Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(nodes_count()));
-
- {
- size_t column = 0;
- for (size_t i = 0; i < valid_nodes.size(); ++i) {
- if (valid_nodes[i]) {
- std::get<0>(result)(i) = std::get<0>(valid_result)(column);
- std::get<1>(result)(i) = std::get<1>(valid_result)(column);
-
- ++column;
- }
- }
- }
-
- return result;
}
+ times = std::move(resolved_times);
+ next_filter->Consume();
} else {
- const Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> mpq_map =
- map_matrix_ + slope_matrix_;
- VLOG(2) << "map " << ToDouble(map_matrix_ + slope_matrix_).format(HeavyFmt);
- VLOG(2) << "offsets " << ToDouble(offset_matrix_).format(HeavyFmt);
+ next_filter->Consume();
+ // We found a good sample, so consume it. If it is a duplicate, we still
+ // want to consume it. But, if this is the first time around, we want to
+ // re-solve by recursing (once) to pickup the better base.
- return Solve(mpq_map, offset_matrix_);
+ TimeComparison compare = CompareTimes(last_monotonics_, times, true);
+ switch (compare) {
+ case TimeComparison::kBefore:
+ break;
+ case TimeComparison::kAfter:
+ LOG(FATAL) << "Found a solution before the last returned solution.";
+ break;
+ case TimeComparison::kEq:
+ return NextTimestamp();
+ case TimeComparison::kInvalid:
+ CHECK_EQ(last_monotonics_.size(), times.size());
+ for (size_t i = 0; i < times.size(); ++i) {
+ LOG(INFO) << " " << last_monotonics_[i] << " vs " << times[i];
+ }
+ LOG(FATAL) << "Found solutions which can't be ordered.";
+ break;
+ }
}
-}
-size_t MultiNodeNoncausalOffsetEstimator::ConnectedNodes() {
- // Rank of the map matrix tells you if all the nodes are in communication
- // with each other, which tells you if the offsets are observable.
- return Eigen::FullPivLU<
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic>>(
- map_matrix_)
- .rank();
+ // Now, figure out what distributed should be. It should move at the rate of
+ // the max elapsed time so that conversions to and from it don't round to bad
+ // values.
+ const chrono::nanoseconds dt = MaxElapsedTime(last_monotonics_, times);
+ last_distributed_ += dt;
+ for (size_t i = 0; i < times.size(); ++i) {
+ if (times[i] == monotonic_clock::min_time) {
+ // Found an unknown node. Move its time along by the amount the
+ // distributed clock moved.
+ times[i] = last_monotonics_[i] + dt;
+ }
+ }
+ last_monotonics_ = std::move(times);
+
+ // And freeze everything.
+ {
+ size_t node_index = 0;
+ for (const auto &filters : filters_per_node_) {
+ for (const auto &filter : filters) {
+ filter.filter->FreezeUntil(last_monotonics_[node_index]);
+ filter.filter->FreezeUntilRemote(last_monotonics_[filter.b_index]);
+ }
+ ++node_index;
+ }
+ }
+
+ if (fp_) {
+ fprintf(
+ fp_, "%.9f",
+ chrono::duration<double>(last_distributed_.time_since_epoch()).count());
+ for (const monotonic_clock::time_point t : last_monotonics_) {
+ fprintf(fp_, ", %.9f",
+ chrono::duration<double>(t.time_since_epoch()).count());
+ }
+ fprintf(fp_, "\n");
+ }
+
+ return std::make_tuple(last_distributed_, last_monotonics_);
}
} // namespace message_bridge
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index b4f20ff..5e4c690 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -10,7 +10,6 @@
#include "aos/events/simulated_event_loop.h"
#include "aos/network/timestamp_filter.h"
#include "aos/time/time.h"
-#include "third_party/gmp/gmpxx.h"
namespace aos {
namespace message_bridge {
@@ -21,8 +20,8 @@
//
// The problem is defined to be the squared error between the offset computed
// using packets from one node to another, and the corresponding difference in
-// time the pair of node. This handles connections with data in 1 direction and
-// connections with data in both.
+// time between the pair of nodes. This handles connections with data in 1
+// direction and connections with data in both.
//
// All solved times are relative to the times in base_clock, and all math is
// done such that large values of base_clock don't contribute to numerical
@@ -34,6 +33,8 @@
public:
TimestampProblem(size_t count);
+ size_t size() const { return base_clock_.size(); }
+
// Sets node to fix time for and not solve for.
void set_solution_node(size_t solution_node) {
solution_node_ = solution_node;
@@ -63,17 +64,21 @@
std::vector<monotonic_clock::time_point> Solve();
// Returns the squared error for all of the offsets.
- // x is the offsets from the base_clock for every node (in order) except the
- // solution node. It should be one element shorter than the number of nodes
- // this problem was constructed with.
- // grad (if non-nullptr) is the place to put the current gradient and needs to
- // be the same length as x.
- double Cost(const double *x, double *grad);
+ // time_offsets is the offsets from the base_clock for every node (in order)
+ // except the solution node. It should be one element shorter than the number
+ // of nodes this problem was constructed with. grad (if non-nullptr) is the
+ // place to put the current gradient and needs to be the same length as
+ // time_offsets.
+ double Cost(const double *time_offsets, double *grad);
// Returns the time offset from base for a node.
- double get_t(const double *x, size_t time_index) const {
- return time_index == solution_node_ ? 0.0
- : x[NodeToSolutionIndex(time_index)];
+ double get_t(const double *time_offsets, size_t node_index) const {
+ if (node_index == solution_node_) return 0.0;
+ size_t mapped_index = NodeToSolutionIndex(node_index);
+ if (mapped_index == std::numeric_limits<size_t>::max()) {
+ return 0.0;
+ }
+ return time_offsets[mapped_index];
}
// Converts a list of solutions to the corresponding monotonic times for all
@@ -84,34 +89,76 @@
// LOGs a representation of the problem.
void Debug();
+ // A live node is a node which has valid observations and participates in the
+ // optimization problem. Any nodes marked non-live won't be solved for.
+ bool live(size_t index) const { return live_[index]; }
+ void set_live(size_t index, bool live) {
+ node_mapping_valid_ = false;
+ live_[index] = live;
+ }
+
+ // Returns the number of live nodes.
+ size_t LiveNodesCount() const {
+ size_t count = 0;
+ for (bool live : live_) {
+ if (live) ++count;
+ }
+ return count;
+ }
+
private:
- // Static trampoline for nlopt. n is the number of constraints, x is input
- // solution to solve for, grad is the gradient to fill out (if not nullptr),
- // and data is an untyped pointer to a TimestampProblem.
- static double DoCost(unsigned n, const double *x, double *grad, void *data) {
+ // Static trampoline for nlopt. n is the number of constraints, time_offsets
+ // is input solution to solve for, grad is the gradient to fill out (if not
+ // nullptr), and data is an untyped pointer to a TimestampProblem.
+ static double DoCost(unsigned n, const double *time_offsets, double *grad,
+ void *data) {
CHECK_EQ(n + 1u,
reinterpret_cast<TimestampProblem *>(data)->filters_.size());
- return reinterpret_cast<TimestampProblem *>(data)->Cost(x, grad);
+ return reinterpret_cast<TimestampProblem *>(data)->Cost(time_offsets, grad);
+ }
+
+ void MaybeUpdateNodeMapping() {
+ if (node_mapping_valid_) {
+ return;
+ }
+ size_t live_node_index = 0;
+ for (size_t i = 0; i < node_mapping_.size(); ++i) {
+ if (live(i)) {
+ node_mapping_[i] = live_node_index;
+ ++live_node_index;
+ } else {
+ node_mapping_[i] = std::numeric_limits<size_t>::max();
+ }
+ }
+ node_mapping_valid_ = true;
}
// Converts from a node index to an index in the solution.
size_t NodeToSolutionIndex(size_t node_index) const {
+ CHECK(node_mapping_valid_);
+ CHECK_NE(node_index, solution_node_);
// The solver is going to provide us a matrix with solution_node_ removed.
// The indices of all nodes before solution_node_ are in the same spot, and
// the indices of the nodes after solution node are shifted over.
- return node_index < solution_node_ ? node_index : (node_index - 1);
+ size_t mapped_node_index = node_mapping_[node_index];
+ return node_index < solution_node_ ? mapped_node_index
+ : (mapped_node_index - 1);
}
// Number of times Cost has been called for tracking.
- int count_ = 0;
+ int cost_call_count_ = 0;
// The node to hold fixed when solving.
size_t solution_node_ = 0;
- // The optimization problem is solved as base_clock + x to minimize numerical
- // precision problems. This contains all the base times. The base time
- // corresponding to solution_node is fixed and not solved.
+ // The optimization problem is solved as base_clock + time_offsets to minimize
+ // numerical precision problems. This contains all the base times. The base
+ // time corresponding to solution_node is fixed and not solved.
std::vector<monotonic_clock::time_point> base_clock_;
+ std::vector<bool> live_;
+
+ bool node_mapping_valid_ = false;
+ std::vector<size_t> node_mapping_;
// Filter and the node index it is referencing.
// filter->Offset(ta) + ta => t_(b_node);
@@ -181,48 +228,70 @@
bool at_end_ = false;
};
+enum class TimeComparison { kBefore, kAfter, kInvalid, kEq };
+
+// Compares two times.
+TimeComparison CompareTimes(const std::vector<monotonic_clock::time_point> &ta,
+ const std::vector<monotonic_clock::time_point> &tb);
+
+// Returns the maximum amount of elapsed time between the two samples in time.
+std::chrono::nanoseconds MaxElapsedTime(
+ const std::vector<monotonic_clock::time_point> &ta,
+ const std::vector<monotonic_clock::time_point> &tb);
+
// Class to hold a NoncausalOffsetEstimator per pair of communicating nodes, and
// to estimate and set the overall time of all nodes.
-class MultiNodeNoncausalOffsetEstimator {
+//
+// The basic idea is that we support freezing and exploding on invalid time
+// changes, but don't support unwinding time to recompute those. This only
+// works if timestamps are queued far enough in the future that we can estimate
+// time far enough in the future that the current time estimate doesn't need to
+// change. The --time_estimation_buffer_seconds flag lets the user configure
+// how far ahead to look, and the noncausal filters are pretty good about
+// generating a couple of points a second so there is no need to adjust time.
+//
+// We then use lazy evaluation from the SimulatedEventLoop using the
+// TimeConverter interface to trigger the oldest time to be found and added.
+// This lets us evaluate time as late as possible.
+//
+// Each node has a list of filters, and each of those filters has an oldest
+// unprocessed point on its polyline. So, for each node, we can solve the time
+// estimation problem for the oldest timestamp, find the oldest solution of all
+// the nodes, add that to the InterpolatedTimeConverter polyline, and consume
+// the point.
+//
+// TODO(austin): The event scheduler is going to ask for the time on each node
+// for sorting purposes. A node may be arbitrarily in the future, but we only
+// need to compare, not return an accurate result. If this breaks something,
+// fix it.
+class MultiNodeNoncausalOffsetEstimator final
+ : public InterpolatedTimeConverter {
public:
MultiNodeNoncausalOffsetEstimator(
- SimulatedEventLoopFactory *event_loop_factory)
- : event_loop_factory_(event_loop_factory) {}
+ SimulatedEventLoopFactory *event_loop_factory,
+ const Configuration *logged_configuration, bool skip_order_validation);
- // Constructs all the matricies. Needs to be called after the log files have
- // been opened and all the filters have been created.
- void Initialize(const Configuration *logged_configuration);
+ ~MultiNodeNoncausalOffsetEstimator() override;
+
+ std::optional<std::tuple<distributed_clock::time_point,
+ std::vector<monotonic_clock::time_point>>>
+ NextTimestamp() override;
+
+ // Checks that all the nodes in the graph are connected. Needs all filters to
+ // be constructed first.
+ void CheckGraph();
// Returns the filter for a pair of nodes. The same filter will be returned
// for a pair of nodes, regardless of argument order.
message_bridge::NoncausalOffsetEstimator *GetFilter(const Node *node_a,
const Node *node_b);
- // Prints out debug information about the line segments for each node.
- void LogFit(std::string_view prefix);
-
// Captures the start time.
void Start(SimulatedEventLoopFactory *factory);
// Returns the number of nodes.
- size_t nodes_count() const {
- return !configuration::MultiNode(logged_configuration())
- ? 1u
- : logged_configuration()->nodes()->size();
- }
-
- // Returns the offset from the monotonic clock for a node to the distributed
- // clock. monotonic = distributed * slope() + offset();
- double slope(int node_index) const {
- CHECK_LT(node_index, time_slope_matrix_.rows())
- << ": Got too high of a node index.";
- return time_slope_matrix_(node_index);
- }
- std::chrono::nanoseconds offset(int node_index) const {
- CHECK_LT(node_index, time_offset_matrix_.rows())
- << ": Got too high of a node index.";
- return std::chrono::duration_cast<std::chrono::nanoseconds>(
- std::chrono::duration<double>(time_offset_matrix_(node_index)));
+ size_t NodesCount() const {
+ return configuration::NodesCount(logged_configuration());
}
// Returns the configuration that was logged.
@@ -235,78 +304,43 @@
return event_loop_factory_->configuration();
}
- // Returns the number of nodes connected to each other.
- size_t ConnectedNodes();
-
- // Recomputes the offsets and sets them on event_loop_factory_.
- void UpdateOffsets();
-
private:
- // Returns [ta; tb; ...] = tuple[0] * t + tuple[1]
- std::tuple<Eigen::Matrix<double, Eigen::Dynamic, 1>,
- Eigen::Matrix<double, Eigen::Dynamic, 1>>
- SolveOffsets();
+ TimestampProblem MakeProblem();
+
+ std::tuple<NoncausalTimestampFilter *,
+ std::vector<aos::monotonic_clock::time_point>>
+ NextSolution(TimestampProblem *problem,
+ const std::vector<aos::monotonic_clock::time_point> &base_times);
SimulatedEventLoopFactory *event_loop_factory_;
const Configuration *logged_configuration_;
+ // If true, skip any validation which would trigger if we see evidance that
+ // time estimation between nodes was incorrect.
+ const bool skip_order_validation_;
+
// List of filters for a connection. The pointer to the first node will be
// less than the second node.
std::map<std::tuple<const Node *, const Node *>, NoncausalOffsetEstimator>
filters_;
- // We have 2 types of equations to do a least squares regression over to fully
- // constrain our time function.
- //
- // One is simple. The distributed clock is the average of all the clocks.
- // (ta + tb + tc + td) / num_nodes = t_distributed
- //
- // The second is a bit more complicated. Our basic time conversion function
- // is:
- // tb = ta + (ta * slope + offset)
- // We can rewrite this as follows
- // tb - (1 + slope) * ta = offset
- //
- // From here, we have enough equations to solve for t{a,b,c,...} We want to
- // take as an input the offsets and slope, and solve for the per-node times as
- // a function of the distributed clock.
- //
- // We need to massage our equations to make this work. If we solve for the
- // per-node times at two set distributed clock times, we will be able to
- // recreate the linear function (we know it is linear). We can do a similar
- // thing by breaking our equation up into:
- //
- // [1/3 1/3 1/3 ] [ta] [t_distributed]
- // [ 1 -1-m1 0 ] [tb] = [oab]
- // [ 1 0 -1-m2 ] [tc] [oac]
- //
- // This solves to:
- //
- // [ta] [ a00 a01 a02] [t_distributed]
- // [tb] = [ a10 a11 a12] * [oab]
- // [tc] [ a20 a21 a22] [oac]
- //
- // and can be split into:
- //
- // [ta] [ a00 ] [a01 a02]
- // [tb] = [ a10 ] * t_distributed + [a11 a12] * [oab]
- // [tc] [ a20 ] [a21 a22] [oac]
- //
- // (map_matrix_ + slope_matrix_) * [ta; tb; tc] = [offset_matrix_];
- // offset_matrix_ will be in nanoseconds.
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> map_matrix_;
- Eigen::Matrix<mpq_class, Eigen::Dynamic, Eigen::Dynamic> slope_matrix_;
- Eigen::Matrix<mpq_class, Eigen::Dynamic, 1> offset_matrix_;
- // Matrix tracking which offsets are valid.
- Eigen::Matrix<bool, Eigen::Dynamic, 1> valid_matrix_;
- // Matrix tracking the last valid matrix we used to determine connected nodes.
- Eigen::Matrix<bool, Eigen::Dynamic, 1> last_valid_matrix_;
- size_t cached_valid_node_count_ = 0;
+ // Filter and the node index it is referencing.
+ // filter->Offset(ta) + ta => t_(b_node);
+ struct FilterPair {
+ FilterPair(NoncausalTimestampFilter *my_filter, size_t my_b_index)
+ : filter(my_filter), b_index(my_b_index) {}
+ NoncausalTimestampFilter *const filter;
+ const size_t b_index;
+ };
+ std::vector<std::vector<FilterPair>> filters_per_node_;
- // [ta; tb; tc] = time_slope_matrix_ * t + time_offset_matrix;
- // t is in seconds.
- Eigen::Matrix<double, Eigen::Dynamic, 1> time_slope_matrix_;
- Eigen::Matrix<double, Eigen::Dynamic, 1> time_offset_matrix_;
+ distributed_clock::time_point last_distributed_ = distributed_clock::epoch();
+ std::vector<aos::monotonic_clock::time_point> last_monotonics_;
+
+ bool first_solution_ = true;
+ bool all_done_ = false;
+
+ FILE *fp_ = NULL;
};
} // namespace message_bridge
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index c94317a..453180e 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -2,13 +2,14 @@
#include <chrono>
-#include <nlopt.h>
#include "aos/configuration.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/macros.h"
#include "aos/network/multinode_timestamp_filter.h"
#include "aos/network/testing_time_converter.h"
#include "gtest/gtest.h"
+#include "nlopt.h"
+#include "third_party/gmp/gmpxx.h"
namespace aos {
namespace message_bridge {
@@ -17,6 +18,102 @@
namespace chrono = std::chrono;
using aos::monotonic_clock;
+// Converts a int64_t into a mpq_class. This only uses 32 bit precision
+// internally, so it will work on ARM. This should only be used on 64 bit
+// platforms to test out the 32 bit implementation.
+inline mpq_class FromInt64(int64_t i) {
+ uint64_t absi = std::abs(i);
+ mpq_class bits(static_cast<uint32_t>((absi >> 32) & 0xffffffffu));
+ bits *= mpq_class(0x10000);
+ bits *= mpq_class(0x10000);
+ bits += mpq_class(static_cast<uint32_t>(absi & 0xffffffffu));
+
+ if (i < 0) {
+ return -bits;
+ } else {
+ return bits;
+ }
+}
+
+// Class to hold an affine function for the time offset.
+// O(t) = slope * t + offset
+//
+// This is stored using mpq_class, which stores everything as full rational
+// fractions.
+class Line {
+ public:
+ Line() {}
+
+ // Constructs a line given the offset and slope.
+ Line(mpq_class offset, mpq_class slope) : offset_(offset), slope_(slope) {}
+
+ // TODO(austin): Remove this one.
+ Line(std::chrono::nanoseconds offset, double slope)
+ : offset_(DoFromInt64(offset.count())), slope_(slope) {}
+
+ // Fits a line to 2 points and returns the associated line.
+ static Line Fit(
+ const std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> a,
+ const std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>
+ b);
+
+ // Returns the full precision slopes and offsets.
+ mpq_class mpq_offset() const { return offset_; }
+ mpq_class mpq_slope() const { return slope_; }
+ void increment_mpq_offset(mpq_class increment) { offset_ += increment; }
+
+ // Returns the rounded offsets and slopes.
+ std::chrono::nanoseconds offset() const {
+ double o = offset_.get_d();
+ return std::chrono::nanoseconds(static_cast<int64_t>(o));
+ }
+ double slope() const { return slope_.get_d(); }
+
+ std::string DebugString() const {
+ std::stringstream ss;
+ ss << "Offset " << mpq_offset() << " slope " << mpq_slope();
+ return ss.str();
+ }
+
+ void Debug() const {
+ LOG(INFO) << DebugString();
+ }
+
+ // Returns the offset at a given time.
+ // TODO(austin): get_d() ie double -> int64 can't be accurate...
+ std::chrono::nanoseconds Eval(monotonic_clock::time_point pt) const {
+ mpq_class result =
+ mpq_class(FromInt64(pt.time_since_epoch().count())) * slope_ + offset_;
+ return std::chrono::nanoseconds(static_cast<int64_t>(result.get_d()));
+ }
+
+ private:
+ static mpq_class DoFromInt64(int64_t i) {
+#if GMP_NUMB_BITS == 32
+ return FromInt64(i);
+#else
+ return i;
+#endif
+ }
+
+ mpq_class offset_;
+ mpq_class slope_;
+};
+
+Line Line::Fit(
+ const std::tuple<monotonic_clock::time_point, chrono::nanoseconds> a,
+ const std::tuple<monotonic_clock::time_point, chrono::nanoseconds> b) {
+ mpq_class slope = FromInt64((std::get<1>(b) - std::get<1>(a)).count()) /
+ FromInt64((std::get<0>(b) - std::get<0>(a)).count());
+ slope.canonicalize();
+ mpq_class offset =
+ FromInt64(std::get<1>(a).count()) -
+ FromInt64(std::get<0>(a).time_since_epoch().count()) * slope;
+ offset.canonicalize();
+ Line f(offset, slope);
+ return f;
+}
+
mpq_class SolveExact(Line la, Line lb, monotonic_clock::time_point ta) {
mpq_class ma = la.mpq_slope();
mpq_class ba = la.mpq_offset();
@@ -39,7 +136,7 @@
// tb = (((1 + d.ma) + (1 + d.mb)) * ta + d.ba - (1 + d.mb) d.bb) / (1 + (1
// + d.mb) (1 + d.mb))
- mpq_class mpq_ta(message_bridge::FromInt64(ta.time_since_epoch().count()));
+ mpq_class mpq_ta(FromInt64(ta.time_since_epoch().count()));
mpq_class one(1);
mpq_class mpq_tb =
(((one + ma) + (one + mb)) * mpq_ta + ba - (one + mb) * bb) /
@@ -48,6 +145,15 @@
return mpq_tb;
}
+Line FitLine(const NoncausalTimestampFilter &filter) {
+ if (filter.timestamps_size() == 1) {
+ Line fit(std::get<1>(filter.timestamp(0)), 0.0);
+ return fit;
+ } else {
+ return Line::Fit(filter.timestamp(0), filter.timestamp(1));
+ }
+}
+
// Tests that an infinite precision solution matches our numeric solver solution
// for a couple of simple problems.
TEST(TimestampProblemTest, Solve) {
@@ -80,7 +186,7 @@
const std::vector<double> result = problem.SolveDouble();
mpq_class tb_mpq =
- SolveExact(a.FitLine(), b.FitLine(), problem.base_clock(0));
+ SolveExact(FitLine(a), FitLine(b), problem.base_clock(0));
EXPECT_EQ(tb_mpq.get_d(), result[0])
<< std::setprecision(12) << std::fixed << " Expected " << tb_mpq.get_d()
<< " " << tb_mpq << " got " << result[0];
@@ -92,7 +198,7 @@
std::vector<double> result = problem.SolveDouble();
mpq_class tb_mpq =
- SolveExact(a.FitLine(), b.FitLine(), problem.base_clock(0));
+ SolveExact(FitLine(a), FitLine(b), problem.base_clock(0));
EXPECT_EQ(tb_mpq.get_d(), result[0])
<< std::setprecision(12) << std::fixed << " Expected " << tb_mpq.get_d()
@@ -113,7 +219,7 @@
const std::vector<double> result = problem.SolveDouble();
mpq_class tb_mpq =
- SolveExact(a.FitLine(), b.FitLine(), problem.base_clock(0));
+ SolveExact(FitLine(a), FitLine(b), problem.base_clock(0));
EXPECT_NEAR(tb_mpq.get_d(), result[0], 1e-6)
<< std::setprecision(12) << std::fixed << " Expected "
@@ -125,7 +231,7 @@
const std::vector<double> result = problem.SolveDouble();
mpq_class tb_mpq =
- SolveExact(a.FitLine(), b.FitLine(), problem.base_clock(0));
+ SolveExact(FitLine(a), FitLine(b), problem.base_clock(0));
EXPECT_EQ(tb_mpq.get_d(), result[0])
<< std::setprecision(12) << std::fixed << " Expected "
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 8315c71..a6325f1 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -9,7 +9,6 @@
#include "absl/strings/str_format.h"
#include "aos/configuration.h"
#include "aos/time/time.h"
-#include "third_party/gmp/gmpxx.h"
namespace aos {
namespace message_bridge {
@@ -25,14 +24,12 @@
}
void PrintNoncausalTimestampFilterHeader(FILE *fp) {
- fprintf(fp,
- "# time_since_start, sample_ns, filtered_offset, offset, "
- "velocity, filtered_velocity, velocity_contribution, "
- "sample_contribution, time_contribution\n");
+ fprintf(fp, "time_since_start,sample_ns,filtered_offset\n");
}
void PrintNoncausalTimestampFilterSamplesHeader(FILE *fp) {
- fprintf(fp, "# time_since_start, sample_ns, offset\n");
+ fprintf(fp,
+ "time_since_start,sample_ns,monotonic,monotonic+offset(remote)\n");
}
void NormalizeTimestamps(monotonic_clock::time_point *ta_base, double *ta) {
@@ -448,87 +445,6 @@
}
}
-Line Line::Fit(
- const std::tuple<monotonic_clock::time_point, chrono::nanoseconds> a,
- const std::tuple<monotonic_clock::time_point, chrono::nanoseconds> b) {
- mpq_class slope = FromInt64((std::get<1>(b) - std::get<1>(a)).count()) /
- FromInt64((std::get<0>(b) - std::get<0>(a)).count());
- slope.canonicalize();
- mpq_class offset =
- FromInt64(std::get<1>(a).count()) -
- FromInt64(std::get<0>(a).time_since_epoch().count()) * slope;
- offset.canonicalize();
- Line f(offset, slope);
- return f;
-}
-
-Line AverageFits(Line fa, Line fb) {
- // tb = Oa(ta) + ta
- // ta = Ob(tb) + tb
- // tb - ta = Oa(ta)
- // tb - ta = -Ob(tb)
- // Oa(ta) = ma * ta + ba
- // Ob(tb) = mb * tb + bb
- //
- // ta + O(ta, tb) = tb
- // tb - ta = O(ta, tb)
- // O(ta, tb) = (Oa(ta) - Ob(tb)) / 2.0
- // ta + (ma * ta + ba - mb * tb - bb) / 2 = tb
- // (2 + ma) / 2 * ta + (ba - bb) / 2 = (2 + mb) / 2 * tb
- // (2 + ma) * ta + (ba - bb) = (2 + mb) * tb
- // tb = (2 + ma) / (2 + mb) * ta + (ba - bb) / (2 + mb)
- // ta = (2 + mb) / (2 + ma) * tb + (bb - ba) / (2 + ma)
- //
- // ta - tb = (mb - ma) / (2 + ma) * tb + (bb - ba) / (2 + ma)
- // mb = (mb - ma) / (2 + ma)
- // bb = (bb - ba) / (2 + ma)
- //
- // tb - ta = (ma - mb) / (2 + mb) * tb + (ba - bb) / (2 + mb)
- // ma = (ma - mb) / (2 + mb)
- // ba = (ba - bb) / (2 + mb)
- //
- // O(ta) = ma * ta + ba
- // tb = O(ta) + ta
- // ta = O(tb) + tb
-
- mpq_class m =
- (fa.mpq_slope() - fb.mpq_slope()) / (mpq_class(2) + fb.mpq_slope());
- m.canonicalize();
-
- mpq_class b =
- (fa.mpq_offset() - fb.mpq_offset()) / (mpq_class(2) + fb.mpq_slope());
- b.canonicalize();
-
- Line f(b, m);
- return f;
-}
-
-Line Invert(Line fb) {
- // ta = Ob(tb) + tb
- // tb = Oa(ta) + ta
- // Ob(tb) = mb * tb + bb
- // Oa(ta) = ma * ta + ba
- //
- // ta = mb * tb + tb + bb
- // ta = (mb + 1) * tb + bb
- // 1 / (mb + 1) ta - bb / (mb + 1) = tb
- // ta + (-1 + 1 / (mb + 1)) ta - bb / (mb + 1) = tb
- // ta + ((-mb - 1) / (mb + 1) + 1 / (mb + 1)) ta - bb / (mb + 1) = tb
- // ta + -mb / (mb + 1) ta - bb / (mb + 1) = tb
- //
- // ma = -mb / (mb + 1)
- // ba = -bb / (mb + 1)
-
- mpq_class denom = (mpq_class(1) + fb.mpq_slope());
- mpq_class ma = -fb.mpq_slope() / denom;
- ma.canonicalize();
-
- mpq_class ba = -fb.mpq_offset() / denom;
-
- Line f(ba, ma);
- return f;
-}
-
NoncausalTimestampFilter::~NoncausalTimestampFilter() {
// Destroy the filter by popping until empty. This will trigger any
// timestamps to be written to the files.
@@ -563,23 +479,20 @@
return result;
}
-Line NoncausalTimestampFilter::FitLine() {
- DCHECK_GE(timestamps_.size(), 1u);
- if (timestamps_.size() == 1) {
- Line fit(std::get<1>(timestamps_[0]), 0.0);
- return fit;
- } else {
- return Line::Fit(TrimTuple(timestamps_[0]), TrimTuple(timestamps_[1]));
- }
-}
void NoncausalTimestampFilter::FlushSavedSamples() {
for (const std::tuple<aos::monotonic_clock::time_point,
std::chrono::nanoseconds> &sample : saved_samples_) {
- fprintf(samples_fp_, "%.9f, %.9f\n",
+ fprintf(samples_fp_, "%.9f, %.9f, %.9f, %.9f\n",
chrono::duration_cast<chrono::duration<double>>(
std::get<0>(sample) - first_time_)
.count(),
chrono::duration_cast<chrono::duration<double>>(std::get<1>(sample))
+ .count(),
+ chrono::duration_cast<chrono::duration<double>>(
+ std::get<0>(sample).time_since_epoch())
+ .count(),
+ chrono::duration_cast<chrono::duration<double>>(
+ (std::get<0>(sample) + std::get<1>(sample)).time_since_epoch())
.count());
}
saved_samples_.clear();
@@ -804,11 +717,11 @@
std::string NoncausalTimestampFilter::DebugDCostDta(
aos::monotonic_clock::time_point ta_base, double ta,
- aos::monotonic_clock::time_point tb_base, double tb,
- size_t node_a, size_t node_b) const {
+ aos::monotonic_clock::time_point tb_base, double tb, size_t node_a,
+ size_t node_b) const {
if (timestamps_size() == 1u) {
return absl::StrFormat("-2. * (t%d - t%d - %d)", node_b, node_a,
- std::get<1>(timestamp(0)).count());
+ std::get<1>(timestamp(0)).count());
}
NormalizeTimestamps(&ta_base, &ta);
@@ -943,7 +856,7 @@
}
}
-bool NoncausalTimestampFilter::Sample(
+void NoncausalTimestampFilter::Sample(
aos::monotonic_clock::time_point monotonic_now,
chrono::nanoseconds sample_ns) {
if (samples_fp_) {
@@ -953,13 +866,17 @@
}
}
- 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));
- return true;
+ 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_ << ".";
} else {
// Future samples get quite a bit harder. We want the line to track the
// highest point without volating the slope constraint.
@@ -970,7 +887,7 @@
aos::monotonic_clock::duration doffset = sample_ns - std::get<1>(back);
if (dt == chrono::nanoseconds(0) && doffset == chrono::nanoseconds(0)) {
- return false;
+ return;
}
// If the point is higher than the max negative slope, the slope will either
@@ -979,6 +896,22 @@
// 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_ << ".";
+
// Back propagate the max velocity and remove any elements violating the
// velocity constraint.
while (dt * kMaxVelocity() < doffset && timestamps_.size() > 1u) {
@@ -1005,10 +938,8 @@
static_cast<aos::monotonic_clock::duration::rep>(
dt.count() * kMaxVelocity()));
- VLOG(1) << csv_file_name_ << " slope " << std::setprecision(20)
- << FitLine().slope() << " offset " << FitLine().offset().count()
- << " a [(" << std::get<0>(timestamps_[0]) << " -> "
- << std::get<1>(timestamps_[0]).count() << "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)
@@ -1028,11 +959,10 @@
std::get<1>(timestamps_[0]) = adjusted_initial_time;
}
- if (timestamps_.size() == 2) {
- return true;
- }
+ } else {
+ VLOG(1) << "Rejecting sample because " << doffset.count() << " > "
+ << (-dt * kMaxVelocity()).count();
}
- return false;
}
}
@@ -1159,15 +1089,9 @@
VLOG(1) << "Sample delivered " << node_delivered_time << " sent "
<< other_node_sent_time << " to " << node->name()->string_view();
if (node == node_a_) {
- if (a_.Sample(node_delivered_time,
- other_node_sent_time - node_delivered_time)) {
- Refit();
- }
+ a_.Sample(node_delivered_time, other_node_sent_time - node_delivered_time);
} else if (node == node_b_) {
- if (b_.Sample(node_delivered_time,
- other_node_sent_time - node_delivered_time)) {
- Refit();
- }
+ b_.Sample(node_delivered_time, other_node_sent_time - node_delivered_time);
} else {
LOG(FATAL) << "Unknown node " << node->name()->string_view();
}
@@ -1180,7 +1104,6 @@
VLOG(1) << "Popping forward sample to " << node_a_->name()->string_view()
<< " from " << node_b_->name()->string_view() << " at "
<< node_monotonic_now;
- Refit();
return true;
}
} else if (node == node_b_) {
@@ -1188,7 +1111,6 @@
VLOG(1) << "Popping reverse sample to " << node_b_->name()->string_view()
<< " from " << node_a_->name()->string_view() << " at "
<< node_monotonic_now;
- Refit();
return true;
}
} else {
@@ -1197,120 +1119,5 @@
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();
- 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}]";
- } else if (a_timestamps.size() == 1u) {
- 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)";
- } else {
- LOG(INFO) << prefix << " " << node_a_->name()->string_view() << " from "
- << node_b_->name()->string_view() << " slope "
- << std::setprecision(20) << fit_.slope() << " offset "
- << fit_.offset().count() << " no samples.";
- }
- if (b_timestamps.size() >= 2u) {
- LOG(INFO)
- << prefix << " " << node_b_->name()->string_view() << " from "
- << node_a_->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 "
- << std::setprecision(20) << fit_.slope() << " offset "
- << fit_.offset().count() << " b [("
- << std::get<0>(b_timestamps[0]) << " -> "
- << std::get<1>(b_timestamps[0]).count() << "ns)";
- } else {
- LOG(INFO) << prefix << " " << node_b_->name()->string_view() << " from "
- << node_a_->name()->string_view() << " slope "
- << std::setprecision(20) << fit_.slope() << " offset "
- << fit_.offset().count() << " no samples.";
- }
-}
-
-void NoncausalOffsetEstimator::Refit() {
- if (a_timestamps_size() == 0 && b_timestamps_size() == 0) {
- VLOG(1) << "Not fitting because there is no data";
- return;
- }
-
- // If we only have one side of the timestamp estimation, we will be on the
- // ragged edge of non-causal. Events will traverse the network in "0 ns".
- // Combined with rounding errors, this causes sorting to not work. Assume
- // some amount of network delay.
- constexpr int kSmidgeOfTimeNs = 10;
-
- if (a_timestamps_size() == 0) {
- fit_ = Invert(b_.FitLine());
- fit_.increment_mpq_offset(-mpq_class(kSmidgeOfTimeNs));
- } else if (b_timestamps_size() == 0) {
- fit_ = a_.FitLine();
- fit_.increment_mpq_offset(-mpq_class(kSmidgeOfTimeNs));
- } else {
- fit_ = AverageFits(a_.FitLine(), b_.FitLine());
- }
-
- if (offset_pointer_) {
- VLOG(2) << " Setting offset to " << fit_.mpq_offset();
- *offset_pointer_ = fit_.mpq_offset();
- }
- if (slope_pointer_) {
- VLOG(2) << " Setting slope to " << fit_.mpq_slope();
- *slope_pointer_ = -fit_.mpq_slope();
- }
- if (valid_pointer_) {
- *valid_pointer_ = true;
- }
-
- if (VLOG_IS_ON(1)) {
- LogFit("Refitting to");
- }
-}
-
} // namespace message_bridge
} // namespace aos
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 7a2c386..ef86a75 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -10,7 +10,6 @@
#include "aos/configuration.h"
#include "aos/time/time.h"
#include "glog/logging.h"
-#include "third_party/gmp/gmpxx.h"
namespace aos {
namespace message_bridge {
@@ -218,113 +217,6 @@
FILE *rev_fp_ = nullptr;
};
-// Converts a int64_t into a mpq_class. This only uses 32 bit precision
-// internally, so it will work on ARM. This should only be used on 64 bit
-// platforms to test out the 32 bit implementation.
-inline mpq_class FromInt64(int64_t i) {
- uint64_t absi = std::abs(i);
- mpq_class bits(static_cast<uint32_t>((absi >> 32) & 0xffffffffu));
- bits *= mpq_class(0x10000);
- bits *= mpq_class(0x10000);
- bits += mpq_class(static_cast<uint32_t>(absi & 0xffffffffu));
-
- if (i < 0) {
- return -bits;
- } else {
- return bits;
- }
-}
-
-// Class to hold an affine function for the time offset.
-// O(t) = slope * t + offset
-//
-// This is stored using mpq_class, which stores everything as full rational
-// fractions.
-class Line {
- public:
- Line() {}
-
- // Constructs a line given the offset and slope.
- Line(mpq_class offset, mpq_class slope) : offset_(offset), slope_(slope) {}
-
- // TODO(austin): Remove this one.
- Line(std::chrono::nanoseconds offset, double slope)
- : offset_(DoFromInt64(offset.count())), slope_(slope) {}
-
- // Fits a line to 2 points and returns the associated line.
- static Line Fit(
- const std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> a,
- const std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>
- b);
-
- // Returns the full precision slopes and offsets.
- mpq_class mpq_offset() const { return offset_; }
- mpq_class mpq_slope() const { return slope_; }
- void increment_mpq_offset(mpq_class increment) { offset_ += increment; }
-
- // Returns the rounded offsets and slopes.
- std::chrono::nanoseconds offset() const {
- double o = offset_.get_d();
- return std::chrono::nanoseconds(static_cast<int64_t>(o));
- }
- double slope() const { return slope_.get_d(); }
-
- std::string DebugString() const {
- std::stringstream ss;
- ss << "Offset " << mpq_offset() << " slope " << mpq_slope();
- return ss.str();
- }
-
- void Debug() const {
- LOG(INFO) << DebugString();
- }
-
- // Returns the offset at a given time.
- // TODO(austin): get_d() ie double -> int64 can't be accurate...
- std::chrono::nanoseconds Eval(monotonic_clock::time_point pt) const {
- mpq_class result =
- mpq_class(FromInt64(pt.time_since_epoch().count())) * slope_ + offset_;
- return std::chrono::nanoseconds(static_cast<int64_t>(result.get_d()));
- }
-
- private:
- static mpq_class DoFromInt64(int64_t i) {
-#if GMP_NUMB_BITS == 32
- return message_bridge::FromInt64(i);
-#else
- return i;
-#endif
- }
-
- mpq_class offset_;
- mpq_class slope_;
-};
-
-// Averages 2 fits per the equations below
-//
-// Oa(ta) = fa.slope * ta + fa.offset;
-// tb = Oa(ta) + ta;
-// Ob(tb) = fb.slope * tb + fb.offset;
-// ta = Ob(tb) + tb;
-//
-// This splits the difference between Oa and Ob and solves:
-// tb - ta = (Oa(ta) - Ob(tb)) / 2.0
-// and returns O(ta) such that
-// tb = O(ta) + ta
-Line AverageFits(Line fa, Line fb);
-
-// Inverts an offset line
-//
-// Oa(ta) = fa.slope * ta + fa.offset;
-// tb = Oa(ta) + ta;
-// Ob(tb) = fb.slope * tb + fb.offset;
-// ta = Ob(tb) + tb;
-//
-// This takes a line in the form ta = Ob(tb) + tb,
-// and returns one in the form tb = Oa(ta) + ta. This is a pure algebreic
-// reshuffling of terms.
-Line Invert(Line fb);
-
// This class implements a noncausal timestamp filter. It tracks the maximum
// points while enforcing both a maximum positive and negative slope constraint.
// It does this by building up a buffer of samples, and removing any samples
@@ -345,10 +237,6 @@
public:
~NoncausalTimestampFilter();
- // Returns a line fit to the oldest 2 points in the timestamp list if
- // available, or the only point (assuming 0 slope) if not available.
- Line FitLine();
-
// Returns the offset for the point in time, using the timestamps in the deque
// to form a polyline used to interpolate.
std::chrono::nanoseconds Offset(monotonic_clock::time_point ta) const;
@@ -391,12 +279,11 @@
}
// Adds a new sample to our filtered timestamp list.
- // Returns true if adding the sample changed the output from FitLine().
- bool Sample(aos::monotonic_clock::time_point monotonic_now,
+ void Sample(aos::monotonic_clock::time_point monotonic_now,
std::chrono::nanoseconds sample_ns);
// Removes any old timestamps from our timestamps list.
- // Returns true if adding the sample changed the output from FitLine().
+ // Returns true if any points were popped.
bool Pop(aos::monotonic_clock::time_point time);
// Returns the current list of timestamps in our list.
@@ -539,7 +426,7 @@
NoncausalOffsetEstimator(const Node *node_a, const Node *node_b)
: node_a_(node_a), node_b_(node_b) {}
- const NoncausalTimestampFilter *GetFilter(const Node *n) {
+ NoncausalTimestampFilter *GetFilter(const Node *n) {
if (n == node_a_) {
return &a_;
}
@@ -558,41 +445,11 @@
aos::monotonic_clock::time_point other_node_sent_time);
// Removes old data points from a node before the provided time.
- // Returns true if the line fit changes.
+ // Returns true if any points were popped.
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_; }
-
- // Sets the locations to update when the fit changes.
- void set_offset_pointer(mpq_class *offset_pointer) {
- offset_pointer_ = offset_pointer;
- }
- void set_slope_pointer(mpq_class *slope_pointer) {
- slope_pointer_ = slope_pointer;
- }
- void set_valid_pointer(bool *valid_pointer) {
- valid_pointer_ = valid_pointer;
- }
-
// Returns the data points from each filter.
- std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- ATimestamps() {
- return a_.Timestamps();
- }
- std::deque<
- std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- BTimestamps() {
- return b_.Timestamps();
- }
-
size_t a_timestamps_size() const { return a_.timestamps_size(); }
size_t b_timestamps_size() const { return b_.timestamps_size(); }
@@ -605,21 +462,10 @@
}
void SetRevCsvFileName(std::string_view name) { b_.SetCsvFileName(name); }
- // Logs the fits and timestamps for all the filters.
- void LogFit(std::string_view prefix);
-
private:
- void Refit();
-
NoncausalTimestampFilter a_;
NoncausalTimestampFilter b_;
- mpq_class *offset_pointer_ = nullptr;
- mpq_class *slope_pointer_ = nullptr;
- bool *valid_pointer_ = nullptr;
-
- Line fit_{std::chrono::nanoseconds(0), 0.0};
-
const Node *const node_a_;
const Node *const node_b_;
};
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 874f531..a046726 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -76,141 +76,6 @@
EXPECT_EQ(filter.offset(), chrono::microseconds(100500));
}
-// Tests that the FromInt64 function correctly produces a mpq even though it
-// only can use 32 bit numbers.
-TEST(LineTest, Int64) {
- EXPECT_EQ(FromInt64(0x9710000000ll),
- mpq_class(0x971) * mpq_class(0x10000000));
-
- EXPECT_EQ(FromInt64(-0x9710000000ll),
- mpq_class(-0x971) * mpq_class(0x10000000));
-}
-
-// Tests that we can create a simple line and the methods return sane results.
-TEST(LineTest, SimpleLine) {
- mpq_class offset(1023);
- mpq_class slope(1);
- Line l(offset, slope);
-
- EXPECT_EQ(l.mpq_offset(), offset);
- EXPECT_EQ(l.mpq_slope(), slope);
-
- EXPECT_EQ(l.offset(), chrono::nanoseconds(1023));
- EXPECT_EQ(l.slope(), 1.0);
-
- EXPECT_EQ(chrono::nanoseconds(1023 + 100),
- l.Eval(monotonic_clock::time_point(chrono::nanoseconds(100))));
-}
-
-// Tests that we can fit a line to 2 points and they recover correctly.
-TEST(LineTest, FitLine) {
- const monotonic_clock::time_point ta(chrono::nanoseconds(1000));
- const monotonic_clock::time_point tb(chrono::nanoseconds(100001013));
- Line l = Line::Fit(std::make_tuple(ta, chrono::nanoseconds(100)),
- std::make_tuple(tb, chrono::nanoseconds(105)));
-
- EXPECT_EQ(chrono::nanoseconds(100), l.Eval(ta));
- EXPECT_EQ(chrono::nanoseconds(105), l.Eval(tb));
-}
-
-// Tests that averaging 2 lines results in the correct outcome.
-// Try to compute the correct outcome a couple of different ways to confirm the
-// math is done right.
-TEST(LineTest, AverageFits) {
- const monotonic_clock::time_point ta(chrono::nanoseconds(1000));
- const monotonic_clock::time_point tb(chrono::nanoseconds(100001013));
-
- // Test 2 lines which both diverge and should average back to nothing.
- {
- Line l1(mpq_class(999000), mpq_class(1000, 1000));
- Line l2(-mpq_class(990000), mpq_class(1000, 1000));
-
- Line a = AverageFits(l1, l2);
- a.Debug();
-
- EXPECT_EQ(a.mpq_slope(), mpq_class(0));
-
- // Confirm some points to make sure everything works.
- //
- // tb = ta + O(ta)
- // tb = Oa(ta) + ta
- // ta = Ob(tb) + tb
- // tb - ta = O(ta, tb)
- // So, if we pick a point at t=x, we can evaluate both functions and should
- // get back O(x)
-
- const monotonic_clock::time_point ta(chrono::nanoseconds(1000));
- const monotonic_clock::time_point tb(chrono::nanoseconds(100001013));
-
- EXPECT_EQ((l1.Eval(ta) - l2.Eval(a.Eval(ta) + ta)) / 2, a.Eval(ta));
- EXPECT_EQ((l1.Eval(tb) - l2.Eval(a.Eval(tb) + tb)) / 2, a.Eval(tb));
- }
-
- // Test 2 lines which are parallel, so there should be a slope.
- {
- Line l1(mpq_class(990000), mpq_class(1000, 1000));
- Line l2(-mpq_class(990000), -mpq_class(1000, 1000));
-
- Line a = AverageFits(l1, l2);
- a.Debug();
-
- EXPECT_EQ(a.mpq_slope(), mpq_class(2));
-
- // Confirm some points to make sure everything works.
-
- EXPECT_EQ((l1.Eval(ta) - l2.Eval(a.Eval(ta) + ta)) / 2, a.Eval(ta));
- EXPECT_EQ((l1.Eval(tb) - l2.Eval(a.Eval(tb) + tb)) / 2, a.Eval(tb));
- }
-}
-
-// Tests that the Invert function returns sane results.
-TEST(LineTest, Invert) {
- const monotonic_clock::time_point ta(chrono::nanoseconds(1000000000));
- const monotonic_clock::time_point tb(chrono::nanoseconds(2001000000));
-
- // Double inversion should get us back where we started. Make sure there are
- // enough digits to catch rounding problems.
- Line l1(mpq_class(1000000000), mpq_class(1, 1000));
- Line l2 = Invert(l1);
- Line l1_again = Invert(l2);
-
- // Confirm we can convert time back and forth as expected.
- EXPECT_EQ(l1.Eval(ta) + ta, tb);
- EXPECT_EQ(l2.Eval(tb) + tb, ta);
-
- // And we got back our original line.
- EXPECT_EQ(l1.mpq_slope(), l1_again.mpq_slope());
- EXPECT_EQ(l1.mpq_offset(), l1_again.mpq_offset());
-}
-
-// Tests that 2 samples results in the correct line between them, and the
-// correct intermediate as it is being built.
-TEST(NoncausalTimestampFilterTest, SingleSample) {
- const monotonic_clock::time_point ta(chrono::nanoseconds(100000));
- const monotonic_clock::time_point tb(chrono::nanoseconds(200000));
-
- NoncausalTimestampFilter filter;
-
- filter.Sample(ta, chrono::nanoseconds(1000));
- EXPECT_EQ(filter.Timestamps().size(), 1u);
-
- {
- Line l1 = filter.FitLine();
-
- EXPECT_EQ(l1.mpq_offset(), mpq_class(1000));
- EXPECT_EQ(l1.mpq_slope(), mpq_class(0));
- }
-
- filter.Sample(tb, chrono::nanoseconds(1100));
- EXPECT_EQ(filter.Timestamps().size(), 2u);
-
- {
- Line l2 = filter.FitLine();
- EXPECT_EQ(l2.mpq_offset(), mpq_class(900));
- EXPECT_EQ(l2.mpq_slope(), mpq_class(1, 1000));
- }
-}
-
// Tests that 2 samples results in the correct line between them, and the
// correct intermediate as it is being built.
TEST(NoncausalTimestampFilterTest, PeekPop) {
@@ -292,11 +157,10 @@
filter.Debug();
ASSERT_EQ(filter.Timestamps().size(), 2u);
- {
- Line l2 = filter.FitLine();
- EXPECT_EQ(l2.mpq_offset(), mpq_class(1000));
- EXPECT_EQ(l2.mpq_slope(), mpq_class(1, 1000));
- }
+ EXPECT_EQ(filter.timestamp(0),
+ std::make_tuple(ta, chrono::microseconds(1)));
+ EXPECT_EQ(filter.timestamp(1),
+ std::make_tuple(tb, chrono::microseconds(2)));
}
{
@@ -309,11 +173,10 @@
filter.Debug();
ASSERT_EQ(filter.Timestamps().size(), 2u);
- {
- Line l2 = filter.FitLine();
- EXPECT_EQ(l2.mpq_offset(), mpq_class(1000));
- EXPECT_EQ(l2.mpq_slope(), -mpq_class(1, 1000));
- }
+ EXPECT_EQ(filter.timestamp(0),
+ std::make_tuple(ta, chrono::microseconds(1)));
+ EXPECT_EQ(filter.timestamp(1),
+ std::make_tuple(tb, chrono::microseconds(0)));
}
{
@@ -804,42 +667,29 @@
// Add 3 timestamps in and confirm that the slopes come out reasonably.
estimator.Sample(node_a, ta1, tb1);
estimator.Sample(node_b, tb1, ta1);
- EXPECT_EQ(estimator.ATimestamps().size(), 1u);
- EXPECT_EQ(estimator.BTimestamps().size(), 1u);
-
- // 1 point -> a line.
- EXPECT_EQ(estimator.fit().mpq_slope(), mpq_class(0));
+ EXPECT_EQ(estimator.a_timestamps_size(), 1u);
+ EXPECT_EQ(estimator.b_timestamps_size(), 1u);
estimator.Sample(node_a, ta2, tb2);
estimator.Sample(node_b, tb2, ta2);
- EXPECT_EQ(estimator.ATimestamps().size(), 2u);
- EXPECT_EQ(estimator.BTimestamps().size(), 2u);
-
- // Adding the second point should slope up.
- EXPECT_EQ(estimator.fit().mpq_slope(), mpq_class(1, 100000));
+ EXPECT_EQ(estimator.a_timestamps_size(), 2u);
+ EXPECT_EQ(estimator.b_timestamps_size(), 2u);
estimator.Sample(node_a, ta3, tb3);
estimator.Sample(node_b, tb3, ta3);
- EXPECT_EQ(estimator.ATimestamps().size(), 3u);
- EXPECT_EQ(estimator.BTimestamps().size(), 3u);
-
- // And the third point shouldn't change anything.
- EXPECT_EQ(estimator.fit().mpq_slope(), mpq_class(1, 100000));
+ EXPECT_EQ(estimator.a_timestamps_size(), 3u);
+ EXPECT_EQ(estimator.b_timestamps_size(), 3u);
estimator.Pop(node_a, ta2);
estimator.Pop(node_b, tb2);
- EXPECT_EQ(estimator.ATimestamps().size(), 2u);
- EXPECT_EQ(estimator.BTimestamps().size(), 2u);
-
- // Dropping the first point should have the slope point back down.
- EXPECT_EQ(estimator.fit().mpq_slope(), mpq_class(-1, 100000));
+ EXPECT_EQ(estimator.a_timestamps_size(), 2u);
+ EXPECT_EQ(estimator.b_timestamps_size(), 2u);
// And dropping down to 1 point means 0 slope.
estimator.Pop(node_a, ta3);
estimator.Pop(node_b, tb3);
- EXPECT_EQ(estimator.ATimestamps().size(), 1u);
- EXPECT_EQ(estimator.BTimestamps().size(), 1u);
- EXPECT_EQ(estimator.fit().mpq_slope(), mpq_class(0));
+ EXPECT_EQ(estimator.a_timestamps_size(), 1u);
+ EXPECT_EQ(estimator.b_timestamps_size(), 1u);
}
} // namespace testing