y2019/control_loops/drivetrain/localizer_test.cc

#include "y2019/control_loops/drivetrain/localizer.h"

#include <queue>
#include <random>

#include "aos/testing/random_seed.h"
#include "aos/testing/test_shm.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/trajectory.h"
#include "gflags/gflags.h"
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
#include "gtest/gtest.h"
#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"

DEFINE_bool(plot, false, "If true, plot");

namespace y2019 {
namespace control_loops {
namespace testing {

using ::y2019::constants::Field;

constexpr size_t kNumCameras = 4;
constexpr size_t kNumTargetsPerFrame = 3;

typedef TypedLocalizer<kNumCameras, Field::kNumTargets, Field::kNumObstacles,
                       kNumTargetsPerFrame, double>
    TestLocalizer;
typedef typename TestLocalizer::Camera TestCamera;
typedef typename TestLocalizer::Target Target;
typedef typename TestCamera::Pose Pose;
typedef typename TestCamera::LineSegment Obstacle;

typedef TestLocalizer::StateIdx StateIdx;

using ::frc971::control_loops::drivetrain::DrivetrainConfig;

// When placing the cameras on the robot, set them all kCameraOffset out from
// the center, to test that we really can handle cameras that aren't at the
// center-of-mass.
constexpr double kCameraOffset = 0.1;

#if defined(SUPPORT_PLOT)
// Plots a line from a vector of Pose's.
void PlotPlotPts(const ::std::vector<Pose> &poses,
                 const ::std::map<::std::string, ::std::string> &kwargs) {
  ::std::vector<double> x;
  ::std::vector<double> y;
  for (const Pose &p : poses) {
    x.push_back(p.abs_pos().x());
    y.push_back(p.abs_pos().y());
  }
  matplotlibcpp::plot(x, y, kwargs);
}
#endif

struct LocalizerTestParams {
  // Control points for the spline to make the robot follow.
  ::std::array<float, 6> control_pts_x;
  ::std::array<float, 6> control_pts_y;
  // The actual state to start the robot at. By setting voltage errors and the
  // such you can introduce persistent disturbances.
  TestLocalizer::State true_start_state;
  // The initial state of the estimator.
  TestLocalizer::State known_start_state;
  // Whether or not to add Gaussian noise to the sensors and cameras.
  bool noisify;
  // Whether or not to add unmodelled disturbances.
  bool disturb;
  // The tolerances for the estimator and for the trajectory following at
  // the end of the spline:
  double estimate_tolerance;
  double goal_tolerance;
};

class ParameterizedLocalizerTest
    : public ::testing::TestWithParam<LocalizerTestParams> {
 public:
  ::aos::testing::TestSharedMemory shm_;

  // Set up three targets in a row, at (-1, 0), (0, 0), and (1, 0).
  // Make the right-most target (1, 0) be facing away from the camera, and give
  // the middle target some skew.
  // Place one camera facing forward, the other facing backward, and set the
  // robot at (0, -5) with the cameras each 0.1m from the center.
  // Place one obstacle in a place where it can block the left-most target (-1,
  // 0).
  ParameterizedLocalizerTest()
      : field_(),
        targets_(field_.targets()),
        modeled_obstacles_(field_.obstacles()),
        true_obstacles_(field_.obstacles()),
        dt_config_(drivetrain::GetDrivetrainConfig()),
        // Pull the noise for the encoders/gyros from the R matrix:
        encoder_noise_(::std::sqrt(
            dt_config_.make_kf_drivetrain_loop().observer().coefficients().R(
                0, 0))),
        gyro_noise_(::std::sqrt(
            dt_config_.make_kf_drivetrain_loop().observer().coefficients().R(
                2, 2))),
        // As per the comments in localizer.h, we set the field of view and
        // noise parameters on the robot_cameras_ so that they see a bit more
        // than the true_cameras_. The robot_cameras_ are what is passed to the
        // localizer and used to generate "expected" targets. The true_cameras_
        // are what we actually use to generate targets to pass to the
        // localizer.
        robot_cameras_{
            {TestCamera({&robot_pose_, {0.0, kCameraOffset, 0.0}, M_PI_2},
                        M_PI_2 * 1.1, robot_noise_parameters_, targets_,
                        modeled_obstacles_),
             TestCamera({&robot_pose_, {kCameraOffset, 0.0, 0.0}, 0.0},
                        M_PI_2 * 1.1, robot_noise_parameters_, targets_,
                        modeled_obstacles_),
             TestCamera({&robot_pose_, {-kCameraOffset, 0.0, 0.0}, M_PI},
                        M_PI_2 * 1.1, robot_noise_parameters_, targets_,
                        modeled_obstacles_),
             TestCamera({&robot_pose_, {0.0, -kCameraOffset, 0.0}, -M_PI_2},
                        M_PI_2 * 1.1, robot_noise_parameters_, targets_,
                        modeled_obstacles_)}},
        true_cameras_{
            {TestCamera({&true_robot_pose_, {0.0, kCameraOffset, 0.0}, M_PI_2},
                        M_PI_2 * 0.9, true_noise_parameters_, targets_,
                        true_obstacles_),
             TestCamera({&true_robot_pose_, {kCameraOffset, 0.0, 0.0}, 0.0},
                        M_PI_2 * 0.9, true_noise_parameters_, targets_,
                        true_obstacles_),
             TestCamera({&true_robot_pose_, {-kCameraOffset, 0.0, 0.0}, M_PI},
                        M_PI_2 * 0.9, true_noise_parameters_, targets_,
                        true_obstacles_),
             TestCamera(
                 {&true_robot_pose_, {-0.0, -kCameraOffset, 0.0}, -M_PI_2},
                 M_PI_2 * 0.9, true_noise_parameters_, targets_,
                 true_obstacles_)}},
        localizer_(dt_config_, &robot_pose_),
        spline_drivetrain_(dt_config_) {
    // We use the default P() for initialization.
    localizer_.ResetInitialState(t0_, GetParam().known_start_state,
                                 localizer_.P());
  }

  void SetUp() {
    flatbuffers::DetachedBuffer goal_buffer;
    {
      flatbuffers::FlatBufferBuilder fbb;

      flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
          fbb.CreateVector<float>(GetParam().control_pts_x.begin(),
                                  GetParam().control_pts_x.size());

      flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
          fbb.CreateVector<float>(GetParam().control_pts_y.begin(),
                                  GetParam().control_pts_y.size());

      frc971::MultiSpline::Builder multispline_builder(fbb);

      multispline_builder.add_spline_count(1);
      multispline_builder.add_spline_x(spline_x_offset);
      multispline_builder.add_spline_y(spline_y_offset);

      flatbuffers::Offset<frc971::MultiSpline> multispline_offset =
          multispline_builder.Finish();

      frc971::control_loops::drivetrain::SplineGoal::Builder spline_builder(
          fbb);

      spline_builder.add_spline_idx(1);
      spline_builder.add_spline(multispline_offset);

      flatbuffers::Offset<frc971::control_loops::drivetrain::SplineGoal>
          spline_offset = spline_builder.Finish();

      frc971::control_loops::drivetrain::Goal::Builder goal_builder(fbb);

      goal_builder.add_spline(spline_offset);
      goal_builder.add_controller_type(
          frc971::control_loops::drivetrain::ControllerType::SPLINE_FOLLOWER);
      goal_builder.add_spline_handle(1);

      fbb.Finish(goal_builder.Finish());

      goal_buffer = fbb.Release();
    }
    aos::FlatbufferDetachedBuffer<frc971::control_loops::drivetrain::Goal> goal(
        std::move(goal_buffer));

    // Let the spline drivetrain compute the spline.
    while (true) {
      // We need to keep sending the goal.  There are conditions when the
      // trajectory lock isn't grabbed the first time, and we want to keep
      // banging on it to keep trying.  Otherwise we deadlock.
      spline_drivetrain_.SetGoal(&goal.message());

      ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));

      flatbuffers::FlatBufferBuilder fbb;

      flatbuffers::Offset<frc971::control_loops::drivetrain::TrajectoryLogging>
          trajectory_logging_offset =
              spline_drivetrain_.MakeTrajectoryLogging(&fbb);

      ::frc971::control_loops::drivetrain::Status::Builder status_builder(fbb);
      status_builder.add_trajectory_logging(trajectory_logging_offset);
      spline_drivetrain_.PopulateStatus(&status_builder);
      fbb.Finish(status_builder.Finish());
      aos::FlatbufferDetachedBuffer<::frc971::control_loops::drivetrain::Status>
          status(fbb.Release());

      if (status.message().trajectory_logging()->planning_state() ==
          ::frc971::control_loops::drivetrain::PlanningState::PLANNED) {
        break;
      }
    }
    spline_drivetrain_.SetGoal(&goal.message());
  }

  void TearDown() {
    printf("Each iteration of the simulation took on average %f seconds.\n",
           avg_time_.count());
#if defined(SUPPORT_PLOT)
    if (FLAGS_plot) {
      matplotlibcpp::figure();
      matplotlibcpp::plot(simulation_t_, simulation_vl_, {{"label", "Vl"}});
      matplotlibcpp::plot(simulation_t_, simulation_vr_, {{"label", "Vr"}});
      matplotlibcpp::legend();

      matplotlibcpp::figure();
      matplotlibcpp::plot(spline_x_, spline_y_, {{"label", "spline"}});
      matplotlibcpp::plot(simulation_x_, simulation_y_, {{"label", "robot"}});
      matplotlibcpp::plot(estimated_x_, estimated_y_,
                          {{"label", "estimation"}});
      for (const Target &target : targets_) {
        PlotPlotPts(target.PlotPoints(), {{"c", "g"}});
      }
      for (const Obstacle &obstacle : true_obstacles_) {
        PlotPlotPts(obstacle.PlotPoints(), {{"c", "k"}});
      }
      // Go through and plot true/expected camera targets for a few select
      // time-steps.
      ::std::vector<const char *> colors{"m", "y", "c"};
      int jj = 0;
      for (size_t ii = 0; ii < simulation_x_.size(); ii += 400) {
        *true_robot_pose_.mutable_pos() << simulation_x_[ii], simulation_y_[ii],
            0.0;
        true_robot_pose_.set_theta(simulation_theta_[ii]);
        for (const TestCamera &camera : true_cameras_) {
          for (const auto &plot_pts : camera.PlotPoints()) {
            PlotPlotPts(plot_pts, {{"c", colors[jj]}});
          }
        }
        for (const TestCamera &camera : robot_cameras_) {
          *robot_pose_.mutable_pos() << estimated_x_[ii], estimated_y_[ii], 0.0;
          robot_pose_.set_theta(estimated_theta_[ii]);
          const auto &all_plot_pts = camera.PlotPoints();
          *robot_pose_.mutable_pos() = true_robot_pose_.rel_pos();
          robot_pose_.set_theta(true_robot_pose_.rel_theta());
          for (const auto &plot_pts : all_plot_pts) {
            PlotPlotPts(plot_pts, {{"c", colors[jj]}, {"ls", "--"}});
          }
        }
        jj = (jj + 1) % colors.size();
      }
      matplotlibcpp::legend();

      matplotlibcpp::figure();
      matplotlibcpp::plot(
          simulation_t_, spline_x_,
          {{"label", "spline x"}, {"c", "g"}, {"ls", ""}, {"marker", "o"}});
      matplotlibcpp::plot(simulation_t_, simulation_x_,
                          {{"label", "simulated x"}, {"c", "g"}});
      matplotlibcpp::plot(simulation_t_, estimated_x_,
                          {{"label", "estimated x"}, {"c", "g"}, {"ls", "--"}});

      matplotlibcpp::plot(
          simulation_t_, spline_y_,
          {{"label", "spline y"}, {"c", "b"}, {"ls", ""}, {"marker", "o"}});
      matplotlibcpp::plot(simulation_t_, simulation_y_,
                          {{"label", "simulated y"}, {"c", "b"}});
      matplotlibcpp::plot(simulation_t_, estimated_y_,
                          {{"label", "estimated y"}, {"c", "b"}, {"ls", "--"}});

      matplotlibcpp::plot(simulation_t_, simulation_theta_,
                          {{"label", "simulated theta"}, {"c", "r"}});
      matplotlibcpp::plot(
          simulation_t_, estimated_theta_,
          {{"label", "estimated theta"}, {"c", "r"}, {"ls", "--"}});
      matplotlibcpp::legend();

      matplotlibcpp::show();
    }
#endif
  }

 protected:
  // Returns a random number with a gaussian distribution with a mean of zero
  // and a standard deviation of std, if noisify = true.
  // If noisify is false, then returns 0.0.
  double Normal(double std) {
    if (GetParam().noisify) {
      return normal_(gen_) * std;
    }
    return 0.0;
  }
  // Adds random noise to the given target view.
  void Noisify(TestCamera::TargetView *view) {
    view->reading.heading += Normal(view->noise.heading);
    view->reading.distance += Normal(view->noise.distance);
    view->reading.height += Normal(view->noise.height);
    view->reading.skew += Normal(view->noise.skew);
  }
  // The differential equation for the dynamics of the system.
  TestLocalizer::State DiffEq(const TestLocalizer::State &X,
                              const Eigen::Vector2d &voltage) {
    TestLocalizer::Input U;
    U.setZero();
    U(0) = voltage(0);
    U(1) = voltage(1);
    return localizer_.DiffEq(X, U, true);
  }

  Field field_;
  ::std::array<Target, Field::kNumTargets> targets_;
  // The obstacles that are passed to the camera objects for the localizer.
  ::std::array<Obstacle, Field::kNumObstacles> modeled_obstacles_;
  // The obstacles that are used for actually simulating the cameras.
  ::std::array<Obstacle, Field::kNumObstacles> true_obstacles_;

  DrivetrainConfig<double> dt_config_;

  // Noise information for the actual simulated cameras (true_*) and the
  // parameters that the localizer should use for modelling the cameras. Note
  // how the max_viewable_distance is larger for the localizer, so that if
  // there is any error in the estimation, it still thinks that it can see
  // any targets that might actually be in range.
  TestCamera::NoiseParameters true_noise_parameters_ = {
      .max_viewable_distance = 10.0,
      .heading_noise = 0.02,
      .nominal_distance_noise = 0.06,
      .nominal_skew_noise = 0.1,
      .nominal_height_noise = 0.01};
  TestCamera::NoiseParameters robot_noise_parameters_ = {
      .max_viewable_distance = 11.0,
      .heading_noise = 0.02,
      .nominal_distance_noise = 0.06,
      .nominal_skew_noise = 0.1,
      .nominal_height_noise = 0.01};

  // Standard deviations of the noise for the encoders/gyro.
  double encoder_noise_, gyro_noise_;

  Pose robot_pose_;
  ::std::array<TestCamera, 4> robot_cameras_;
  Pose true_robot_pose_;
  ::std::array<TestCamera, 4> true_cameras_;

  TestLocalizer localizer_;

  ::frc971::control_loops::drivetrain::SplineDrivetrain spline_drivetrain_;

  // All the data we want to end up plotting.
  ::std::vector<double> simulation_t_;

  ::std::vector<double> spline_x_;
  ::std::vector<double> spline_y_;
  ::std::vector<double> estimated_x_;
  ::std::vector<double> estimated_y_;
  ::std::vector<double> estimated_theta_;
  ::std::vector<double> simulation_x_;
  ::std::vector<double> simulation_y_;
  ::std::vector<double> simulation_theta_;

  ::std::vector<double> simulation_vl_;
  ::std::vector<double> simulation_vr_;

  // Simulation start time
  ::aos::monotonic_clock::time_point t0_;

  // Average duration of each iteration; used for debugging and getting a
  // sanity-check on what performance looks like--uses a real system clock.
  ::std::chrono::duration<double> avg_time_;

  ::std::mt19937 gen_{static_cast<uint32_t>(::aos::testing::RandomSeed())};
  ::std::normal_distribution<> normal_;
};

using ::std::chrono::milliseconds;

// Tests that, when we attempt to follow a spline and use the localizer to
// perform the state estimation, we end up roughly where we should and that
// the localizer has a valid position estimate.
TEST_P(ParameterizedLocalizerTest, SplineTest) {
  // state stores the true state of the robot throughout the simulation.
  TestLocalizer::State state = GetParam().true_start_state;

  ::aos::monotonic_clock::time_point t = t0_;

  // The period with which we should take frames from the cameras. Currently,
  // we just trigger all the cameras at once, rather than offsetting them or
  // anything.
  const int camera_period = 5;  // cycles
  // The amount of time to delay the camera images from when they are taken, for
  // each camera.
  const ::std::array<milliseconds, 4> camera_latencies{
      {milliseconds(45), milliseconds(50), milliseconds(55),
       milliseconds(100)}};

  // A queue of camera frames for each camera so that we can add a time delay to
  // the data coming from the cameras.
  ::std::array<
      ::std::queue<::std::tuple<
          ::aos::monotonic_clock::time_point, const TestCamera *,
          ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame>>>,
      4>
      camera_queues;

  // Start time, for debugging.
  const auto begin = ::std::chrono::steady_clock::now();

  size_t i;
  // Run the feedback-controller slightly past the nominal end-time. This both
  // exercises the code to see what happens when we are trying to stand still,
  // and gives the control loops time to stabilize.
  aos::monotonic_clock::duration extra_time = std::chrono::seconds(2);
  for (i = 0;
       !spline_drivetrain_.IsAtEnd() || extra_time > std::chrono::seconds(0);
       ++i) {
    if (spline_drivetrain_.IsAtEnd()) {
      extra_time -= dt_config_.dt;
    }
    // Get the current state estimate into a matrix that works for the
    // trajectory code.
    ::Eigen::Matrix<double, 5, 1> known_state;
    TestLocalizer::State X_hat = localizer_.X_hat();
    known_state << X_hat(StateIdx::kX, 0), X_hat(StateIdx::kY, 0),
        X_hat(StateIdx::kTheta, 0), X_hat(StateIdx::kLeftVelocity, 0),
        X_hat(StateIdx::kRightVelocity, 0);

    Eigen::Vector2d voltage_error(X_hat(StateIdx::kLeftVoltageError),
                                  X_hat(StateIdx::kRightVoltageError));
    spline_drivetrain_.Update(true, known_state, voltage_error);

    ::frc971::control_loops::drivetrain::OutputT output;
    output.left_voltage = 0;
    output.right_voltage = 0;
    spline_drivetrain_.SetOutput(&output);
    Eigen::Vector2d U(output.left_voltage, output.right_voltage);

    const ::Eigen::Matrix<double, 5, 1> goal_state =
        spline_drivetrain_.CurrentGoalState();
    simulation_t_.push_back(
        ::aos::time::DurationInSeconds(t.time_since_epoch()));
    spline_x_.push_back(goal_state(0, 0));
    spline_y_.push_back(goal_state(1, 0));
    simulation_x_.push_back(state(StateIdx::kX, 0));
    simulation_y_.push_back(state(StateIdx::kY, 0));
    simulation_theta_.push_back(state(StateIdx::kTheta, 0));
    estimated_x_.push_back(known_state(0, 0));
    estimated_y_.push_back(known_state(1, 0));
    estimated_theta_.push_back(known_state(StateIdx::kTheta, 0));

    simulation_vl_.push_back(U(0));
    simulation_vr_.push_back(U(1));
    U(0, 0) = ::std::max(::std::min(U(0, 0), 12.0), -12.0);
    U(1, 0) = ::std::max(::std::min(U(1, 0), 12.0), -12.0);

    state = ::frc971::control_loops::RungeKuttaU(
        [this](const ::Eigen::Matrix<double, 12, 1> &X,
               const ::Eigen::Matrix<double, 2, 1> &U) { return DiffEq(X, U); },
        state, U, ::aos::time::DurationInSeconds(dt_config_.dt));

    // Add arbitrary disturbances at some arbitrary interval. The main points of
    // interest here are that we (a) stop adding disturbances at the very end of
    // the trajectory, to allow us to actually converge to the goal, and (b)
    // scale disturbances by the corruent velocity.
    // TODO(james): Figure out how to persist good accelerometer values through
    // the disturbances.
    if (GetParam().disturb && i % 75 == 0) {
      // Scale the disturbance so that when we have near-zero velocity we don't
      // have much disturbance.
      double disturbance_scale = ::std::min(
          1.0, ::std::sqrt(::std::pow(state(StateIdx::kLeftVelocity, 0), 2) +
                           ::std::pow(state(StateIdx::kRightVelocity, 0), 2)) /
                   3.0);
      TestLocalizer::State disturbance;
      disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0,
          0.0, 0.0;
      disturbance *= disturbance_scale;
      state += disturbance;
    }

    t += dt_config_.dt;
    *true_robot_pose_.mutable_pos() << state(StateIdx::kX, 0),
        state(StateIdx::kY, 0), 0.0;
    true_robot_pose_.set_theta(state(StateIdx::kTheta, 0));
    const double left_enc = state(StateIdx::kLeftEncoder, 0);
    const double right_enc = state(StateIdx::kRightEncoder, 0);

    const double gyro = (state(StateIdx::kRightVelocity) -
                         state(StateIdx::kLeftVelocity)) /
                        dt_config_.robot_radius / 2.0;
    const TestLocalizer::State xdot = DiffEq(state, U);
    const Eigen::Vector3d accel(
        localizer_.CalcLongitudinalVelocity(xdot) -
            gyro * state(StateIdx::kLateralVelocity),
        gyro * localizer_.CalcLongitudinalVelocity(state), 1.0);

    localizer_.UpdateEncodersAndGyro(left_enc + Normal(encoder_noise_),
                                     right_enc + Normal(encoder_noise_),
                                     gyro + Normal(gyro_noise_), U, accel, t);

    for (size_t cam_idx = 0; cam_idx < camera_queues.size(); ++cam_idx) {
      auto &camera_queue = camera_queues[cam_idx];
      // Clear out the camera frames that we are ready to pass to the localizer.
      while (!camera_queue.empty() && ::std::get<0>(camera_queue.front()) <
                                          t - camera_latencies[cam_idx]) {
        const auto tuple = camera_queue.front();
        camera_queue.pop();
        ::aos::monotonic_clock::time_point t_obs = ::std::get<0>(tuple);
        const TestCamera *camera = ::std::get<1>(tuple);
        ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame> views =
            ::std::get<2>(tuple);
        localizer_.UpdateTargets(*camera, views, t_obs);
      }

      // Actually take all the images and store them in the queue.
      if (i % camera_period == 0) {
        for (size_t jj = 0; jj < true_cameras_.size(); ++jj) {
          const auto target_views = true_cameras_[jj].target_views();
          ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame>
              pass_views;
          for (size_t ii = 0;
               ii < ::std::min(kNumTargetsPerFrame, target_views.size());
               ++ii) {
            TestCamera::TargetView view = target_views[ii];
            Noisify(&view);
            pass_views.push_back(view);
          }
          camera_queue.emplace(t, &robot_cameras_[jj], pass_views);
        }
      }
    }
  }

  const auto end = ::std::chrono::steady_clock::now();
  avg_time_ = (end - begin) / i;
  TestLocalizer::State estimate_err = state - localizer_.X_hat();
  EXPECT_LT(estimate_err.template topRows<7>().norm(),
            GetParam().estimate_tolerance);
  // Check that none of the states that we actually care about (x/y/theta, and
  // wheel positions/speeds) are too far off, individually:
  EXPECT_LT(estimate_err.template topRows<3>().cwiseAbs().maxCoeff(),
            GetParam().estimate_tolerance / 3.0)
      << "Estimate error: " << estimate_err.transpose();
  Eigen::Matrix<double, 5, 1> final_trajectory_state;
  final_trajectory_state << state(StateIdx::kX, 0), state(StateIdx::kY, 0),
      state(StateIdx::kTheta, 0), state(StateIdx::kLeftVelocity, 0),
      state(StateIdx::kRightVelocity, 0);
  const Eigen::Matrix<double, 5, 1> final_trajectory_state_err =
      final_trajectory_state - spline_drivetrain_.CurrentGoalState();
  EXPECT_LT(final_trajectory_state_err.norm(), GetParam().goal_tolerance)
      << "Goal error: " << final_trajectory_state_err.transpose();
}

// NOTE: Following the 2019 season, we stopped maintaining this as rigorously.
// This means that changes to either the base HybridEKF class or the spline
// controller can cause us to violate the tolerances specified here. We
// currently just up the tolerances whenever they cause issues, so long as
// things don't appear to be unstable (since these tests do do a test of the
// full localizer + spline system, we should pay attention if there is actual
// instability rather than just poor tolerances).
INSTANTIATE_TEST_CASE_P(
    LocalizerTest, ParameterizedLocalizerTest,
    ::testing::Values(
        // Checks a "perfect" scenario, where we should track perfectly.
        LocalizerTestParams({
            /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
            /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            /*noisify=*/false,
            /*disturb=*/false,
            /*estimate_tolerance=*/1e-2,
            /*goal_tolerance=*/0.2,
        }),
        // Checks "perfect" estimation, but start off the spline and check
        // that we can still follow it.
        LocalizerTestParams({
            /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
            /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
            (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            /*noisify=*/false,
            /*disturb=*/false,
            /*estimate_tolerance=*/1e-2,
            /*goal_tolerance=*/0.4,
        }),
        // Repeats perfect scenario, but add sensor noise.
        LocalizerTestParams({
            /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
            /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            /*noisify=*/true,
            /*disturb=*/false,
            /*estimate_tolerance=*/0.4,
            /*goal_tolerance=*/0.8,
        }),
        // Repeats perfect scenario, but add initial estimator error.
        LocalizerTestParams({
            /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
            /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0, 0.0)
                .finished(),
            /*noisify=*/false,
            /*disturb=*/false,
            /*estimate_tolerance=*/1e-2,
            /*goal_tolerance=*/0.2,
        }),
        // Repeats perfect scenario, but add voltage + angular errors:
        LocalizerTestParams({
            /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
            /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5,
             0.25, 0.02, 0.0, 0.0)
                .finished(),
            (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0, 0.0)
                .finished(),
            /*noisify=*/false,
            /*disturb=*/false,
            /*estimate_tolerance=*/1e-2,
            /*goal_tolerance=*/0.3,
        }),
        // Add disturbances while we are driving:
        LocalizerTestParams({
            /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
            /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            /*noisify=*/false,
            /*disturb=*/true,
            /*estimate_tolerance=*/4e-2,
            /*goal_tolerance=*/0.25,
        }),
        // Add noise and some initial error in addition:
        LocalizerTestParams({
            /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
            /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
            (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            (TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            /*noisify=*/true,
            /*disturb=*/true,
            /*estimate_tolerance=*/0.5,
            /*goal_tolerance=*/1.0,
        }),
        // Try another spline, just in case the one I was using is special for
        // some reason; this path will also go straight up to a target, to
        // better simulate what might happen when trying to score:
        LocalizerTestParams({
            /*control_pts_x=*/{{0.5, 3.5, 4.0, 8.0, 11.0, 10.2}},
            /*control_pts_y=*/{{1.0, 1.0, -3.0, -2.0, -3.5, -3.65}},
            (TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            (TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
             0.0, 0.0, 0.0, 0.0)
                .finished(),
            /*noisify=*/true,
            /*disturb=*/false,
            // TODO(james): Improve tests so that we aren't constantly
            // readjusting the tolerances up.
            /*estimate_tolerance=*/0.3,
            /*goal_tolerance=*/0.7,
        })));

}  // namespace testing
}  // namespace control_loops
}  // namespace y2019