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/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 7e0500e..86c1ec0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -831,17 +831,3 @@
"//aos/network/www:proxy",
],
)
-
-cc_library(
- name = "localization_utils",
- srcs = ["localization_utils.cc"],
- hdrs = ["localization_utils.h"],
- deps = [
- ":drivetrain_output_fbs",
- "//aos/events:event_loop",
- "//aos/network:message_bridge_server_fbs",
- "//frc971/input:joystick_state_fbs",
- "//frc971/vision:calibration_fbs",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
diff --git a/frc971/control_loops/drivetrain/localization/BUILD b/frc971/control_loops/drivetrain/localization/BUILD
new file mode 100644
index 0000000..06c8d57
--- /dev/null
+++ b/frc971/control_loops/drivetrain/localization/BUILD
@@ -0,0 +1,66 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+cc_library(
+ name = "utils",
+ srcs = ["utils.cc"],
+ hdrs = ["utils.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/input:joystick_state_fbs",
+ "//frc971/vision:calibration_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "puppet_localizer",
+ srcs = ["puppet_localizer.cc"],
+ hdrs = ["puppet_localizer.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:localizer",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ ],
+)
+
+cc_test(
+ name = "puppet_localizer_test",
+ srcs = ["puppet_localizer_test.cc"],
+ data = ["//y2022/control_loops/drivetrain:simulation_config"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":puppet_localizer",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/network:team_number",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//y2022/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+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"],
+)
diff --git a/y2022/localizer/localizer_output.fbs b/frc971/control_loops/drivetrain/localization/localizer_output.fbs
similarity index 100%
rename from y2022/localizer/localizer_output.fbs
rename to frc971/control_loops/drivetrain/localization/localizer_output.fbs
diff --git a/y2022/control_loops/drivetrain/localizer.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
similarity index 82%
rename from y2022/control_loops/drivetrain/localizer.cc
rename to frc971/control_loops/drivetrain/localization/puppet_localizer.cc
index 0aa4456..3125793 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
@@ -1,10 +1,10 @@
-#include "y2022/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
-Localizer::Localizer(
+PuppetLocalizer::PuppetLocalizer(
aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
&dt_config)
@@ -15,8 +15,6 @@
localizer_output_fetcher_(
event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
"/localizer")),
- joystick_state_fetcher_(
- event_loop_->MakeFetcher<aos::JoystickState>("/aos")),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")) {
@@ -30,7 +28,7 @@
target_selector_.set_has_target(false);
}
-void Localizer::Reset(
+void PuppetLocalizer::Reset(
aos::monotonic_clock::time_point t,
const frc971::control_loops::drivetrain::HybridEkf<double>::State &state) {
// Go through and clear out all of the fetchers so that we don't get behind.
@@ -38,19 +36,12 @@
ekf_.ResetInitialState(t, state.cast<float>(), ekf_.P());
}
-void Localizer::Update(const Eigen::Matrix<double, 2, 1> &U,
+void PuppetLocalizer::Update(const Eigen::Matrix<double, 2, 1> &U,
aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
double gyro_rate, const Eigen::Vector3d &accel) {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
U.cast<float>(), accel.cast<float>(), now);
- joystick_state_fetcher_.Fetch();
- if (joystick_state_fetcher_.get() != nullptr &&
- joystick_state_fetcher_->autonomous()) {
- // TODO(james): This is an inelegant way to avoid having the localizer mess
- // up splines. Do better.
- // return;
- }
if (localizer_output_fetcher_.Fetch()) {
clock_offset_fetcher_.Fetch();
bool message_bridge_connected = true;
@@ -79,8 +70,6 @@
std::chrono::nanoseconds(
localizer_output_fetcher_->monotonic_timestamp_ns()) -
monotonic_offset);
- // TODO: Finish implementing simple x/y/theta updater with state_at_capture.
- // TODO: Implement turret/camera processing logic on pi side.
std::optional<State> state_at_capture =
ekf_.LastStateBeforeTime(capture_time);
if (!state_at_capture.has_value()) {
@@ -104,4 +93,4 @@
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
diff --git a/y2022/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
similarity index 78%
rename from y2022/control_loops/drivetrain/localizer.h
rename to frc971/control_loops/drivetrain/localization/puppet_localizer.h
index 77b29eb..4f8f4f3 100644
--- a/y2022/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
@@ -1,5 +1,5 @@
-#ifndef Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
-#define Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
#include <string_view>
@@ -7,20 +7,20 @@
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
-#include "frc971/input/joystick_state_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
-// This class handles the localization for the 2022 robot. Rather than actually
-// doing any work on the roborio, we farm all the localization out to a
+// This class handles the localization for the 2022/2023 robots. Rather than
+// actually doing any work on the roborio, we farm all the localization out to a
// raspberry pi and it then sends out LocalizerOutput messages that we treat as
-// measurement updates. See //y2022/localizer.
-// TODO(james): Needs tests. Should refactor out some of the code from the 2020
-// localizer test.
-class Localizer : public frc971::control_loops::drivetrain::LocalizerInterface {
+// measurement updates. See //y202*/localizer.
+// TODO(james): Needs more tests. Should refactor out some of the code from the
+// 2020 localizer test.
+class PuppetLocalizer
+ : public frc971::control_loops::drivetrain::LocalizerInterface {
public:
typedef frc971::control_loops::TypedPose<float> Pose;
typedef frc971::control_loops::drivetrain::HybridEkf<float> HybridEkf;
@@ -29,9 +29,10 @@
typedef typename HybridEkf::StateSquare StateSquare;
typedef typename HybridEkf::Input Input;
typedef typename HybridEkf::Output Output;
- Localizer(aos::EventLoop *event_loop,
- const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config);
+ PuppetLocalizer(
+ aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config);
frc971::control_loops::drivetrain::HybridEkf<double>::State Xhat()
const override {
return ekf_.X_hat().cast<double>();
@@ -93,7 +94,6 @@
HybridEkf::ExpectedObservationAllocator<Corrector> observations_;
aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
- aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
// Target selector to allow us to satisfy the LocalizerInterface requirements.
@@ -102,6 +102,6 @@
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
-#endif // Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_PUPPET_LOCALIZER_H_
diff --git a/y2022/control_loops/drivetrain/localizer_test.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
similarity index 92%
rename from y2022/control_loops/drivetrain/localizer_test.cc
rename to frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
index 77f3988..d64c419 100644
--- a/y2022/control_loops/drivetrain/localizer_test.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
@@ -1,4 +1,4 @@
-#include "y2022/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include <queue>
@@ -8,17 +8,17 @@
#include "aos/network/testing_time_converter.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
-#include "y2022/localizer/localizer_output_generated.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
DECLARE_bool(die_on_malloc);
-namespace y2022 {
+namespace frc971 {
namespace control_loops {
namespace drivetrain {
namespace testing {
@@ -29,7 +29,8 @@
namespace {
DrivetrainConfig<double> GetTest2022DrivetrainConfig() {
- DrivetrainConfig<double> config = GetDrivetrainConfig();
+ DrivetrainConfig<double> config =
+ y2022::control_loops::drivetrain::GetDrivetrainConfig();
return config;
}
} // namespace
@@ -39,11 +40,13 @@
using frc971::control_loops::drivetrain::DrivetrainLoop;
using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
+
// TODO(james): Make it so this actually tests the full system of the localizer.
class LocalizedDrivetrainTest : public frc971::testing::ControlLoopTest {
protected:
- // We must use the 2022 drivetrain config so that we don't have to deal
- // with shifting:
+ // We must use the 2022 drivetrain config so that we actually have a multi-nde
+ // config with a LocalizerOutput message.
+ // TODO(james): Refactor this test to be year-agnostic.
LocalizedDrivetrainTest()
: frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig(
@@ -108,7 +111,8 @@
void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
*drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
xytheta(2, 0), 0.0, 0.0;
- Eigen::Matrix<double, Localizer::HybridEkf::kNStates, 1> localizer_state;
+ Eigen::Matrix<double, PuppetLocalizer::HybridEkf::kNStates, 1>
+ localizer_state;
localizer_state.setZero();
localizer_state.block<3, 1>(0, 0) = xytheta;
localizer_.Reset(monotonic_now(), localizer_state);
@@ -169,7 +173,7 @@
std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
- Localizer localizer_;
+ PuppetLocalizer localizer_;
DrivetrainLoop drivetrain_;
std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
@@ -208,4 +212,4 @@
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
-} // namespace y2022
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/localization_utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
similarity index 97%
rename from frc971/control_loops/drivetrain/localization_utils.cc
rename to frc971/control_loops/drivetrain/localization/utils.cc
index c7968e1..ff027d0 100644
--- a/frc971/control_loops/drivetrain/localization_utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/drivetrain/localization_utils.h"
+#include "frc971/control_loops/drivetrain/localization/utils.h"
namespace frc971::control_loops::drivetrain {
diff --git a/frc971/control_loops/drivetrain/localization_utils.h b/frc971/control_loops/drivetrain/localization/utils.h
similarity index 100%
rename from frc971/control_loops/drivetrain/localization_utils.h
rename to frc971/control_loops/drivetrain/localization/utils.h
diff --git a/y2022/BUILD b/y2022/BUILD
index e978f7a..1a04c2e 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -102,7 +102,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//frc971/vision:vision_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//y2022/vision:target_estimate_fbs",
"//y2022/vision:ball_color_fbs",
@@ -132,7 +132,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_status_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//y2022/localizer:localizer_visualization_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2022/control_loops/drivetrain/BUILD b/y2022/control_loops/drivetrain/BUILD
index 18855a6..148a998 100644
--- a/y2022/control_loops/drivetrain/BUILD
+++ b/y2022/control_loops/drivetrain/BUILD
@@ -71,19 +71,6 @@
],
)
-cc_library(
- name = "localizer",
- srcs = ["localizer.cc"],
- hdrs = ["localizer.h"],
- deps = [
- "//aos/events:event_loop",
- "//aos/network:message_bridge_server_fbs",
- "//frc971/control_loops/drivetrain:hybrid_ekf",
- "//frc971/control_loops/drivetrain:localizer",
- "//y2022/localizer:localizer_output_fbs",
- ],
-)
-
cc_binary(
name = "drivetrain",
srcs = [
@@ -93,10 +80,10 @@
visibility = ["//visibility:public"],
deps = [
":drivetrain_base",
- ":localizer",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain/localization:puppet_localizer",
],
)
@@ -111,25 +98,6 @@
],
)
-cc_test(
- name = "localizer_test",
- srcs = ["localizer_test.cc"],
- data = [":simulation_config"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":drivetrain_base",
- ":localizer",
- "//aos/events:simulated_event_loop",
- "//aos/events/logging:log_writer",
- "//aos/network:team_number",
- "//frc971/control_loops:control_loop_test",
- "//frc971/control_loops:team_number_test_environment",
- "//frc971/control_loops/drivetrain:drivetrain_lib",
- "//frc971/control_loops/drivetrain:drivetrain_test_lib",
- "//y2022/localizer:localizer_output_fbs",
- ],
-)
-
cc_binary(
name = "trajectory_generator",
srcs = [
diff --git a/y2022/control_loops/drivetrain/drivetrain_main.cc b/y2022/control_loops/drivetrain/drivetrain_main.cc
index a422eaa..6e02cc7 100644
--- a/y2022/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_main.cc
@@ -3,8 +3,8 @@
#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 "y2022/control_loops/drivetrain/drivetrain_base.h"
-#include "y2022/control_loops/drivetrain/localizer.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -15,10 +15,11 @@
aos::configuration::ReadConfig("aos_config.json");
aos::ShmEventLoop event_loop(&config.message());
- std::unique_ptr<::y2022::control_loops::drivetrain::Localizer> localizer =
- std::make_unique<y2022::control_loops::drivetrain::Localizer>(
- &event_loop,
- y2022::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+ localizer =
+ std::make_unique<frc971::control_loops::drivetrain::PuppetLocalizer>(
+ &event_loop,
+ y2022::control_loops::drivetrain::GetDrivetrainConfig());
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
y2022::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
localizer.get());
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index a085049..e0db64c 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -193,10 +193,10 @@
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/queues:gyro_fbs",
"//third_party:phoenix",
"//third_party:wpilib",
- "//y2022/localizer:localizer_output_fbs",
],
)
diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
index c058254..71bc73b 100644
--- a/y2022/control_loops/superstructure/led_indicator.h
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -8,11 +8,11 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/queues/gyro_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
namespace y2022::control_loops::superstructure {
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/vision/BUILD b/y2022/vision/BUILD
index 8d8a43c..99fb95d 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -98,11 +98,11 @@
"//aos/events:event_loop",
"//aos/events:shm_event_loop",
"//aos/network:team_number",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:v4l2_reader",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
- "//y2022/localizer:localizer_output_fbs",
],
)
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index c2d4f5f..7f07704 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -11,10 +11,10 @@
#include "aos/events/shm_event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/v4l2_reader.h"
#include "frc971/vision/vision_generated.h"
-#include "y2022/localizer/localizer_output_generated.h"
#include "y2022/vision/calibration_data.h"
#include "y2022/vision/gpio.h"
#include "y2022/vision/target_estimate_generated.h"
diff --git a/y2022/www/BUILD b/y2022/www/BUILD
index 386ca90..e806404 100644
--- a/y2022/www/BUILD
+++ b/y2022/www/BUILD
@@ -24,8 +24,8 @@
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
"//y2022/control_loops/superstructure:superstructure_status_ts_fbs",
- "//y2022/localizer:localizer_output_ts_fbs",
"//y2022/localizer:localizer_status_ts_fbs",
"//y2022/localizer:localizer_visualization_ts_fbs",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
diff --git a/y2022/www/field_handler.ts b/y2022/www/field_handler.ts
index ec25607..d20637f 100644
--- a/y2022/www/field_handler.ts
+++ b/y2022/www/field_handler.ts
@@ -1,7 +1,7 @@
import {ByteBuffer} from 'flatbuffers';
import {Connection} from '../../aos/network/www/proxy';
import {IntakeState, Status as SuperstructureStatus, SuperstructureState} from '../control_loops/superstructure/superstructure_status_generated'
-import {LocalizerOutput} from '../localizer/localizer_output_generated';
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated';
import {RejectionReason} from '../localizer/localizer_status_generated';
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated';
import {LocalizerVisualization, TargetEstimateDebug} from '../localizer/localizer_visualization_generated';
diff --git a/y2023/BUILD b/y2023/BUILD
index 16273e5..ec30c34 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -102,7 +102,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2023/constants:constants_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
@@ -135,7 +135,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_status_fbs",
- "//y2022/localizer:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//y2022/localizer:localizer_visualization_fbs",
"//frc971/vision:target_map_fbs",
],