Initialize y2020 Localizer correctly
This prevents a crash loop.
Change-Id: I5683faa7752a4b0fcea81023ae784a9990c24288
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 4b11f6f..0178790 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -40,6 +40,11 @@
HandleSuperstructureStatus(status);
});
+ event_loop->OnRun([this, event_loop]() {
+ ekf_.ResetInitialState(event_loop->monotonic_now(), Ekf::State::Zero(),
+ ekf_.P());
+ });
+
image_fetchers_.emplace_back(
event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
"/pi1/camera"));
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 1e72c39..e81691c 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -124,7 +124,6 @@
drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
last_frame_(monotonic_now()) {
set_team_id(frc971::control_loops::testing::kTeamNumber);
- SetStartingPosition({3.0, 2.0, 0.0});
set_battery_voltage(12.0);
if (!FLAGS_output_file.empty()) {
@@ -167,6 +166,8 @@
},
chrono::milliseconds(5));
+ test_event_loop_->OnRun([this]() { SetStartingPosition({3.0, 2.0, 0.0}); });
+
// Run for enough time to allow the gyro/imu zeroing code to run.
RunFor(std::chrono::seconds(10));
}