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/BUILD b/y2022/localizer/BUILD
index a3b1774..dd0fb67 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -17,22 +17,6 @@
)
flatbuffer_cc_library(
- name = "localizer_output_fbs",
- srcs = [
- "localizer_output.fbs",
- ],
- gen_reflections = True,
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
-)
-
-flatbuffer_ts_library(
- name = "localizer_output_ts_fbs",
- srcs = ["localizer_output.fbs"],
- visibility = ["//visibility:public"],
-)
-
-flatbuffer_cc_library(
name = "localizer_status_fbs",
srcs = [
"localizer_status.fbs",
@@ -94,7 +78,6 @@
hdrs = ["localizer.h"],
visibility = ["//visibility:public"],
deps = [
- ":localizer_output_fbs",
":localizer_status_fbs",
":localizer_visualization_fbs",
"//aos/containers:ring_buffer",
@@ -107,8 +90,9 @@
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:improved_down_estimator",
- "//frc971/control_loops/drivetrain:localization_utils",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:utils",
"//frc971/imu_reader:imu_watcher",
"//frc971/input:joystick_state_fbs",
"//frc971/vision:calibration_fbs",
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;
}
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index a403ca8..59ad75c 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -9,18 +9,18 @@
#include "aos/network/message_bridge_server_generated.h"
#include "aos/time/time.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
-#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/utils.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
-#include "frc971/control_loops/drivetrain/localization_utils.h"
+#include "frc971/imu_reader/imu_watcher.h"
+#include "frc971/input/joystick_state_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
#include "frc971/zeroing/wrap.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
#include "y2022/localizer/localizer_status_generated.h"
#include "y2022/localizer/localizer_visualization_generated.h"
#include "y2022/vision/target_estimate_generated.h"
-#include "frc971/imu_reader/imu_watcher.h"
namespace frc971::controls {
diff --git a/y2022/localizer/localizer_output.fbs b/y2022/localizer/localizer_output.fbs
deleted file mode 100644
index ff25c31..0000000
--- a/y2022/localizer/localizer_output.fbs
+++ /dev/null
@@ -1,42 +0,0 @@
-namespace frc971.controls;
-
-// This provides a minimal output from the localizer that can be forwarded to
-// the roborio and used for corrections to its (simpler) localizer.
-
-struct Quaternion {
- w:double (id: 0);
- x:double (id: 1);
- y:double (id: 2);
- z:double (id: 3);
-}
-
-// Used to tell different LEDs to be on or off
-enum LedOutput : byte {
- ON,
- OFF
-}
-
-table LocalizerOutput {
- // Timestamp (on the source node) that this sample corresponds with. This
- // may be older than the sent time to account for delays in sensor readings.
- monotonic_timestamp_ns:int64 (id: 0);
- // Current x/y position estimate, in meters.
- x:double (id: 1);
- y:double (id: 2);
- // Current heading, in radians.
- theta:double (id: 3);
- // Current estimate of the robot's 3-D rotation.
- orientation:Quaternion (id: 4);
- // Whether we have zeroed the IMU (may go false if we observe a fault with the
- // IMU).
- zeroed:bool (id: 5);
-
- // Whether each led should be on.
- // Indices correspond to pi number.
- led_outputs:[LedOutput] (id: 6);
-
- // Cumulative number of accepted images.
- image_accepted_count:uint (id: 7);
-}
-
-root_type LocalizerOutput;