Support time having slope in SimulatedEventLoop
This sets us up to use our new piecewise linear time estimation
algorithm in the scheduler.
Change-Id: I8922e7c4b790e5f14a583b6e52d7be40e5997df2
diff --git a/aos/events/BUILD b/aos/events/BUILD
index 0212eaa..01e256c 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -312,6 +312,16 @@
],
)
+cc_test(
+ name = "event_scheduler_test",
+ srcs = ["event_scheduler_test.cc"],
+ deps = [
+ ":simulated_event_loop",
+ "//aos/testing:googletest",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
cc_library(
name = "aos_logging",
srcs = [
diff --git a/aos/events/event_scheduler.cc b/aos/events/event_scheduler.cc
index f985eb1..202c772 100644
--- a/aos/events/event_scheduler.cc
+++ b/aos/events/event_scheduler.cc
@@ -44,11 +44,13 @@
void EventScheduler::CallOldestEvent() {
CHECK_GT(events_list_.size(), 0u);
auto iter = events_list_.begin();
- now_ = iter->first;
+ monotonic_now_ = iter->first;
+ monotonic_now_valid_ = true;
::std::function<void()> callback = ::std::move(iter->second);
events_list_.erase(iter);
callback();
+ monotonic_now_valid_ = false;
}
void EventScheduler::RunOnRun() {
diff --git a/aos/events/event_scheduler.h b/aos/events/event_scheduler.h
index e6c732b..468d904 100644
--- a/aos/events/event_scheduler.h
+++ b/aos/events/event_scheduler.h
@@ -81,16 +81,20 @@
// measurement.
distributed_clock::time_point ToDistributedClock(
monotonic_clock::time_point time) const {
- return distributed_clock::epoch() + time.time_since_epoch() +
- monotonic_offset_;
+ return distributed_clock::epoch() +
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ (time.time_since_epoch() - distributed_offset_) /
+ distributed_slope_);
}
// 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() + time.time_since_epoch() -
- monotonic_offset_;
+ return monotonic_clock::epoch() +
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ time.time_since_epoch() * distributed_slope_) +
+ distributed_offset_;
}
// Returns the current monotonic time on this node calculated from the
@@ -98,24 +102,31 @@
inline monotonic_clock::time_point monotonic_now() const;
// Sets the offset between the distributed and monotonic clock.
- // distributed = monotonic + offset;
- void SetDistributedOffset(std::chrono::nanoseconds monotonic_offset) {
- monotonic_offset_ = monotonic_offset;
- }
+ // 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;
- // Returns the offset used to convert to and from the distributed clock.
- std::chrono::nanoseconds monotonic_offset() const {
- return monotonic_offset_;
+ // 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.
- monotonic_clock::time_point now_ = monotonic_clock::epoch();
+ 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 monotonic_offset_ = std::chrono::seconds(0);
+ 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_;
@@ -184,8 +195,14 @@
inline monotonic_clock::time_point EventScheduler::monotonic_now() const {
// Make sure we stay in sync.
- CHECK_EQ(now_, FromDistributedClock(scheduler_scheduler_->distributed_now()));
- return now_;
+ if (monotonic_now_valid_) {
+ CHECK_NEAR(monotonic_now_,
+ FromDistributedClock(scheduler_scheduler_->distributed_now()),
+ std::chrono::nanoseconds(1));
+ return monotonic_now_;
+ } else {
+ 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
new file mode 100644
index 0000000..e2523db
--- /dev/null
+++ b/aos/events/event_scheduler_test.cc
@@ -0,0 +1,50 @@
+#include "aos/events/event_scheduler.h"
+
+#include <chrono>
+
+#include "gtest/gtest.h"
+
+namespace aos {
+
+namespace chrono = std::chrono;
+
+// Tests that the default parameters (slope of 1, offest of 0) behave as
+// an identity.
+TEST(EventSchedulerTest, IdentityTimeConversion) {
+ EventScheduler s;
+ EXPECT_EQ(s.FromDistributedClock(distributed_clock::epoch()),
+ monotonic_clock::epoch());
+
+ EXPECT_EQ(
+ s.FromDistributedClock(distributed_clock::epoch() + chrono::seconds(1)),
+ monotonic_clock::epoch() + chrono::seconds(1));
+
+ 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));
+}
+
+// Tests that a non-unity slope is computed correctly.
+TEST(EventSchedulerTest, DoubleTimeConversion) {
+ EventScheduler s;
+ s.SetDistributedOffset(std::chrono::seconds(7), 2.0);
+
+ EXPECT_EQ(s.FromDistributedClock(distributed_clock::epoch()),
+ monotonic_clock::epoch() + chrono::seconds(7));
+
+ EXPECT_EQ(
+ s.FromDistributedClock(distributed_clock::epoch() + chrono::seconds(1)),
+ monotonic_clock::epoch() + chrono::seconds(9));
+
+ 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));
+}
+
+} // namespace aos
diff --git a/aos/events/logging/logger.cc b/aos/events/logging/logger.cc
index d630e98..eff977b 100644
--- a/aos/events/logging/logger.cc
+++ b/aos/events/logging/logger.cc
@@ -614,7 +614,8 @@
size_t node_index = 0;
for (std::unique_ptr<State> &state : states_) {
- state->node_event_loop_factory->SetDistributedOffset(offset(node_index));
+ state->node_event_loop_factory->SetDistributedOffset(-offset(node_index),
+ 1.0);
++node_index;
}
}
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 7843647..b6a30cb 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -667,7 +667,7 @@
const chrono::nanoseconds initial_pi2_offset = -chrono::seconds(1000);
chrono::nanoseconds pi2_offset = initial_pi2_offset;
- pi2->SetDistributedOffset(pi2_offset);
+ pi2->SetDistributedOffset(-pi2_offset, 1.0);
LOG(INFO) << "pi2 times: " << pi2->monotonic_now() << " "
<< pi2->realtime_now() << " distributed "
<< pi2->ToDistributedClock(pi2->monotonic_now());
@@ -675,7 +675,7 @@
for (int i = 0; i < 95; ++i) {
pi2_offset += chrono::nanoseconds(200);
- pi2->SetDistributedOffset(pi2_offset);
+ pi2->SetDistributedOffset(-pi2_offset, 1.0);
event_loop_factory_.RunFor(chrono::milliseconds(1));
}
@@ -692,7 +692,7 @@
for (int i = 0; i < 20000; ++i) {
pi2_offset += chrono::nanoseconds(200);
- pi2->SetDistributedOffset(pi2_offset);
+ pi2->SetDistributedOffset(-pi2_offset, 1.0);
event_loop_factory_.RunFor(chrono::milliseconds(1));
}
@@ -702,7 +702,7 @@
for (int i = 0; i < 40000; ++i) {
pi2_offset -= chrono::nanoseconds(200);
- pi2->SetDistributedOffset(pi2_offset);
+ pi2->SetDistributedOffset(-pi2_offset, 1.0);
event_loop_factory_.RunFor(chrono::milliseconds(1));
}
}
diff --git a/aos/events/simulated_event_loop.h b/aos/events/simulated_event_loop.h
index c8029de..de19b11 100644
--- a/aos/events/simulated_event_loop.h
+++ b/aos/events/simulated_event_loop.h
@@ -158,11 +158,14 @@
// measurement.
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) {
- scheduler_.SetDistributedOffset(monotonic_offset);
+ void SetDistributedOffset(std::chrono::nanoseconds monotonic_offset,
+ double monotonic_slope) {
+ scheduler_.SetDistributedOffset(monotonic_offset, monotonic_slope);
}
private:
@@ -192,7 +195,7 @@
inline monotonic_clock::time_point NodeEventLoopFactory::monotonic_now() const {
// TODO(austin): Confirm that time never goes backwards?
- return scheduler_.FromDistributedClock(factory_->distributed_now());
+ return scheduler_.monotonic_now();
}
inline realtime_clock::time_point NodeEventLoopFactory::realtime_now() const {
@@ -200,6 +203,11 @@
realtime_offset_);
}
+inline monotonic_clock::time_point NodeEventLoopFactory::FromDistributedClock(
+ distributed_clock::time_point time) const {
+ return scheduler_.FromDistributedClock(time);
+}
+
inline distributed_clock::time_point NodeEventLoopFactory::ToDistributedClock(
monotonic_clock::time_point time) const {
return scheduler_.ToDistributedClock(time);
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 34d772c..a7360c1 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -86,7 +86,7 @@
return locations;
}
-constexpr std::chrono::seconds kPiTimeOffset(10);
+constexpr std::chrono::seconds kPiTimeOffset(-10);
} // namespace
namespace chrono = std::chrono;
@@ -129,7 +129,7 @@
drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
last_frame_(monotonic_now()) {
event_loop_factory()->GetNodeEventLoopFactory(pi1_)->SetDistributedOffset(
- kPiTimeOffset);
+ kPiTimeOffset, 1.0);
set_team_id(frc971::control_loops::testing::kTeamNumber);
set_battery_voltage(12.0);
@@ -167,7 +167,7 @@
builder.MakeBuilder<aos::message_bridge::ServerConnection>();
connection_builder.add_node(node_offset);
connection_builder.add_monotonic_offset(
- chrono::duration_cast<chrono::nanoseconds>(-kPiTimeOffset)
+ chrono::duration_cast<chrono::nanoseconds>(kPiTimeOffset)
.count());
auto connection_offset = connection_builder.Finish();
auto connections_offset =