Move y2022 roborio "localizer" to frc971 for y2023
Rename the 2022 roborio "localizer" to PuppetLocalizer and make it
available to all the years.
This adds a test to frc971/ with some dependencies on y2022.
Change-Id: Id626440f5bf5d9ce714c5a0c5bac13ec50a42f5a
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 46eff94..ec81573 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -479,21 +479,6 @@
}
namespace {
-// Converts a flatbuffer TransformationMatrix to an Eigen matrix. Technically,
-// this should be able to do a single memcpy, but the extra verbosity here seems
-// appropriate.
-Eigen::Matrix<double, 4, 4> FlatbufferToTransformationMatrix(
- const frc971::vision::calibration::TransformationMatrix &flatbuffer) {
- CHECK_EQ(16u, CHECK_NOTNULL(flatbuffer.data())->size());
- Eigen::Matrix<double, 4, 4> result;
- result.setIdentity();
- for (int row = 0; row < 4; ++row) {
- for (int col = 0; col < 4; ++col) {
- result(row, col) = (*flatbuffer.data())[row * 4 + col];
- }
- }
- return result;
-}
// Node names of the pis to listen for cameras from.
constexpr std::array<std::string_view, ModelBasedLocalizer::kNumPis> kPisToUse{
@@ -521,7 +506,8 @@
}
CHECK(calibration->has_fixed_extrinsics());
const Eigen::Matrix<double, 4, 4> fixed_extrinsics =
- FlatbufferToTransformationMatrix(*calibration->fixed_extrinsics());
+ control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *calibration->fixed_extrinsics());
// Calculate the pose of the camera relative to the robot origin.
Eigen::Matrix<double, 4, 4> H_robot_camera = fixed_extrinsics;
@@ -530,7 +516,8 @@
H_robot_camera *
frc971::control_loops::TransformationMatrixForYaw<double>(
state.turret_position) *
- FlatbufferToTransformationMatrix(*calibration->turret_extrinsics());
+ control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *calibration->turret_extrinsics());
}
return H_robot_camera;
}