Improve localizer update debugging
* Only send LocalizerDebug messages when something interesting happens.
* On debug field, draw both the implied robot position of the update as
well as the camera position.
* Indicate accepted vs. rejected images via red/green colors.
* Leave old corrections on the display for 2 seconds and gradually fade
them out.
* Add aggregate statistics about why we are rejecting image corrections
to the readouts table.
Change-Id: Ibc3397cb5654aacbc6cce4e5f3eb71f0371692cc
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 19ee4ad..5458797 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -78,6 +78,23 @@
double velocity = 0.0; // rad/sec
};
+ static constexpr size_t kNumRejectionReasons =
+ static_cast<int>(RejectionReason::MAX) -
+ static_cast<int>(RejectionReason::MIN) + 1;
+
+ struct Statistics {
+ int total_accepted = 0;
+ int total_candidates = 0;
+ static_assert(0 == static_cast<int>(RejectionReason::MIN));
+ static_assert(
+ kNumRejectionReasons ==
+ sizeof(
+ std::invoke_result<decltype(EnumValuesRejectionReason)>::type) /
+ sizeof(RejectionReason),
+ "RejectionReason has non-contiguous error values.");
+ std::array<int, kNumRejectionReasons> rejection_counts;
+ };
+
// Processes new image data from the given pi and updates the EKF.
aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5> HandleImageMatch(
size_t camera_index, std::string_view pi,
@@ -111,6 +128,8 @@
// Target selector to allow us to satisfy the LocalizerInterface requirements.
frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
+
+ Statistics statistics_;
};
} // namespace drivetrain