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/localizer_replay.cc b/y2024/localizer/localizer_replay.cc
new file mode 100644
index 0000000..763ad36
--- /dev/null
+++ b/y2024/localizer/localizer_replay.cc
@@ -0,0 +1,68 @@
+#include "gflags/gflags.h"
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/network/team_number.h"
+#include "aos/util/simulation_logger.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/localizer/localizer.h"
+
+DEFINE_string(config, "y2024/aos_config.json",
+ "Name of the config file to replay using.");
+DEFINE_int32(team, 9971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/replayed",
+ "Name of the folder to write replayed logs to.");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ // sort logfiles
+ const std::vector<aos::logger::LogFile> logfiles =
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv));
+
+ // open logfiles
+ aos::logger::LogReader reader(logfiles, &config.message());
+
+ reader.RemapLoggedChannel("/localizer", "y2024.localizer.Status");
+ for (const auto orin : {"orin1", "orin2"}) {
+ for (const auto camera : {"camera0", "camera1"}) {
+ reader.RemapLoggedChannel(absl::StrCat("/", orin, "/", camera),
+ "y2024.localizer.Visualization");
+ }
+ }
+ reader.RemapLoggedChannel("/localizer", "frc971.controls.LocalizerOutput");
+
+ auto factory =
+ std::make_unique<aos::SimulatedEventLoopFactory>(reader.configuration());
+
+ reader.RegisterWithoutStarting(factory.get());
+
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), "imu");
+ }
+ std::vector<std::unique_ptr<aos::util::LoggerState>> loggers;
+
+ reader.OnStart(node, [&factory, node, &loggers]() {
+ aos::NodeEventLoopFactory *node_factory =
+ factory->GetNodeEventLoopFactory(node);
+ node_factory->AlwaysStart<y2024::localizer::Localizer>("localizer");
+ loggers.push_back(std::make_unique<aos::util::LoggerState>(
+ factory.get(), node, FLAGS_output_folder));
+ });
+
+ reader.event_loop_factory()->Run();
+
+ reader.Deregister();
+
+ return 0;
+}