Add more tests for 2020 localizer
Actually exercise the turret angle compensation code and generally
improve tests.
Change-Id: I11f75d83db4e49e9660f00c7b99ab32695544ab3
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 910cd31..3be86da 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -10,6 +10,7 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/control_loops/drivetrain/localizer.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
DEFINE_string(output_file, "",
"If set, logs all channels to the provided logfile.");
@@ -53,7 +54,7 @@
Eigen::Matrix<double, 4, 4> TurretRobotTransformation() {
Eigen::Matrix<double, 4, 4> H;
H.setIdentity();
- H.block<3, 1>(0, 3) << 1, 1, 1;
+ H.block<3, 1>(0, 3) << 1, 1.1, 0.9;
return H;
}
@@ -65,7 +66,7 @@
H.block<3, 1>(0, 3) << 0.1, 0, 0;
// Introduce a bit of pitch to make sure that we're exercising all the code.
H.block<3, 3>(0, 0) =
- Eigen::AngleAxis<double>(M_PI_2, Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxis<double>(0.1, Eigen::Vector3d::UnitY()) *
H.block<3, 3>(0, 0);
return H;
}
@@ -73,11 +74,15 @@
// The absolute target location to use. Not meant to correspond with a
// particular field target.
// TODO(james): Make more targets.
-Eigen::Matrix<double, 4, 4> TargetLocation() {
+std::vector<Eigen::Matrix<double, 4, 4>> TargetLocations() {
+ std::vector<Eigen::Matrix<double, 4, 4>> locations;
Eigen::Matrix<double, 4, 4> H;
H.setIdentity();
H.block<3, 1>(0, 3) << 10.0, 0, 0;
- return H;
+ locations.push_back(H);
+ H.block<3, 1>(0, 3) << -10.0, 0, 0;
+ locations.push_back(H);
+ return locations;
}
} // namespace
@@ -87,7 +92,7 @@
using frc971::control_loops::drivetrain::testing::GetTestDrivetrainConfig;
using aos::monotonic_clock;
-class LocalizedDrivetrainTest : public ::aos::testing::ControlLoopTest {
+class LocalizedDrivetrainTest : public aos::testing::ControlLoopTest {
protected:
// We must use the 2020 drivetrain config so that we don't have to deal
// with shifting:
@@ -103,6 +108,9 @@
test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
localizer_control_sender_(
test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
+ superstructure_status_sender_(
+ test_event_loop_->MakeSender<superstructure::Status>(
+ "/superstructure")),
drivetrain_event_loop_(MakeEventLoop("drivetrain")),
dt_config_(GetTest2020DrivetrainConfig()),
camera_sender_(
@@ -136,7 +144,25 @@
last_frame_ = monotonic_now();
}
}
- });
+ });
+
+ test_event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Also use the opportunity to send out turret messages.
+ UpdateTurretPosition();
+ auto builder = superstructure_status_sender_.MakeBuilder();
+ auto turret_builder =
+ builder
+ .MakeBuilder<frc971::control_loops::
+ PotAndAbsoluteEncoderProfiledJointStatus>();
+ turret_builder.add_position(turret_position_);
+ turret_builder.add_velocity(turret_velocity_);
+ auto turret_offset = turret_builder.Finish();
+ auto status_builder = builder.MakeBuilder<superstructure::Status>();
+ status_builder.add_turret(turret_offset);
+ builder.Send(status_builder.Finish());
+ },
+ chrono::milliseconds(5));
// Run for enough time to allow the gyro/imu zeroing code to run.
RunFor(std::chrono::seconds(10));
@@ -161,13 +187,36 @@
drivetrain_plant_.GetRightPosition(), eps);
}
- void VerifyEstimatorAccurate(double 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();
- EXPECT_NEAR(localizer_.x(), true_state(0, 0), eps);
- EXPECT_NEAR(localizer_.y(), true_state(1, 0), eps);
- EXPECT_NEAR(localizer_.theta(), true_state(2, 0), eps);
- EXPECT_NEAR(localizer_.left_velocity(), true_state(3, 0), eps);
- EXPECT_NEAR(localizer_.right_velocity(), true_state(4, 0), eps);
+ ::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;
}
// Goes through and captures frames on the camera(s), queueing them up to be
@@ -179,20 +228,26 @@
drivetrain_plant_.state()(2, 0));
std::unique_ptr<ImageMatchResultT> frame(new ImageMatchResultT());
- // TODO(james): Test with more than one (and no) target(s).
- {
+ for (const auto &H_target_field : TargetLocations()) {
std::unique_ptr<CameraPoseT> camera_target(new CameraPoseT());
camera_target->field_to_target.reset(new TransformationMatrixT());
- camera_target->field_to_target->data = MatrixToVector(TargetLocation());
+ camera_target->field_to_target->data = MatrixToVector(H_target_field);
+
+ Eigen::Matrix<double, 4, 4> H_camera_turret =
+ Eigen::Matrix<double, 4, 4>::Identity();
+ if (is_turreted_) {
+ H_camera_turret = frc971::control_loops::TransformationMatrixForYaw(
+ turret_position_) *
+ CameraTurretTransformation();
+ }
// TODO(james): Use non-zero turret angles.
camera_target->camera_to_target.reset(new TransformationMatrixT());
camera_target->camera_to_target->data = MatrixToVector(
(robot_pose.AsTransformationMatrix() * TurretRobotTransformation() *
- CameraTurretTransformation())
- .inverse() *
- TargetLocation());
+ H_camera_turret).inverse() *
+ H_target_field);
frame->camera_poses.emplace_back(std::move(camera_target));
}
@@ -209,7 +264,8 @@
frame->camera_calibration->fixed_extrinsics.get();
H_turret_robot->data = MatrixToVector(TurretRobotTransformation());
}
- {
+
+ if (is_turreted_) {
frame->camera_calibration->turret_extrinsics.reset(
new TransformationMatrixT());
TransformationMatrixT *H_camera_turret =
@@ -237,6 +293,7 @@
aos::Sender<Goal> drivetrain_goal_sender_;
aos::Fetcher<Goal> drivetrain_goal_fetcher_;
aos::Sender<LocalizerControl> localizer_control_sender_;
+ aos::Sender<superstructure::Status> superstructure_status_sender_;
std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
@@ -258,9 +315,44 @@
camera_delay_queue_;
void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
+ void set_camera_is_turreted(bool turreted) { is_turreted_ = turreted; }
+
+ void set_turret(double position, double velocity) {
+ turret_position_ = position;
+ turret_velocity_ = velocity;
+ }
+
+ 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_TRUE(builder.Send(drivetrain_builder.Finish()));
+ }
private:
+ void UpdateTurretPosition() {
+ turret_position_ +=
+ turret_velocity_ *
+ aos::time::DurationInSeconds(monotonic_now() - last_turret_update_);
+ last_turret_update_ = monotonic_now();
+ }
+
bool enable_cameras_ = false;
+ // Whether to make the camera be on the turret or not.
+ bool is_turreted_ = true;
+
+ // The time at which we last incremented turret_position_.
+ monotonic_clock::time_point last_turret_update_ = monotonic_clock::min_time;
+ // Current turret position and velocity. These are set directly by the user in
+ // the test, and if velocity is non-zero, then we will automatically increment
+ // turret_position_ with every timestep.
+ double turret_position_ = 0.0; // rad
+ double turret_velocity_ = 0.0; // rad / sec
std::unique_ptr<aos::EventLoop> logger_event_loop_;
std::unique_ptr<aos::logger::DetachedBufferWriter> log_buffer_writer_;
@@ -272,61 +364,131 @@
TEST_F(LocalizedDrivetrainTest, NoCameraUpdate) {
SetEnabled(true);
set_enable_cameras(false);
- VerifyEstimatorAccurate(1e-7);
- {
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-7));
- Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
- drivetrain_builder.add_controller_type(
- frc971::control_loops::drivetrain::ControllerType::MOTION_PROFILE);
- drivetrain_builder.add_left_goal(-1.0);
- drivetrain_builder.add_right_goal(1.0);
+ SendGoal(-1.0, 1.0);
- EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
- }
RunFor(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate(5e-4);
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-4));
}
-// Tests that camera udpates with a perfect models results in no errors.
+// Tests that camera updates with a perfect models results in no errors.
TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
SetEnabled(true);
set_enable_cameras(true);
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ set_camera_is_turreted(true);
- Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
- drivetrain_builder.add_controller_type(
- frc971::control_loops::drivetrain::ControllerType::MOTION_PROFILE);
- drivetrain_builder.add_left_goal(-1.0);
- drivetrain_builder.add_right_goal(1.0);
+ SendGoal(-1.0, 1.0);
- EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
RunFor(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate(5e-4);
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-4));
}
-// Tests that camera udpates with a constant initial error in the position
+// Tests that camera updates with a constant initial error in the position
// results in convergence.
TEST_F(LocalizedDrivetrainTest, InitialPositionError) {
SetEnabled(true);
set_enable_cameras(true);
+ set_camera_is_turreted(true);
drivetrain_plant_.mutable_state()->topRows(3) +=
Eigen::Vector3d(0.1, 0.1, 0.01);
- 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(-1.0);
- drivetrain_builder.add_right_goal(1.0);
+ // Confirm that some translational movement does get handled correctly.
+ SendGoal(-0.9, 1.0);
- EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
// Give the filters enough time to converge.
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-3);
- VerifyEstimatorAccurate(1e-2);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Tests that camera updates using a non-turreted camera work.
+TEST_F(LocalizedDrivetrainTest, InitialPositionErrorNoTurret) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(false);
+ drivetrain_plant_.mutable_state()->topRows(3) +=
+ Eigen::Vector3d(0.1, 0.1, 0.01);
+
+ SendGoal(-1.0, 1.0);
+
+ // Give the filters enough time to converge.
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Tests that we are able to handle a constant, non-zero turret angle.
+TEST_F(LocalizedDrivetrainTest, NonZeroTurret) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+ set_turret(1.0, 0.0);
+ drivetrain_plant_.mutable_state()->topRows(3) +=
+ Eigen::Vector3d(0.1, 0.1, 0.0);
+
+ SendGoal(-1.0, 1.0);
+
+ // Give the filters enough time to converge.
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+}
+
+// Tests that we are able to handle a constant velocity turret.
+TEST_F(LocalizedDrivetrainTest, MovingTurret) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+ set_turret(0.0, 0.2);
+ drivetrain_plant_.mutable_state()->topRows(3) +=
+ Eigen::Vector3d(0.1, 0.1, 0.0);
+
+ SendGoal(-1.0, 1.0);
+
+ // Give the filters enough time to converge.
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+}
+
+// Tests that we reject camera measurements when the turret is spinning too
+// fast.
+TEST_F(LocalizedDrivetrainTest, TooFastTurret) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+ set_turret(0.0, -10.0);
+ const Eigen::Vector3d disturbance(0.1, 0.1, 0.0);
+ drivetrain_plant_.mutable_state()->topRows(3) += disturbance;
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(10));
+ EXPECT_FALSE(VerifyEstimatorAccurate(1e-3));
+ // If we remove the disturbance, we should now be correct.
+ drivetrain_plant_.mutable_state()->topRows(3) -= disturbance;
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+}
+
+// Tests that we don't reject camera measurements when the turret is spinning
+// too fast but we aren't using a camera attached to the turret.
+TEST_F(LocalizedDrivetrainTest, TooFastTurretDoesntAffectFixedCamera) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(false);
+ set_turret(0.0, -10.0);
+ const Eigen::Vector3d disturbance(0.1, 0.1, 0.0);
+ drivetrain_plant_.mutable_state()->topRows(3) += disturbance;
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
}
} // namespace testing