Fix same-node clock offsets in localization utils
Change-Id: Ieed100aa877d27b5257900041ed639505ed9aa5c
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index fb46219..1f4aadc 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -3,7 +3,8 @@
namespace frc971::control_loops::drivetrain {
LocalizationUtils::LocalizationUtils(aos::EventLoop *event_loop)
- : output_fetcher_(event_loop->MakeFetcher<Output>("/drivetrain")),
+ : event_loop_(event_loop),
+ output_fetcher_(event_loop->MakeFetcher<Output>("/drivetrain")),
position_fetcher_(event_loop->TryMakeFetcher<Position>("/drivetrain")),
clock_offset_fetcher_(
event_loop->MakeFetcher<aos::message_bridge::ServerStatistics>(
@@ -54,6 +55,9 @@
std::optional<aos::monotonic_clock::duration> LocalizationUtils::ClockOffset(
std::string_view node) {
+ if (node == event_loop_->node()->name()->string_view()) {
+ return std::chrono::seconds(0);
+ }
std::optional<aos::monotonic_clock::duration> monotonic_offset;
clock_offset_fetcher_.Fetch();
if (clock_offset_fetcher_.get() != nullptr) {
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index e2985af..07f9aff 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -47,6 +47,7 @@
std::string_view node);
private:
+ aos::EventLoop *const event_loop_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
aos::Fetcher<frc971::control_loops::drivetrain::Position> position_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;