Populate a JSON file with all the possible scoring locations.
Note: I have not double-checked these actual locations. Needs further
checking.
Change-Id: I8338f9389dad7f4e81fcf7cbb7d55faa3c8c064e
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2023/localizer/utils.cc b/y2023/localizer/utils.cc
new file mode 100644
index 0000000..7faca1f
--- /dev/null
+++ b/y2023/localizer/utils.cc
@@ -0,0 +1,14 @@
+#include "y2023/localizer/utils.h"
+
+namespace y2023::localizer {
+Eigen::Matrix<double, 4, 4> PoseToTransform(
+ const frc971::vision::TargetPoseFbs *pose) {
+ const frc971::vision::Position *position = pose->position();
+ const frc971::vision::Quaternion *quaternion = pose->orientation();
+ return (Eigen::Translation3d(
+ Eigen::Vector3d(position->x(), position->y(), position->z())) *
+ Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
+ quaternion->z()))
+ .matrix();
+}
+} // namespace y2023::localizer