Respect WasReset() indicator in the drivetrain
At least, pay attention to it for the localizer/down estimator.
Change-Id: I9cf8720c7ad9b2bd1f16b8e65acf824b10f3c7ed
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 7d7c83f..79035df 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -95,6 +95,16 @@
target_selector_.set_has_target(false);
}
+void Localizer::Reset(
+ aos::monotonic_clock::time_point t,
+ const frc971::control_loops::drivetrain::HybridEkf<double>::State &state) {
+ // Go through and clear out all of the fetchers so that we don't get behind.
+ for (auto &fetcher : image_fetchers_) {
+ fetcher.Fetch();
+ }
+ ekf_.ResetInitialState(t, state.cast<float>(), ekf_.P());
+}
+
void Localizer::HandleSuperstructureStatus(
const y2020::control_loops::superstructure::Status &status) {
CHECK(status.has_turret());
@@ -162,7 +172,10 @@
LOG(WARNING) << "Got camera frame from the future.";
return;
}
- CHECK(result.has_camera_calibration());
+ if (!result.has_camera_calibration()) {
+ LOG(WARNING) << "Got camera frame without calibration data.";
+ return;
+ }
// Per the ImageMatchResult specification, we can actually determine whether
// the camera is the turret camera just from the presence of the
// turret_extrinsics member.
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 341f304..5dbad02 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -51,9 +51,9 @@
double right_encoder, double gyro_rate,
const Eigen::Vector3d &accel) override;
- void Reset(aos::monotonic_clock::time_point t, const State &state) {
- ekf_.ResetInitialState(t, state, ekf_.P());
- }
+ void Reset(aos::monotonic_clock::time_point t,
+ const frc971::control_loops::drivetrain::HybridEkf<double>::State
+ &state) override;
void ResetPosition(aos::monotonic_clock::time_point t, double x, double y,
double theta, double /*theta_override*/,
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