Create localizer log replay test
This creates a log replay test that confirms that the localizer can
sanely handle a situation where the drivebase's wheels are spinning. I
used the logfile when working on integrating the IMU into the EKF, and
figured it would be appropriate to test for regressions...
Change-Id: Ib7d5c5412d9e404e165ca99df41940235564b86e
diff --git a/WORKSPACE b/WORKSPACE
index fb6e2cb..f14f10b 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -641,6 +641,13 @@
urls = ["http://www.frc971.org/Build-Dependencies/small_sample_logfile.fbs"],
)
+http_file(
+ name = "drivetrain_replay",
+ downloaded_file_path = "spinning_wheels_while_still.bfbs",
+ sha256 = "3fa56d9af0852798bdd00ea5cc02f8261ad2a389a12a054ba619f9f7c43ab6fd",
+ urls = ["http://www.frc971.org/Build-Dependencies/spinning_wheels_while_still.bfbs"],
+)
+
# OpenCV armhf (for raspberry pi)
http_archive(
name = "opencv_armhf",
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index f7924c5..b766db8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -38,6 +38,8 @@
LocalizerInterface *localizer,
const ::std::string &name = "/drivetrain");
+ virtual ~DrivetrainLoop() {}
+
int ControllerIndexFromGears();
protected:
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 43462b8..6677145 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -40,6 +40,8 @@
typedef HybridEkf<double> Ekf;
typedef typename Ekf::StateIdx StateIdx;
+ virtual ~LocalizerInterface() {}
+
// Perform a single step of the filter, using the information that is
// available on every drivetrain iteration.
// The user should pass in the U that the real system experienced from the
@@ -136,6 +138,8 @@
target_selector_.set_has_target(false);
}
+ virtual ~DeadReckonEkf() {}
+
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
::aos::monotonic_clock::time_point now, double left_encoder,
double right_encoder, double gyro_rate,
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 41c6d81..de669d6 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -125,6 +125,27 @@
],
)
+cc_test(
+ name = "drivetrain_replay_test",
+ srcs = ["drivetrain_replay_test.cc"],
+ data = [
+ "//y2020:config.json",
+ "@drivetrain_replay//file:spinning_wheels_while_still.bfbs",
+ ],
+ deps = [
+ ":drivetrain_base",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:logger",
+ "//aos/testing:googletest",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
cc_binary(
name = "drivetrain_replay",
srcs = ["drivetrain_replay.cc"],
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
new file mode 100644
index 0000000..6933a51
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -0,0 +1,99 @@
+// This file serves to test replaying the localizer over existing logfiles to
+// check for regressions. Currently, it just uses a single logfile pulled from
+// running the 2016 robot against a wall and confirming that the X/Y estimate
+// does not change too much.
+//
+// Note that the current logfile test will break once we update the drivetrain
+// config for 2020, since both the gear ratios and IMU transformation wil no
+// longer be valid.
+// TODO(james): Do something about that when the time comes--could just copy
+// the existing drivetrain config into this file and use it directly.
+#include "gtest/gtest.h"
+
+#include "aos/configuration.h"
+#include "aos/events/logging/logger.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/network/team_number.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "gflags/gflags.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(
+ logfile, "external/drivetrain_replay/file/spinning_wheels_while_still.bfbs",
+ "Name of the logfile to read from.");
+DEFINE_string(config, "y2020/config.json",
+ "Name of the config file to replay using.");
+
+namespace y2020 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+class DrivetrainReplayTest : public ::testing::Test {
+ public:
+ DrivetrainReplayTest()
+ : config_(aos::configuration::ReadConfig(FLAGS_config)),
+ reader_(FLAGS_logfile, &config_.message()) {
+ aos::network::OverrideTeamNumber(971);
+
+ // TODO(james): Actually enforce not sending on the same buses as the
+ // logfile spews out.
+ reader_.RemapLoggedChannel("/drivetrain",
+ "frc971.control_loops.drivetrain.Status");
+ reader_.RemapLoggedChannel("/drivetrain",
+ "frc971.control_loops.drivetrain.Output");
+ reader_.Register();
+
+ drivetrain_event_loop_ =
+ reader_.event_loop_factory()->MakeEventLoop("drivetrain");
+ drivetrain_event_loop_->SkipTimingReport();
+
+ localizer_ =
+ std::make_unique<frc971::control_loops::drivetrain::DeadReckonEkf>(
+ drivetrain_event_loop_.get(), GetDrivetrainConfig());
+ drivetrain_ =
+ std::make_unique<frc971::control_loops::drivetrain::DrivetrainLoop>(
+ GetDrivetrainConfig(), drivetrain_event_loop_.get(),
+ localizer_.get());
+
+ test_event_loop_ =
+ reader_.event_loop_factory()->MakeEventLoop("drivetrain");
+ status_fetcher_ = test_event_loop_->MakeFetcher<
+ frc971::control_loops::drivetrain::Status>("/drivetrain");
+ }
+
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::logger::LogReader reader_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
+ std::unique_ptr<frc971::control_loops::drivetrain::DeadReckonEkf> localizer_;
+ std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop>
+ drivetrain_;
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status> status_fetcher_;
+};
+
+// Tests that we do a good job of trusting the IMU when the wheels are spinning
+// and the actual robot is not moving.
+TEST_F(DrivetrainReplayTest, SpinningWheels) {
+ reader_.event_loop_factory()->Run();
+
+ ASSERT_TRUE(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->has_x());
+ ASSERT_TRUE(status_fetcher_->has_y());
+ ASSERT_TRUE(status_fetcher_->has_theta());
+ EXPECT_LT(std::abs(status_fetcher_->x()), 0.1);
+ // Because the encoders should not be affecting the y or yaw axes, expect a
+ // reasonably precise result (although, since this is a real worl dtest, the
+ // robot probably did actually move be some non-zero amount).
+ EXPECT_LT(std::abs(status_fetcher_->y()), 0.05);
+ EXPECT_LT(std::abs(status_fetcher_->theta()), 0.02);
+}
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2020