Create y2023 localizer
This creates a localizer implementation for y2023, and implements simple
tests. This is actually more similar to the y2020 iteration than the
y2022 iteration in implementation, as I backed away from trying to
maintain the more complex y2022 logic (since I wasn't actually using the
features of that code). Somewhat notable changes from past years:
* New target format using the TargetMap that the april robotics detector
sends out.
* Does not attempt to entirely ignore the implied yaw indicated by the
target detections, since unlike in 2020/2022, we are not shooting at
the target and instead attempting to go somewhere offset from the
target.
Currently has no way of getting confidence values from the april
robotics code, so treats all detections the same. Will need to tune
this.
Change-Id: I072cd3fb2657081bca74c55570842960c5ad7b1b
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2023/control_loops/drivetrain/drivetrain_main.cc b/y2023/control_loops/drivetrain/drivetrain_main.cc
index 0817b3d..32b2455 100644
--- a/y2023/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_main.cc
@@ -3,6 +3,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -14,11 +15,11 @@
aos::configuration::ReadConfig("aos_config.json");
aos::ShmEventLoop event_loop(&config.message());
- std::unique_ptr<::frc971::control_loops::drivetrain::DeadReckonEkf>
- localizer =
- std::make_unique<::frc971::control_loops::drivetrain::DeadReckonEkf>(
- &event_loop,
- ::y2023::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+ localizer = std::make_unique<
+ ::frc971::control_loops::drivetrain::PuppetLocalizer>(
+ &event_loop,
+ ::y2023::control_loops::drivetrain::GetDrivetrainConfig());
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
y2023::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
localizer.get());