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/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/drivetrain/localizer.cc b/y2022/control_loops/drivetrain/localizer.cc
deleted file mode 100644
index 0aa4456..0000000
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ /dev/null
@@ -1,107 +0,0 @@
-#include "y2022/control_loops/drivetrain/localizer.h"
-
-namespace y2022 {
-namespace control_loops {
-namespace drivetrain {
-
-Localizer::Localizer(
- aos::EventLoop *event_loop,
- const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config)
- : event_loop_(event_loop),
- dt_config_(dt_config),
- ekf_(dt_config),
- observations_(&ekf_),
- 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")) {
- ekf_.set_ignore_accel(true);
-
- event_loop->OnRun([this, event_loop]() {
- ekf_.ResetInitialState(event_loop->monotonic_now(),
- HybridEkf::State::Zero(), ekf_.P());
- });
-
- target_selector_.set_has_target(false);
-}
-
-void Localizer::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.
- localizer_output_fetcher_.Fetch();
- ekf_.ResetInitialState(t, state.cast<float>(), ekf_.P());
-}
-
-void Localizer::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;
- std::chrono::nanoseconds monotonic_offset{0};
- if (clock_offset_fetcher_.get() != nullptr) {
- for (const auto connection : *clock_offset_fetcher_->connections()) {
- if (connection->has_node() && connection->node()->has_name() &&
- connection->node()->name()->string_view() == "imu") {
- if (connection->has_monotonic_offset()) {
- monotonic_offset =
- std::chrono::nanoseconds(connection->monotonic_offset());
- } else {
- // If we don't have a monotonic offset, that means we aren't
- // connected, in which case we should break the loop but shouldn't
- // populate the offset.
- message_bridge_connected = false;
- }
- break;
- }
- }
- }
- if (!message_bridge_connected) {
- return;
- }
- aos::monotonic_clock::time_point capture_time(
- 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()) {
- state_at_capture = ekf_.OldestState();
- if (!state_at_capture.has_value()) {
- return;
- }
- }
-
- const Eigen::Vector3f Z{
- static_cast<float>(localizer_output_fetcher_->x()),
- static_cast<float>(localizer_output_fetcher_->y()),
- static_cast<float>(localizer_output_fetcher_->theta())};
- Eigen::Matrix3f R = Eigen::Matrix3f::Zero();
- R.diagonal() << 0.01, 0.01, 1e-4;
- const Input U_correct = ekf_.MostRecentInput();
- observations_.CorrectKnownH(Eigen::Vector3f::Zero(), &U_correct,
- Corrector(state_at_capture.value(), Z), R, now);
- }
-}
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2022
diff --git a/y2022/control_loops/drivetrain/localizer.h b/y2022/control_loops/drivetrain/localizer.h
deleted file mode 100644
index 77b29eb..0000000
--- a/y2022/control_loops/drivetrain/localizer.h
+++ /dev/null
@@ -1,107 +0,0 @@
-#ifndef Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
-#define Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
-
-#include <string_view>
-
-#include "aos/events/event_loop.h"
-#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"
-
-namespace y2022 {
-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
-// 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 {
- public:
- typedef frc971::control_loops::TypedPose<float> Pose;
- typedef frc971::control_loops::drivetrain::HybridEkf<float> HybridEkf;
- typedef typename HybridEkf::State State;
- typedef typename HybridEkf::StateIdx StateIdx;
- 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);
- frc971::control_loops::drivetrain::HybridEkf<double>::State Xhat()
- const override {
- return ekf_.X_hat().cast<double>();
- }
- frc971::control_loops::drivetrain::TrivialTargetSelector *target_selector()
- override {
- return &target_selector_;
- }
-
- void 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) override;
-
- void Reset(aos::monotonic_clock::time_point t,
- const frc971::control_loops::drivetrain::HybridEkf<double>::State
- &state) override;
-
- void ResetPosition(aos::monotonic_clock::time_point t, double x, double y,
- double theta, double /*theta_override*/,
- bool /*reset_theta*/) override {
- const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
- const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
- ekf_.ResetInitialState(t,
- (HybridEkf::State() << x, y, theta, left_encoder, 0,
- right_encoder, 0, 0, 0, 0, 0, 0)
- .finished(),
- ekf_.P());
- }
-
- private:
- class Corrector : public HybridEkf::ExpectedObservationFunctor {
- public:
- Corrector(const State &state_at_capture, const Eigen::Vector3f &Z)
- : state_at_capture_(state_at_capture), Z_(Z) {
- H_.setZero();
- H_(0, StateIdx::kX) = 1;
- H_(1, StateIdx::kY) = 1;
- H_(2, StateIdx::kTheta) = 1;
- }
- Output H(const State &, const Input &) final {
- Eigen::Vector3f error = H_ * state_at_capture_ - Z_;
- error(2) = aos::math::NormalizeAngle(error(2));
- return error;
- }
- Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> DHDX(
- const State &) final {
- return H_;
- }
-
- private:
- Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H_;
- State state_at_capture_;
- Eigen::Vector3f Z_;
- };
- aos::EventLoop *const event_loop_;
- const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
- HybridEkf ekf_;
- 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.
- frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
-};
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2022
-
-#endif // Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
diff --git a/y2022/control_loops/drivetrain/localizer_test.cc b/y2022/control_loops/drivetrain/localizer_test.cc
deleted file mode 100644
index 77f3988..0000000
--- a/y2022/control_loops/drivetrain/localizer_test.cc
+++ /dev/null
@@ -1,211 +0,0 @@
-#include "y2022/control_loops/drivetrain/localizer.h"
-
-#include <queue>
-
-#include "aos/events/logging/log_writer.h"
-#include "aos/network/message_bridge_server_generated.h"
-#include "aos/network/team_number.h"
-#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 "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 control_loops {
-namespace drivetrain {
-namespace testing {
-
-using frc971::control_loops::drivetrain::DrivetrainConfig;
-using frc971::control_loops::drivetrain::Goal;
-using frc971::control_loops::drivetrain::LocalizerControl;
-
-namespace {
-DrivetrainConfig<double> GetTest2022DrivetrainConfig() {
- DrivetrainConfig<double> config = GetDrivetrainConfig();
- return config;
-}
-} // namespace
-
-namespace chrono = std::chrono;
-using aos::monotonic_clock;
-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:
- LocalizedDrivetrainTest()
- : frc971::testing::ControlLoopTest(
- aos::configuration::ReadConfig(
- "y2022/control_loops/drivetrain/simulation_config.json"),
- GetTest2022DrivetrainConfig().dt),
- roborio_(aos::configuration::GetNode(configuration(), "roborio")),
- imu_(aos::configuration::GetNode(configuration(), "imu")),
- test_event_loop_(MakeEventLoop("test", roborio_)),
- imu_test_event_loop_(MakeEventLoop("test", imu_)),
- drivetrain_goal_sender_(
- test_event_loop_->MakeSender<Goal>("/drivetrain")),
- localizer_output_sender_(
- imu_test_event_loop_->MakeSender<frc971::controls::LocalizerOutput>(
- "/localizer")),
- drivetrain_goal_fetcher_(
- test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
- drivetrain_status_fetcher_(
- test_event_loop_
- ->MakeFetcher<frc971::control_loops::drivetrain::Status>(
- "/drivetrain")),
- localizer_control_sender_(
- test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
- drivetrain_event_loop_(MakeEventLoop("drivetrain", roborio_)),
- dt_config_(GetTest2022DrivetrainConfig()),
- localizer_(drivetrain_event_loop_.get(), dt_config_),
- drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
- drivetrain_plant_event_loop_(MakeEventLoop("plant", roborio_)),
- drivetrain_plant_imu_event_loop_(MakeEventLoop("plant", imu_)),
- drivetrain_plant_(drivetrain_plant_event_loop_.get(),
- drivetrain_plant_imu_event_loop_.get(), dt_config_,
- std::chrono::microseconds(500)) {
- FLAGS_die_on_malloc = true;
- set_team_id(frc971::control_loops::testing::kTeamNumber);
- set_battery_voltage(12.0);
-
- if (!FLAGS_output_folder.empty()) {
- logger_event_loop_ = MakeEventLoop("logger", roborio_);
- logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
- logger_->StartLoggingOnRun(FLAGS_output_folder);
- }
-
- test_event_loop_->OnRun([this]() { SetStartingPosition({3.0, 2.0, 0.0}); });
-
- imu_test_event_loop_
- ->AddTimer([this]() {
- auto builder = localizer_output_sender_.MakeBuilder();
- frc971::controls::LocalizerOutput::Builder output_builder =
- builder.MakeBuilder<frc971::controls::LocalizerOutput>();
- output_builder.add_monotonic_timestamp_ns(
- imu_test_event_loop_->monotonic_now().time_since_epoch().count());
- output_builder.add_x(drivetrain_plant_.state()(0));
- output_builder.add_y(drivetrain_plant_.state()(1));
- output_builder.add_theta(drivetrain_plant_.state()(2));
- builder.CheckOk(builder.Send(output_builder.Finish()));
- })
- ->Setup(imu_test_event_loop_->monotonic_now(),
- std::chrono::milliseconds(5));
- }
-
- virtual ~LocalizedDrivetrainTest() override {}
-
- 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;
- localizer_state.setZero();
- localizer_state.block<3, 1>(0, 0) = xytheta;
- localizer_.Reset(monotonic_now(), localizer_state);
- }
-
- void VerifyNearGoal(double eps = 1e-2) {
- drivetrain_goal_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
- drivetrain_plant_.GetLeftPosition(), eps);
- EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
- drivetrain_plant_.GetRightPosition(), eps);
- }
-
- ::testing::AssertionResult IsNear(double expected, double actual,
- double epsilon) {
- if (std::abs(expected - actual) < epsilon) {
- return ::testing::AssertionSuccess();
- } else {
- return ::testing::AssertionFailure()
- << "Expected " << expected << " but got " << actual
- << " with a max difference of " << epsilon
- << " and an actual difference of " << std::abs(expected - actual);
- }
- }
- ::testing::AssertionResult VerifyEstimatorAccurate(double eps) {
- const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
- ::testing::AssertionResult result(true);
- if (!(result = IsNear(localizer_.x(), true_state(0), eps))) {
- return result;
- }
- if (!(result = IsNear(localizer_.y(), true_state(1), eps))) {
- return result;
- }
- if (!(result = IsNear(localizer_.theta(), true_state(2), eps))) {
- return result;
- }
- if (!(result = IsNear(localizer_.left_velocity(), true_state(3), eps))) {
- return result;
- }
- if (!(result = IsNear(localizer_.right_velocity(), true_state(4), eps))) {
- return result;
- }
- return result;
- }
-
- const aos::Node *const roborio_;
- const aos::Node *const imu_;
-
- std::unique_ptr<aos::EventLoop> test_event_loop_;
- std::unique_ptr<aos::EventLoop> imu_test_event_loop_;
- aos::Sender<Goal> drivetrain_goal_sender_;
- aos::Sender<frc971::controls::LocalizerOutput> localizer_output_sender_;
- aos::Fetcher<Goal> drivetrain_goal_fetcher_;
- aos::Fetcher<frc971::control_loops::drivetrain::Status>
- drivetrain_status_fetcher_;
- aos::Sender<LocalizerControl> localizer_control_sender_;
-
- std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
- const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
-
- Localizer localizer_;
- DrivetrainLoop drivetrain_;
-
- std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
- std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_;
- DrivetrainSimulation drivetrain_plant_;
-
- void SendGoal(double left, double right) {
- auto builder = drivetrain_goal_sender_.MakeBuilder();
-
- Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
- drivetrain_builder.add_controller_type(
- frc971::control_loops::drivetrain::ControllerType::MOTION_PROFILE);
- drivetrain_builder.add_left_goal(left);
- drivetrain_builder.add_right_goal(right);
-
- EXPECT_EQ(builder.Send(drivetrain_builder.Finish()),
- aos::RawSender::Error::kOk);
- }
-
- private:
- std::unique_ptr<aos::EventLoop> logger_event_loop_;
- std::unique_ptr<aos::logger::Logger> logger_;
-};
-
-TEST_F(LocalizedDrivetrainTest, Nominal) {
- SetEnabled(true);
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-7));
-
- SendGoal(-1.0, 1.0);
-
- RunFor(chrono::seconds(10));
- VerifyNearGoal();
- EXPECT_TRUE(VerifyEstimatorAccurate(5e-3));
-}
-
-} // namespace testing
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2022
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 {