| #include "y2022/localizer/localizer.h" |
| |
| #include "absl/flags/flag.h" |
| #include "gtest/gtest.h" |
| |
| #include "aos/events/logging/log_writer.h" |
| #include "aos/events/simulated_event_loop.h" |
| #include "frc971/control_loops/drivetrain/drivetrain_test_lib.h" |
| #include "frc971/control_loops/pose.h" |
| #include "y2022/control_loops/drivetrain/drivetrain_base.h" |
| #include "y2022/control_loops/superstructure/superstructure_status_generated.h" |
| #include "y2022/vision/target_estimate_generated.h" |
| |
| ABSL_FLAG(std::string, output_folder, "", |
| "If set, logs all channels to the provided logfile."); |
| |
| namespace frc971::controls::testing { |
| typedef ModelBasedLocalizer::ModelState ModelState; |
| typedef ModelBasedLocalizer::AccelState AccelState; |
| typedef ModelBasedLocalizer::ModelInput ModelInput; |
| typedef ModelBasedLocalizer::AccelInput AccelInput; |
| |
| using frc971::control_loops::Pose; |
| using frc971::control_loops::drivetrain::DrivetrainConfig; |
| using frc971::control_loops::drivetrain::LocalizerControl; |
| using frc971::vision::calibration::CameraCalibrationT; |
| using frc971::vision::calibration::TransformationMatrixT; |
| using y2022::vision::TargetEstimate; |
| using y2022::vision::TargetEstimateT; |
| |
| namespace { |
| constexpr size_t kX = ModelBasedLocalizer::kX; |
| constexpr size_t kY = ModelBasedLocalizer::kY; |
| constexpr size_t kTheta = ModelBasedLocalizer::kTheta; |
| constexpr size_t kVelocityX = ModelBasedLocalizer::kVelocityX; |
| constexpr size_t kVelocityY = ModelBasedLocalizer::kVelocityY; |
| constexpr size_t kAccelX = ModelBasedLocalizer::kAccelX; |
| constexpr size_t kAccelY = ModelBasedLocalizer::kAccelY; |
| constexpr size_t kThetaRate = ModelBasedLocalizer::kThetaRate; |
| constexpr size_t kLeftEncoder = ModelBasedLocalizer::kLeftEncoder; |
| constexpr size_t kLeftVelocity = ModelBasedLocalizer::kLeftVelocity; |
| constexpr size_t kLeftVoltageError = ModelBasedLocalizer::kLeftVoltageError; |
| constexpr size_t kRightEncoder = ModelBasedLocalizer::kRightEncoder; |
| constexpr size_t kRightVelocity = ModelBasedLocalizer::kRightVelocity; |
| constexpr size_t kRightVoltageError = ModelBasedLocalizer::kRightVoltageError; |
| constexpr size_t kLeftVoltage = ModelBasedLocalizer::kLeftVoltage; |
| constexpr size_t kRightVoltage = ModelBasedLocalizer::kRightVoltage; |
| |
| Eigen::Matrix<double, 4, 4> TurretRobotTransformation() { |
| Eigen::Matrix<double, 4, 4> H; |
| H.setIdentity(); |
| H.block<3, 1>(0, 3) << 1, 1.1, 0.9; |
| return H; |
| } |
| |
| // Provides the location of the camera on the turret. |
| Eigen::Matrix<double, 4, 4> CameraTurretTransformation() { |
| Eigen::Matrix<double, 4, 4> H; |
| H.setIdentity(); |
| H.block<3, 1>(0, 3) << 0.1, 0, 0; |
| H.block<3, 3>(0, 0) << 0, 0, 1, -1, 0, 0, 0, -1, 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>(0.1, Eigen::Vector3d::UnitY()) * |
| H.block<3, 3>(0, 0); |
| return H; |
| } |
| |
| // Copies an Eigen matrix into a row-major vector of the data. |
| std::vector<float> MatrixToVector(const Eigen::Matrix<double, 4, 4> &H) { |
| std::vector<float> data; |
| for (int row = 0; row < 4; ++row) { |
| for (int col = 0; col < 4; ++col) { |
| data.push_back(H(row, col)); |
| } |
| } |
| return data; |
| } |
| |
| DrivetrainConfig<double> GetTest2022DrivetrainConfig() { |
| DrivetrainConfig<double> config = |
| y2022::control_loops::drivetrain::GetDrivetrainConfig(); |
| config.is_simulated = true; |
| return config; |
| } |
| } // namespace |
| |
| class LocalizerTest : public ::testing::Test { |
| protected: |
| LocalizerTest() |
| : dt_config_(GetTest2022DrivetrainConfig()), localizer_(dt_config_) { |
| localizer_.set_longitudinal_offset(0.0); |
| } |
| ModelState CallDiffModel(const ModelState &state, const ModelInput &U) { |
| return localizer_.DiffModel(state, U); |
| } |
| |
| AccelState CallDiffAccel(const AccelState &state, const AccelInput &U) { |
| return localizer_.DiffAccel(state, U); |
| } |
| |
| const control_loops::drivetrain::DrivetrainConfig<double> dt_config_; |
| ModelBasedLocalizer localizer_; |
| }; |
| |
| TEST_F(LocalizerTest, AccelIntegrationTest) { |
| AccelState state; |
| state.setZero(); |
| AccelInput input; |
| input.setZero(); |
| |
| EXPECT_EQ(0.0, CallDiffAccel(state, input).norm()); |
| // Non-zero x/y/theta states should still result in a zero derivative. |
| state(kX) = 1.0; |
| state(kY) = 1.0; |
| state(kTheta) = 1.0; |
| EXPECT_EQ(0.0, CallDiffAccel(state, input).norm()); |
| |
| state.setZero(); |
| state(kVelocityX) = 1.0; |
| state(kVelocityY) = 2.0; |
| EXPECT_EQ((AccelState() << 1.0, 2.0, 0.0, 0.0, 0.0).finished(), |
| CallDiffAccel(state, input)); |
| // Derivatives should be independent of theta. |
| state(kTheta) = M_PI / 2.0; |
| EXPECT_EQ((AccelState() << 1.0, 2.0, 0.0, 0.0, 0.0).finished(), |
| CallDiffAccel(state, input)); |
| |
| state.setZero(); |
| input(kAccelX) = 1.0; |
| input(kAccelY) = 2.0; |
| input(kThetaRate) = 3.0; |
| EXPECT_EQ((AccelState() << 0.0, 0.0, 3.0, 1.0, 2.0).finished(), |
| CallDiffAccel(state, input)); |
| state(kTheta) = M_PI / 2.0; |
| EXPECT_EQ((AccelState() << 0.0, 0.0, 3.0, 1.0, 2.0).finished(), |
| CallDiffAccel(state, input)); |
| } |
| |
| TEST_F(LocalizerTest, ModelIntegrationTest) { |
| ModelState state; |
| state.setZero(); |
| ModelInput input; |
| input.setZero(); |
| ModelState diff; |
| |
| EXPECT_EQ(0.0, CallDiffModel(state, input).norm()); |
| // Non-zero x/y/theta/encoder states should still result in a zero derivative. |
| state(kX) = 1.0; |
| state(kY) = 1.0; |
| state(kTheta) = 1.0; |
| state(kLeftEncoder) = 1.0; |
| state(kRightEncoder) = 1.0; |
| EXPECT_EQ(0.0, CallDiffModel(state, input).norm()); |
| |
| state.setZero(); |
| state(kLeftVelocity) = 1.0; |
| state(kRightVelocity) = 1.0; |
| diff = CallDiffModel(state, input); |
| const ModelState mask_velocities = |
| (ModelState() << 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0).finished(); |
| EXPECT_EQ( |
| (ModelState() << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(), |
| diff.cwiseProduct(mask_velocities)); |
| EXPECT_EQ(diff(kLeftVelocity), diff(kRightVelocity)); |
| EXPECT_GT(0.0, diff(kLeftVelocity)); |
| state(kTheta) = M_PI / 2.0; |
| diff = CallDiffModel(state, input); |
| EXPECT_NEAR(0.0, |
| ((ModelState() << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0) |
| .finished() - |
| diff.cwiseProduct(mask_velocities)) |
| .norm(), |
| 1e-12); |
| EXPECT_EQ(diff(kLeftVelocity), diff(kRightVelocity)); |
| EXPECT_GT(0.0, diff(kLeftVelocity)); |
| |
| state.setZero(); |
| state(kLeftVelocity) = -1.0; |
| state(kRightVelocity) = 1.0; |
| diff = CallDiffModel(state, input); |
| EXPECT_EQ((ModelState() << 0.0, 0.0, 1.0 / dt_config_.robot_radius, -1.0, 0.0, |
| 0.0, 1.0, 0.0, 0.0) |
| .finished(), |
| diff.cwiseProduct(mask_velocities)); |
| EXPECT_EQ(-diff(kLeftVelocity), diff(kRightVelocity)); |
| EXPECT_LT(0.0, diff(kLeftVelocity)); |
| |
| state.setZero(); |
| input(kLeftVoltage) = 5.0; |
| input(kRightVoltage) = 6.0; |
| diff = CallDiffModel(state, input); |
| EXPECT_EQ(0, diff(kX)); |
| EXPECT_EQ(0, diff(kY)); |
| EXPECT_EQ(0, diff(kTheta)); |
| EXPECT_EQ(0, diff(kLeftEncoder)); |
| EXPECT_EQ(0, diff(kRightEncoder)); |
| EXPECT_EQ(0, diff(kLeftVoltageError)); |
| EXPECT_EQ(0, diff(kRightVoltageError)); |
| EXPECT_LT(0, diff(kLeftVelocity)); |
| EXPECT_LT(0, diff(kRightVelocity)); |
| EXPECT_LT(diff(kLeftVelocity), diff(kRightVelocity)); |
| |
| state.setZero(); |
| state(kLeftVoltageError) = -1.0; |
| state(kRightVoltageError) = -2.0; |
| input(kLeftVoltage) = 1.0; |
| input(kRightVoltage) = 2.0; |
| EXPECT_EQ(ModelState::Zero(), CallDiffModel(state, input)); |
| } |
| |
| // Test that the HandleReset does indeed reset the state of the localizer. |
| TEST_F(LocalizerTest, LocalizerReset) { |
| aos::monotonic_clock::time_point t = aos::monotonic_clock::epoch(); |
| localizer_.HandleReset(t, {1.0, 2.0, 3.0}); |
| EXPECT_EQ((Eigen::Vector3d{1.0, 2.0, 3.0}), localizer_.xytheta()); |
| localizer_.HandleReset(t, {4.0, 5.0, 6.0}); |
| EXPECT_EQ((Eigen::Vector3d{4.0, 5.0, 6.0}), localizer_.xytheta()); |
| } |
| |
| // Test that if we are moving only by accelerometer readings (and just assuming |
| // zero voltage/encoders) that we initially don't believe it but then latch into |
| // following the accelerometer. |
| // Note: this test is somewhat sensitive to the exact tuning values used for the |
| // filter. |
| TEST_F(LocalizerTest, AccelOnly) { |
| const aos::monotonic_clock::time_point start = aos::monotonic_clock::epoch(); |
| const std::chrono::microseconds kDt{500}; |
| aos::monotonic_clock::time_point t = start - std::chrono::milliseconds(1000); |
| Eigen::Vector3d gyro{0.0, 0.0, 0.0}; |
| const Eigen::Vector2d encoders{0.0, 0.0}; |
| const Eigen::Vector2d voltages{0.0, 0.0}; |
| Eigen::Vector3d accel{5.0, 2.0, 9.80665}; |
| Eigen::Vector3d accel_gs = |
| dt_config_.imu_transform.inverse() * accel / 9.80665; |
| while (t < start) { |
| // Spin to fill up the buffer. |
| localizer_.HandleImu(t, gyro, Eigen::Vector3d::UnitZ(), encoders, voltages); |
| t += kDt; |
| } |
| while (t < start + std::chrono::milliseconds(100)) { |
| localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages); |
| EXPECT_EQ(Eigen::Vector3d::Zero(), localizer_.xytheta()); |
| t += kDt; |
| } |
| while (t < start + std::chrono::milliseconds(500)) { |
| // Too lazy to hard-code when the transition happens. |
| localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages); |
| t += kDt; |
| } |
| while (t < start + std::chrono::milliseconds(1000)) { |
| SCOPED_TRACE(t); |
| localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages); |
| const Eigen::Vector3d xytheta = localizer_.xytheta(); |
| t += kDt; |
| EXPECT_NEAR( |
| 0.5 * accel(0) * std::pow(aos::time::DurationInSeconds(t - start), 2), |
| xytheta(0), 1e-4); |
| EXPECT_NEAR( |
| 0.5 * accel(1) * std::pow(aos::time::DurationInSeconds(t - start), 2), |
| xytheta(1), 1e-4); |
| EXPECT_EQ(0.0, xytheta(2)); |
| } |
| |
| ASSERT_NEAR(accel(0), localizer_.accel_state()(kVelocityX), 1e-10); |
| ASSERT_NEAR(accel(1), localizer_.accel_state()(kVelocityY), 1e-10); |
| |
| // Start going in a cirlce, and confirm that we |
| // handle things correctly. We rotate the accelerometer readings by 90 degrees |
| // and then leave them constant, which should make it look like we are going |
| // around in a circle. |
| accel = Eigen::Vector3d{-accel(1), accel(0), 9.80665}; |
| accel_gs = dt_config_.imu_transform.inverse() * accel / 9.80665; |
| // v^2 / r = a |
| // w * r = v |
| // v^2 / v * w = a |
| // w = a / v |
| const double omega = accel.topRows<2>().norm() / |
| std::hypot(localizer_.accel_state()(kVelocityX), |
| localizer_.accel_state()(kVelocityY)); |
| gyro << 0.0, 0.0, omega; |
| // Due to the magic of math, omega works out to be 1.0 after having run at the |
| // acceleration for one second. |
| ASSERT_NEAR(1.0, omega, 1e-10); |
| // Yes, we could save some operations here, but let's be at least somewhat |
| // clear about what we're doing... |
| const double radius = accel.topRows<2>().norm() / (omega * omega); |
| const Eigen::Vector2d center = localizer_.xytheta().topRows<2>() + |
| accel.topRows<2>().normalized() * radius; |
| const double initial_theta = std::atan2(-accel(1), -accel(0)); |
| |
| std::chrono::microseconds one_revolution_time( |
| static_cast<int>(2 * M_PI / omega * 1e6)); |
| |
| aos::monotonic_clock::time_point circle_start = t; |
| |
| while (t < circle_start + one_revolution_time) { |
| SCOPED_TRACE(t); |
| localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages); |
| t += kDt; |
| const double t_circle = aos::time::DurationInSeconds(t - circle_start); |
| ASSERT_NEAR(t_circle * omega, localizer_.xytheta()(2), 1e-5); |
| const double theta_circle = t_circle * omega + initial_theta; |
| const Eigen::Vector2d offset = |
| radius * |
| Eigen::Vector2d{std::cos(theta_circle), std::sin(theta_circle)}; |
| const Eigen::Vector2d expected = center + offset; |
| const Eigen::Vector2d estimated = localizer_.xytheta().topRows<2>(); |
| const Eigen::Vector2d implied_offset = estimated - center; |
| const double implied_theta = |
| std::atan2(implied_offset.y(), implied_offset.x()); |
| VLOG(1) << "center: " << center.transpose() << " radius " << radius |
| << "\nlocalizer " << localizer_.xytheta().transpose() |
| << " t_circle " << t_circle << " omega " << omega << " theta " |
| << theta_circle << "\noffset " << offset.transpose() |
| << "\nexpected " << expected.transpose() << "\nimplied offset " |
| << implied_offset << " implied_theta " << implied_theta << "\nvel " |
| << localizer_.accel_state()(kVelocityX) << ", " |
| << localizer_.accel_state()(kVelocityY); |
| ASSERT_NEAR(0.0, (expected - localizer_.xytheta().topRows<2>()).norm(), |
| 1e-2); |
| } |
| |
| // Set accelerometer back to zero and confirm that we recover (the |
| // implementation decays the accelerometer speeds to zero when still, so |
| // should recover). |
| while (t < |
| circle_start + one_revolution_time + std::chrono::milliseconds(3000)) { |
| localizer_.HandleImu(t, Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ(), |
| encoders, voltages); |
| t += kDt; |
| } |
| const Eigen::Vector3d final_pos = localizer_.xytheta(); |
| localizer_.HandleImu(t, Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ(), |
| encoders, voltages); |
| ASSERT_NEAR(0.0, (final_pos - localizer_.xytheta()).norm(), 1e-10); |
| } |
| |
| using control_loops::drivetrain::Output; |
| |
| class EventLoopLocalizerTest : public ::testing::Test { |
| protected: |
| EventLoopLocalizerTest() |
| : configuration_(aos::configuration::ReadConfig("y2022/aos_config.json")), |
| event_loop_factory_(&configuration_.message()), |
| roborio_node_( |
| aos::configuration::GetNode(&configuration_.message(), "roborio")), |
| imu_node_( |
| aos::configuration::GetNode(&configuration_.message(), "imu")), |
| camera_node_( |
| aos::configuration::GetNode(&configuration_.message(), "pi1")), |
| dt_config_( |
| control_loops::drivetrain::testing::GetTestDrivetrainConfig()), |
| localizer_event_loop_( |
| event_loop_factory_.MakeEventLoop("localizer", imu_node_)), |
| localizer_(localizer_event_loop_.get(), dt_config_), |
| drivetrain_plant_event_loop_(event_loop_factory_.MakeEventLoop( |
| "drivetrain_plant", roborio_node_)), |
| drivetrain_plant_imu_event_loop_( |
| event_loop_factory_.MakeEventLoop("drivetrain_plant", imu_node_)), |
| drivetrain_plant_(drivetrain_plant_event_loop_.get(), |
| drivetrain_plant_imu_event_loop_.get(), dt_config_, |
| std::chrono::microseconds(500)), |
| roborio_test_event_loop_( |
| event_loop_factory_.MakeEventLoop("test", roborio_node_)), |
| imu_test_event_loop_( |
| event_loop_factory_.MakeEventLoop("test", imu_node_)), |
| camera_test_event_loop_( |
| event_loop_factory_.MakeEventLoop("test", camera_node_)), |
| logger_test_event_loop_( |
| event_loop_factory_.GetNodeEventLoopFactory("logger") |
| ->MakeEventLoop("test")), |
| output_sender_( |
| roborio_test_event_loop_->MakeSender<Output>("/drivetrain")), |
| turret_sender_( |
| roborio_test_event_loop_ |
| ->MakeSender<y2022::control_loops::superstructure::Status>( |
| "/superstructure")), |
| target_sender_( |
| camera_test_event_loop_->MakeSender<y2022::vision::TargetEstimate>( |
| "/camera")), |
| control_sender_(roborio_test_event_loop_->MakeSender<LocalizerControl>( |
| "/drivetrain")), |
| output_fetcher_(roborio_test_event_loop_->MakeFetcher<LocalizerOutput>( |
| "/localizer")), |
| status_fetcher_( |
| imu_test_event_loop_->MakeFetcher<LocalizerStatus>("/localizer")) { |
| localizer_.localizer()->set_longitudinal_offset(0.0); |
| { |
| aos::TimerHandler *timer = roborio_test_event_loop_->AddTimer([this]() { |
| { |
| auto builder = output_sender_.MakeBuilder(); |
| auto output_builder = builder.MakeBuilder<Output>(); |
| output_builder.add_left_voltage(output_voltages_(0)); |
| output_builder.add_right_voltage(output_voltages_(1)); |
| builder.CheckOk(builder.Send(output_builder.Finish())); |
| } |
| { |
| auto builder = turret_sender_.MakeBuilder(); |
| auto turret_estimator_builder = |
| builder |
| .MakeBuilder<frc971::PotAndAbsoluteEncoderEstimatorState>(); |
| turret_estimator_builder.add_position(turret_position_); |
| const flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState> |
| turret_estimator_offset = turret_estimator_builder.Finish(); |
| auto turret_builder = |
| builder |
| .MakeBuilder<frc971::control_loops:: |
| PotAndAbsoluteEncoderProfiledJointStatus>(); |
| turret_builder.add_position(turret_position_); |
| turret_builder.add_velocity(turret_velocity_); |
| turret_builder.add_zeroed(true); |
| turret_builder.add_estimator_state(turret_estimator_offset); |
| const auto turret_offset = turret_builder.Finish(); |
| auto status_builder = |
| builder |
| .MakeBuilder<y2022::control_loops::superstructure::Status>(); |
| status_builder.add_turret(turret_offset); |
| builder.CheckOk(builder.Send(status_builder.Finish())); |
| } |
| }); |
| roborio_test_event_loop_->OnRun([timer, this]() { |
| timer->Schedule(roborio_test_event_loop_->monotonic_now(), |
| std::chrono::milliseconds(5)); |
| }); |
| } |
| { |
| aos::TimerHandler *timer = camera_test_event_loop_->AddTimer([this]() { |
| if (!send_targets_) { |
| return; |
| } |
| const frc971::control_loops::Pose robot_pose( |
| {drivetrain_plant_.GetPosition().x(), |
| drivetrain_plant_.GetPosition().y(), 0.0}, |
| drivetrain_plant_.state()(2, 0)); |
| const Eigen::Matrix<double, 4, 4> H_turret_camera = |
| frc971::control_loops::TransformationMatrixForYaw( |
| turret_position_) * |
| CameraTurretTransformation(); |
| |
| const Eigen::Matrix<double, 4, 4> H_field_camera = |
| robot_pose.AsTransformationMatrix() * TurretRobotTransformation() * |
| H_turret_camera; |
| const Eigen::Matrix<double, 4, 4> target_transform = |
| Eigen::Matrix<double, 4, 4>::Identity(); |
| const Eigen::Matrix<double, 4, 4> H_camera_target = |
| H_field_camera.inverse() * target_transform; |
| const Eigen::Matrix<double, 4, 4> H_target_camera = |
| H_camera_target.inverse(); |
| |
| std::unique_ptr<y2022::vision::TargetEstimateT> estimate( |
| new y2022::vision::TargetEstimateT()); |
| estimate->distance = H_target_camera.block<2, 1>(0, 3).norm(); |
| estimate->angle_to_target = |
| std::atan2(-H_camera_target(0, 3), H_camera_target(2, 3)); |
| estimate->camera_calibration.reset(new CameraCalibrationT()); |
| { |
| estimate->camera_calibration->fixed_extrinsics.reset( |
| new TransformationMatrixT()); |
| TransformationMatrixT *H_robot_turret = |
| estimate->camera_calibration->fixed_extrinsics.get(); |
| H_robot_turret->data = MatrixToVector(TurretRobotTransformation()); |
| } |
| |
| estimate->camera_calibration->turret_extrinsics.reset( |
| new TransformationMatrixT()); |
| estimate->camera_calibration->turret_extrinsics->data = |
| MatrixToVector(CameraTurretTransformation()); |
| |
| estimate->confidence = 1.0; |
| |
| auto builder = target_sender_.MakeBuilder(); |
| builder.CheckOk( |
| builder.Send(TargetEstimate::Pack(*builder.fbb(), estimate.get()))); |
| }); |
| camera_test_event_loop_->OnRun([timer, this]() { |
| timer->Schedule(camera_test_event_loop_->monotonic_now(), |
| std::chrono::milliseconds(50)); |
| }); |
| } |
| |
| localizer_control_send_timer_ = |
| roborio_test_event_loop_->AddTimer([this]() { |
| auto builder = control_sender_.MakeBuilder(); |
| auto control_builder = builder.MakeBuilder<LocalizerControl>(); |
| control_builder.add_x(localizer_control_x_); |
| control_builder.add_y(localizer_control_y_); |
| control_builder.add_theta(localizer_control_theta_); |
| control_builder.add_theta_uncertainty(0.01); |
| control_builder.add_keep_current_theta(false); |
| builder.CheckOk(builder.Send(control_builder.Finish())); |
| }); |
| |
| // Get things zeroed. |
| event_loop_factory_.RunFor(std::chrono::seconds(10)); |
| CHECK(status_fetcher_.Fetch()); |
| CHECK(status_fetcher_->zeroed()); |
| |
| if (!absl::GetFlag(FLAGS_output_folder).empty()) { |
| logger_event_loop_ = |
| event_loop_factory_.MakeEventLoop("logger", imu_node_); |
| logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get()); |
| logger_->StartLoggingOnRun(absl::GetFlag(FLAGS_output_folder)); |
| } |
| } |
| |
| void SendLocalizerControl(double x, double y, double theta) { |
| localizer_control_x_ = x; |
| localizer_control_y_ = y; |
| localizer_control_theta_ = theta; |
| localizer_control_send_timer_->Schedule( |
| roborio_test_event_loop_->monotonic_now()); |
| } |
| ::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); |
| status_fetcher_.Fetch(); |
| if (!(result = IsNear(status_fetcher_->model_based()->x(), true_state(0), |
| eps))) { |
| return result; |
| } |
| if (!(result = IsNear(status_fetcher_->model_based()->y(), true_state(1), |
| eps))) { |
| return result; |
| } |
| if (!(result = IsNear(status_fetcher_->model_based()->theta(), |
| true_state(2), eps))) { |
| return result; |
| } |
| return result; |
| } |
| |
| aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_; |
| aos::SimulatedEventLoopFactory event_loop_factory_; |
| const aos::Node *const roborio_node_; |
| const aos::Node *const imu_node_; |
| const aos::Node *const camera_node_; |
| const control_loops::drivetrain::DrivetrainConfig<double> dt_config_; |
| std::unique_ptr<aos::EventLoop> localizer_event_loop_; |
| EventLoopLocalizer localizer_; |
| |
| std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_; |
| std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_; |
| control_loops::drivetrain::testing::DrivetrainSimulation drivetrain_plant_; |
| |
| std::unique_ptr<aos::EventLoop> roborio_test_event_loop_; |
| std::unique_ptr<aos::EventLoop> imu_test_event_loop_; |
| std::unique_ptr<aos::EventLoop> camera_test_event_loop_; |
| std::unique_ptr<aos::EventLoop> logger_test_event_loop_; |
| |
| aos::Sender<Output> output_sender_; |
| aos::Sender<y2022::control_loops::superstructure::Status> turret_sender_; |
| aos::Sender<y2022::vision::TargetEstimate> target_sender_; |
| aos::Sender<LocalizerControl> control_sender_; |
| aos::Fetcher<LocalizerOutput> output_fetcher_; |
| aos::Fetcher<LocalizerStatus> status_fetcher_; |
| |
| Eigen::Vector2d output_voltages_ = Eigen::Vector2d::Zero(); |
| |
| aos::TimerHandler *localizer_control_send_timer_; |
| |
| bool send_targets_ = false; |
| double turret_position_ = 0.0; |
| double turret_velocity_ = 0.0; |
| |
| double localizer_control_x_ = 0.0; |
| double localizer_control_y_ = 0.0; |
| double localizer_control_theta_ = 0.0; |
| |
| std::unique_ptr<aos::EventLoop> logger_event_loop_; |
| std::unique_ptr<aos::logger::Logger> logger_; |
| }; |
| |
| TEST_F(EventLoopLocalizerTest, Nominal) { |
| output_voltages_ << 1.0, 1.0; |
| event_loop_factory_.RunFor(std::chrono::seconds(2)); |
| drivetrain_plant_.set_accel_sin_magnitude(0.01); |
| CHECK(output_fetcher_.Fetch()); |
| CHECK(status_fetcher_.Fetch()); |
| // The two can be different because they may've been sent at different times. |
| ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6); |
| ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6); |
| ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(), |
| 1e-6); |
| ASSERT_LT(0.1, output_fetcher_->x()); |
| ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10); |
| ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-10); |
| ASSERT_TRUE(status_fetcher_->has_model_based()); |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| ASSERT_LT(0.1, status_fetcher_->model_based()->accel_state()->velocity_x()); |
| ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(), |
| 1e-10); |
| ASSERT_NEAR( |
| 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(), |
| 1e-1); |
| ASSERT_NEAR( |
| 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(), |
| 1e-1); |
| } |
| |
| TEST_F(EventLoopLocalizerTest, Reverse) { |
| output_voltages_ << -4.0, -4.0; |
| drivetrain_plant_.set_accel_sin_magnitude(0.01); |
| event_loop_factory_.RunFor(std::chrono::seconds(2)); |
| CHECK(output_fetcher_.Fetch()); |
| CHECK(status_fetcher_.Fetch()); |
| // The two can be different because they may've been sent at different times. |
| ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6); |
| ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6); |
| ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(), |
| 1e-6); |
| ASSERT_GT(-0.1, output_fetcher_->x()); |
| ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10); |
| ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-10); |
| ASSERT_TRUE(status_fetcher_->has_model_based()); |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| ASSERT_GT(-0.1, status_fetcher_->model_based()->accel_state()->velocity_x()); |
| ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(), |
| 1e-10); |
| ASSERT_NEAR( |
| 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(), |
| 1e-1); |
| ASSERT_NEAR( |
| 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(), |
| 1e-1); |
| } |
| |
| TEST_F(EventLoopLocalizerTest, SpinInPlace) { |
| output_voltages_ << 4.0, -4.0; |
| event_loop_factory_.RunFor(std::chrono::seconds(2)); |
| CHECK(output_fetcher_.Fetch()); |
| CHECK(status_fetcher_.Fetch()); |
| // The two can be different because they may've been sent at different times. |
| ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6); |
| ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6); |
| ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(), |
| 1e-1); |
| ASSERT_NEAR(0.0, output_fetcher_->x(), 1e-10); |
| ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10); |
| ASSERT_LT(0.1, std::abs(output_fetcher_->theta())); |
| ASSERT_TRUE(status_fetcher_->has_model_based()); |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_x(), |
| 1e-10); |
| ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(), |
| 1e-10); |
| ASSERT_NEAR(-status_fetcher_->model_based()->model_state()->left_velocity(), |
| status_fetcher_->model_based()->model_state()->right_velocity(), |
| 1e-3); |
| ASSERT_NEAR( |
| 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(), |
| 1e-1); |
| ASSERT_NEAR( |
| 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(), |
| 1e-1); |
| ASSERT_NEAR(0.0, status_fetcher_->model_based()->residual(), 1e-3); |
| } |
| |
| TEST_F(EventLoopLocalizerTest, Curve) { |
| output_voltages_ << 2.0, 4.0; |
| event_loop_factory_.RunFor(std::chrono::seconds(2)); |
| CHECK(output_fetcher_.Fetch()); |
| CHECK(status_fetcher_.Fetch()); |
| // The two can be different because they may've been sent at different times. |
| ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-2); |
| ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-2); |
| ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(), |
| 1e-1); |
| ASSERT_LT(0.1, output_fetcher_->x()); |
| ASSERT_LT(0.1, output_fetcher_->y()); |
| ASSERT_LT(0.1, std::abs(output_fetcher_->theta())); |
| ASSERT_TRUE(status_fetcher_->has_model_based()); |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| ASSERT_LT(0.0, status_fetcher_->model_based()->accel_state()->velocity_x()); |
| ASSERT_LT(0.0, status_fetcher_->model_based()->accel_state()->velocity_y()); |
| ASSERT_NEAR( |
| 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(), |
| 1e-1); |
| ASSERT_NEAR( |
| 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(), |
| 1e-1); |
| ASSERT_NEAR(0.0, status_fetcher_->model_based()->residual(), 1e-1) |
| << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true}); |
| } |
| |
| // Tests that small amounts of voltage error are handled by the model-based |
| // half of the localizer. |
| TEST_F(EventLoopLocalizerTest, VoltageError) { |
| output_voltages_ << 0.0, 0.0; |
| drivetrain_plant_.set_left_voltage_offset(2.0); |
| drivetrain_plant_.set_right_voltage_offset(2.0); |
| drivetrain_plant_.set_accel_sin_magnitude(0.01); |
| |
| event_loop_factory_.RunFor(std::chrono::seconds(2)); |
| CHECK(output_fetcher_.Fetch()); |
| CHECK(status_fetcher_.Fetch()); |
| // Should still be using the model, but have a non-trivial residual. |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| ASSERT_LT(0.02, status_fetcher_->model_based()->residual()) |
| << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true}); |
| |
| // Afer running for a while, voltage error terms should converge and result in |
| // low residuals. |
| event_loop_factory_.RunFor(std::chrono::seconds(10)); |
| CHECK(output_fetcher_.Fetch()); |
| CHECK(status_fetcher_.Fetch()); |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| ASSERT_NEAR( |
| 2.0, status_fetcher_->model_based()->model_state()->left_voltage_error(), |
| 0.1) |
| << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true}); |
| ASSERT_NEAR( |
| 2.0, status_fetcher_->model_based()->model_state()->right_voltage_error(), |
| 0.1) |
| << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true}); |
| ASSERT_GT(0.02, status_fetcher_->model_based()->residual()) |
| << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true}); |
| } |
| |
| // Tests that large amounts of voltage error force us into the |
| // acceleration-based localizer. |
| TEST_F(EventLoopLocalizerTest, HighVoltageError) { |
| output_voltages_ << 0.0, 0.0; |
| drivetrain_plant_.set_left_voltage_offset(200.0); |
| drivetrain_plant_.set_right_voltage_offset(200.0); |
| drivetrain_plant_.set_accel_sin_magnitude(0.01); |
| |
| event_loop_factory_.RunFor(std::chrono::seconds(2)); |
| CHECK(output_fetcher_.Fetch()); |
| CHECK(status_fetcher_.Fetch()); |
| // Should still be using the model, but have a non-trivial residual. |
| ASSERT_FALSE(status_fetcher_->model_based()->using_model()); |
| ASSERT_LT(0.1, status_fetcher_->model_based()->residual()) |
| << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true}); |
| ASSERT_NEAR(drivetrain_plant_.state()(0), status_fetcher_->model_based()->x(), |
| 1.0); |
| ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(), |
| 1e-6); |
| } |
| |
| // Tests that image corrections in the nominal case (no errors) causes no |
| // issues. |
| TEST_F(EventLoopLocalizerTest, NominalImageCorrections) { |
| output_voltages_ << 3.0, 2.0; |
| drivetrain_plant_.set_accel_sin_magnitude(0.01); |
| send_targets_ = true; |
| |
| event_loop_factory_.RunFor(std::chrono::seconds(4)); |
| CHECK(status_fetcher_.Fetch()); |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| EXPECT_TRUE(VerifyEstimatorAccurate(1e-1)); |
| ASSERT_TRUE(status_fetcher_->model_based()->has_statistics()); |
| ASSERT_LT(10, |
| status_fetcher_->model_based()->statistics()->total_candidates()); |
| ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(), |
| status_fetcher_->model_based()->statistics()->total_accepted()); |
| } |
| |
| // Tests that image corrections when there is an error at the start results |
| // in us actually getting corrected over time. |
| TEST_F(EventLoopLocalizerTest, ImageCorrections) { |
| output_voltages_ << 0.0, 0.0; |
| drivetrain_plant_.mutable_state()->x() = 2.0; |
| drivetrain_plant_.mutable_state()->y() = 2.0; |
| SendLocalizerControl(5.0, 3.0, 0.0); |
| event_loop_factory_.RunFor(std::chrono::seconds(4)); |
| CHECK(output_fetcher_.Fetch()); |
| ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5); |
| ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5); |
| ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5); |
| |
| send_targets_ = true; |
| |
| event_loop_factory_.RunFor(std::chrono::seconds(4)); |
| CHECK(status_fetcher_.Fetch()); |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| EXPECT_TRUE(VerifyEstimatorAccurate(5e-1)); |
| ASSERT_TRUE(status_fetcher_->model_based()->has_statistics()); |
| ASSERT_LT(10, |
| status_fetcher_->model_based()->statistics()->total_candidates()); |
| ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(), |
| status_fetcher_->model_based()->statistics()->total_accepted()); |
| } |
| |
| // Tests that image corrections are ignored when the turret moves too fast. |
| TEST_F(EventLoopLocalizerTest, ImageCorrectionsTurretTooFast) { |
| output_voltages_ << 0.0, 0.0; |
| drivetrain_plant_.mutable_state()->x() = 2.0; |
| drivetrain_plant_.mutable_state()->y() = 2.0; |
| SendLocalizerControl(5.0, 3.0, 0.0); |
| turret_velocity_ = 10.0; |
| event_loop_factory_.RunFor(std::chrono::seconds(4)); |
| CHECK(output_fetcher_.Fetch()); |
| ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5); |
| ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5); |
| ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5); |
| |
| send_targets_ = true; |
| |
| event_loop_factory_.RunFor(std::chrono::seconds(4)); |
| CHECK(status_fetcher_.Fetch()); |
| CHECK(output_fetcher_.Fetch()); |
| ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5); |
| ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5); |
| ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5); |
| ASSERT_TRUE(status_fetcher_->model_based()->has_statistics()); |
| ASSERT_LT(10, |
| status_fetcher_->model_based()->statistics()->total_candidates()); |
| ASSERT_EQ(0, status_fetcher_->model_based()->statistics()->total_accepted()); |
| ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(), |
| status_fetcher_->model_based() |
| ->statistics() |
| ->rejection_reason_count() |
| ->Get(static_cast<int>(RejectionReason::TURRET_TOO_FAST))); |
| // We expect one more rejection to occur due to the time it takes all the |
| // information to propagate. |
| const int rejected_count = |
| status_fetcher_->model_based()->statistics()->total_candidates() + 1; |
| // Check that when we go back to being still we do successfully converge. |
| turret_velocity_ = 0.0; |
| turret_position_ = 1.0; |
| event_loop_factory_.RunFor(std::chrono::seconds(4)); |
| CHECK(status_fetcher_.Fetch()); |
| ASSERT_TRUE(status_fetcher_->model_based()->using_model()); |
| EXPECT_TRUE(VerifyEstimatorAccurate(5e-1)); |
| ASSERT_TRUE(status_fetcher_->model_based()->has_statistics()); |
| ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(), |
| rejected_count + |
| status_fetcher_->model_based()->statistics()->total_accepted()); |
| } |
| |
| // Tests that image corrections when we are in accel mode works. |
| TEST_F(EventLoopLocalizerTest, ImageCorrectionsInAccel) { |
| output_voltages_ << 0.0, 0.0; |
| drivetrain_plant_.set_left_voltage_offset(200.0); |
| drivetrain_plant_.set_right_voltage_offset(200.0); |
| drivetrain_plant_.set_accel_sin_magnitude(0.01); |
| drivetrain_plant_.mutable_state()->x() = 2.0; |
| drivetrain_plant_.mutable_state()->y() = 2.0; |
| SendLocalizerControl(6.0, 3.0, 0.0); |
| event_loop_factory_.RunFor(std::chrono::seconds(1)); |
| CHECK(output_fetcher_.Fetch()); |
| CHECK(status_fetcher_.Fetch()); |
| ASSERT_FALSE(status_fetcher_->model_based()->using_model()); |
| EXPECT_FALSE(VerifyEstimatorAccurate(3.0)); |
| |
| send_targets_ = true; |
| |
| event_loop_factory_.RunFor(std::chrono::seconds(4)); |
| CHECK(status_fetcher_.Fetch()); |
| ASSERT_FALSE(status_fetcher_->model_based()->using_model()); |
| EXPECT_TRUE(VerifyEstimatorAccurate(3.0)); |
| // y should be noticeably more accurate than x, since we are just driving |
| // straight. |
| ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(), |
| 0.1); |
| ASSERT_TRUE(status_fetcher_->model_based()->has_statistics()); |
| ASSERT_LT(10, |
| status_fetcher_->model_based()->statistics()->total_candidates()); |
| ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(), |
| status_fetcher_->model_based()->statistics()->total_accepted()); |
| } |
| |
| TEST_F(EventLoopLocalizerTest, LedOutputs) { |
| send_targets_ = true; |
| |
| event_loop_factory_.RunFor(std::chrono::milliseconds(10)); |
| CHECK(output_fetcher_.Fetch()); |
| EXPECT_EQ(output_fetcher_->led_outputs()->size(), |
| ModelBasedLocalizer::kNumPis); |
| for (LedOutput led_output : *output_fetcher_->led_outputs()) { |
| EXPECT_EQ(led_output, LedOutput::ON); |
| } |
| } |
| |
| } // namespace frc971::controls::testing |