commit bcd96fc3f8602b609f0a65b231a3a7af001a46b4
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Mon Oct 12 20:29:32 2020 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Tue Oct 20 20:34:36 2020 -0700
tree 6a477562a2813f7468588ee8f58caa4d6c8f3eb6
parent f3950365eceba67b91e75347f76dff08f33784a7

Respect WasReset() indicator in the drivetrain

At least, pay attention to it for the localizer/down estimator.

Change-Id: I9cf8720c7ad9b2bd1f16b8e65acf824b10f3c7ed

y2020/control_loops/drivetrain/localizer_test.cc

diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index dbb4f2f..da3ef17 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -206,9 +206,9 @@
   void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
     *drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
         xytheta(2, 0), 0.0, 0.0;
-    Eigen::Matrix<float, Localizer::HybridEkf::kNStates, 1> localizer_state;
+    Eigen::Matrix<double, Localizer::HybridEkf::kNStates, 1> localizer_state;
     localizer_state.setZero();
-    localizer_state.block<3, 1>(0, 0) = xytheta.cast<float>();
+    localizer_state.block<3, 1>(0, 0) = xytheta;
     localizer_.Reset(monotonic_now(), localizer_state);
   }
 
@@ -547,6 +547,33 @@
   EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
 }
 
+// Tests that we don't blow up if we stop getting updates for an extended period
+// of time and fall behind on fetching fron the cameras.
+TEST_F(LocalizedDrivetrainTest, FetchersHandleTimeGap) {
+  set_enable_cameras(true);
+  set_send_delay(std::chrono::seconds(0));
+  event_loop_factory()->set_network_delay(std::chrono::nanoseconds(1));
+  test_event_loop_
+      ->AddTimer([this]() { drivetrain_plant_.set_send_messages(false); })
+      ->Setup(test_event_loop_->monotonic_now());
+  test_event_loop_->AddPhasedLoop(
+      [this](int) {
+        auto builder = camera_sender_.MakeBuilder();
+        ImageMatchResultT image;
+        ASSERT_TRUE(
+            builder.Send(ImageMatchResult::Pack(*builder.fbb(), &image)));
+      },
+      std::chrono::milliseconds(20));
+  test_event_loop_
+      ->AddTimer([this]() {
+        drivetrain_plant_.set_send_messages(true);
+        SimulateSensorReset();
+      })
+      ->Setup(test_event_loop_->monotonic_now() + std::chrono::seconds(10));
+
+  RunFor(chrono::seconds(20));
+}
+
 }  // namespace testing
 }  // namespace drivetrain
 }  // namespace control_loops