Create y2024 localizer
This is primarily a copy of the y2023 localizer, with updates to
better characterize the noise of the april tag readings (by separating
out heading/distance/skew measurements).
It also listens to the drivetrain Position message for encoder readings
rather than relying on the IMU board to send them.
This adds a few things:
* The main localizer libraries and processes themselves.
* Updates to the AOS configs to pull in the appropriate localization
channels.
* Creates the typescript plots for localization debugging.
* Creates some dummy camera extrinsics for use in the tests.
Change-Id: I58d5c1da0d3dc2dad98bd2a9fc10965db51c4f84
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/localizer/visualization.fbs b/y2024/localizer/visualization.fbs
new file mode 100644
index 0000000..d903e3a
--- /dev/null
+++ b/y2024/localizer/visualization.fbs
@@ -0,0 +1,47 @@
+include "y2024/localizer/status.fbs";
+
+namespace y2024.localizer;
+
+attribute "static_length";
+
+table Measurement {
+ heading:double (id: 0);
+ distance:double (id: 1);
+ skew:double (id: 2);
+}
+
+table TargetEstimateDebug {
+ camera:uint8 (id: 0);
+ camera_x:double (id: 1);
+ camera_y:double (id: 2);
+ camera_theta:double (id: 3);
+ implied_robot_x:double (id: 4);
+ implied_robot_y:double (id: 5);
+ implied_robot_theta:double (id: 6);
+ accepted:bool (id: 7);
+ rejection_reason:RejectionReason (id: 8);
+ // Image age (more human-readable than trying to interpret raw nanosecond
+ // values).
+ image_age_sec:double (id: 9);
+ // Time at which the image was captured.
+ image_monotonic_timestamp_ns:uint64 (id: 10);
+ // April tag ID used for this image detection.
+ april_tag:uint (id: 11);
+ // If the image was accepted, the total correction that occurred as a result.
+ // These numbers will be equal to the value after the correction - the value
+ // before.
+ correction_x: double (id: 12);
+ correction_y: double (id: 13);
+ correction_theta: double (id: 14);
+ // The expected observation given the current estimate of the robot pose.
+ expected_observation:Measurement (id: 15);
+ actual_observation:Measurement (id: 16);
+ modeled_noise:Measurement (id: 17);
+}
+
+table Visualization {
+ targets:[TargetEstimateDebug] (id: 0, static_length: 20);
+ statistics:CumulativeStatistics (id: 1);
+}
+
+root_type Visualization;