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) {