lib7842 unit test demo

customOdometry.cpp

#include "lib7842/api/odometry/customOdometry.hpp"
#include <cmath>

namespace lib7842 {

CustomOdometry::CustomOdometry(std::shared_ptr<ChassisModel> imodel,
                               const ChassisScales& ichassisScales) :
  model(std::move(imodel)), chassisScales(ichassisScales) {}

void CustomOdometry::setScales(const ChassisScales& ichassisScales) {
  chassisScales = ichassisScales;
}

/**
 * Odometry algorithm provided courtesy of the pilons from team 5225A
 */
void CustomOdometry::step() {
  auto newTicks = model->getSensorVals();

  if (newTicks.size() < 3) {
    GLOBAL_ERROR_THROW("CustomOdometry::step: The model does not contain three encoders");
  }

  // REDACTED
}

const State& CustomOdometry::getState() const { return state; }

OdomState CustomOdometry::getState(const StateMode& imode) const {
  const State& istate = getState();
  if (imode == StateMode::CARTESIAN) { return {istate.x, istate.y, istate.theta}; }
  return {istate.y, istate.x, istate.theta};
}

void CustomOdometry::setState(const State& istate) { state = istate; }

void CustomOdometry::resetState() { state = {0_in, 0_in, 0_deg}; }

void CustomOdometry::reset() {
  model->resetSensors();
  lastTicks = {0, 0, 0};
  resetState();
}

void CustomOdometry::setState(const OdomState& istate, const StateMode& imode) {
  if (imode == StateMode::CARTESIAN) {
    state = {istate.x, istate.y, istate.theta};
  } else {
    state = {istate.y, istate.x, istate.theta};
  }
}

std::shared_ptr<ReadOnlyChassisModel> CustomOdometry::getModel() { return model; }

ChassisScales CustomOdometry::getScales() { return chassisScales; }

void CustomOdometry::loop() {
  auto rate = global::getTimeUtil()->getRate();
  rate->delayUntil(20_ms);
  while (true) {
    step();
    rate->delayUntil(5_ms);
  }
}

} // namespace lib7842

#include "lib7842/test/mocks.hpp"
namespace test {
static QLength calculateDistanceTraveled(double ticks) {
  return (double(ticks) / 360.0) * 1_pi * 4_in;
}

static void assertOdomState(const State& istate, const std::shared_ptr<CustomOdometry>& iodom) {
  CHECK(iodom->getState().x.convert(meter) == Approx(istate.x.convert(meter)));
  CHECK(iodom->getState().y.convert(meter) == Approx(istate.y.convert(meter)));
  CHECK(iodom->getState().theta.convert(degree) == Approx(istate.theta.convert(degree)));
}

TEST_CASE("CustomOdometry") {
  auto model = std::make_shared<MockThreeEncoderXDriveModel>();

  SUBCASE("Common Odometry Tests") {

    auto odom =
      std::make_shared<CustomOdometry>(model, ChassisScales({{4_in, 10_in, 5_in, 4_in}, 360}));

    auto assertState = [&](const State& istate) { assertOdomState(istate, odom); };

    SUBCASE("MockSensorsFunction") {
      model->setSensorVals(10, 20, 30);
      auto sensors = model->getSensorVals();
      CHECK(sensors[0] == 10);
      CHECK(sensors[1] == 20);
      CHECK(sensors[2] == 30);
    }

    SUBCASE("NoSensorMovementDoesNotAffectState") {
      assertState({0_m, 0_m, 0_deg});
      odom->step();
      assertState({0_m, 0_m, 0_deg});
    }

    SUBCASE("RepeatedlyCallingStep") {
      for (size_t i = 0; i < 50; i++) {
        assertState({0_m, 0_m, 0_deg});
        odom->step();
        assertState({0_m, 0_m, 0_deg});
      }
    }

    SUBCASE("MoveForwardTest") {
      model->setSensorVals(10, 10, 0);
      odom->step();
      assertState({0_m, calculateDistanceTraveled(10), 0_deg});

      model->setSensorVals(20, 20, 0);
      odom->step();
      assertState({0_m, calculateDistanceTraveled(20), 0_deg});

      model->setSensorVals(10, 10, 0);
      odom->step();
      assertState({0_m, calculateDistanceTraveled(10), 0_deg});
    }

    SUBCASE("TurnInPlaceTest") {
      model->setSensorVals(10, -10, -10);
      odom->step();
      assertState({0_m, 0_m, 4_deg});

      model->setSensorVals(0, 0, 0);
      odom->step();
      assertState({0_m, 0_m, 0_deg});

      model->setSensorVals(-10, 10, 10);
      odom->step();
      assertState({0_m, 0_m, -4_deg});
    }

    SUBCASE("TurnAndDriveTest") {
      model->setSensorVals(90, -90, -90);
      odom->step();
      assertState({0_m, 0_m, 36_deg});

      model->setSensorVals(180, 0, -90);
      odom->step();
      assertState({calculateDistanceTraveled(90) * std::sin((36_deg).convert(radian)),
                   calculateDistanceTraveled(90) * std::cos((36_deg).convert(radian)), 36_deg});
    }

    SUBCASE("StrafeTest") {
      model->setSensorVals(0, 0, 10);
      odom->step();
      assertState({calculateDistanceTraveled(10), 0_in, 0_deg});
    }

    SUBCASE("DriveForwardWhileStrafingTest") {
      model->setSensorVals(10, 10, 10);
      odom->step();
      assertState({calculateDistanceTraveled(10), calculateDistanceTraveled(10), 0_deg});
    }

    SUBCASE("FullPointRotationWithMiddle") {
      assertState({0_m, 0_m, 0_deg});

      model->setSensorVals(900, -900, -900);
      odom->step();
      assertState({0_in, 0_in, 360_deg});
    }
  }

  SUBCASE("SmallSwingTurnOnRightWheels") {
    auto smallOdom = std::make_shared<CustomOdometry>(
      model, ChassisScales({{2.75_in, 12.9_in, 0.5_in, 2.75_in}, quadEncoderTPR}));

    model->setSensorVals(0, 0, 0);
    smallOdom->step();
    assertOdomState({0_in, 0_in, 0_deg}, smallOdom);

    model->setSensorVals(0, -2, 0);
    smallOdom->step();
    assertOdomState({0.00181102_in, -0.024_in, 0.21317829_deg}, smallOdom);
  }

  SUBCASE("NinetyDegreePivotTurn") {
    auto straightOdom =
      std::make_shared<CustomOdometry>(model, ChassisScales({{4_in, 10_in, 0_in, 4_in}, 360}));

    assertOdomState({0_m, 0_m, 0_deg}, straightOdom);

    model->setSensorVals(450, 0, 0);
    straightOdom->step();
    assertOdomState({5_in, 5_in, 90_deg}, straightOdom);
  }

  SUBCASE("FullPivotRotation") {
    auto straightOdom =
      std::make_shared<CustomOdometry>(model, ChassisScales({{4_in, 10_in, 0_in, 4_in}, 360}));

    assertOdomState({0_m, 0_m, 0_deg}, straightOdom);

    model->setSensorVals(1800, 0, 0);
    straightOdom->step();
    assertOdomState({0_in, 0_in, 360_deg}, straightOdom);
  }

  SUBCASE("FullPointRotation") {
    auto straightOdom =
      std::make_shared<CustomOdometry>(model, ChassisScales({{4_in, 10_in, 0_in, 4_in}, 360}));

    assertOdomState({0_m, 0_m, 0_deg}, straightOdom);

    model->setSensorVals(900, -900, 0);
    straightOdom->step();
    assertOdomState({0_in, 0_in, 360_deg}, straightOdom);
  }
}
} // namespace test

customOdometry.hpp

#pragma once
#include "lib7842/api/async/taskWrapper.hpp"
#include "lib7842/api/other/global.hpp"
#include "lib7842/api/positioning/point/state.hpp"
#include "okapi/api/chassis/model/chassisModel.hpp"
#include "okapi/api/odometry/odometry.hpp"

namespace lib7842 {

/**
 * Odometry algorithm that directly uses 5225A's tracking algorithm. Uses the cartesian stateMode.
 */
class CustomOdometry : public Odometry, public TaskWrapper {
public:
  /**
   * CustomOdometry.
   *
   * @param imodel         The chassis model for reading sensors.
   * @param ichassisScales The chassis dimensions.
   */
  CustomOdometry(std::shared_ptr<ChassisModel> imodel, const ChassisScales& ichassisScales);

  /**
   * Set the drive and turn scales.
   *
   * @param ichassisScales The chassis scales
   */
  void setScales(const ChassisScales& ichassisScales) override;

  /**
   * Do one odometry step.
   */
  void step() override;

  /**
   * Return the current state.
   *
   * @return The state.
   */
  virtual const State& getState() const;

  /**
   * Return the current state.
   *
   * @param imode The mode to return the state in.
   * @return The current state in the given format.
   */
  OdomState getState(const StateMode& imode) const override;

  /**
   * Set a new state.
   *
   * @param istate The state
   */
  virtual void setState(const State& istate);

  /**
   * Set a new state to be the current state.
   *
   * @param istate The new state in the given format.
   * @param imode The mode to treat the input state as.
   */
  void setState(const OdomState& istate, const StateMode& imode) override;

  /**
   * Reset state to {0, 0, 0}
   */
  virtual void resetState();

  /**
   * Reset sensors and state
   */
  virtual void reset();

  /**
   * Get the chassis model.
   *
   * @return The internal ChassisModel.
   */
  std::shared_ptr<ReadOnlyChassisModel> getModel() override;

  /**
   * Get the chassis scales.
   *
   * @return The internal ChassisScales.
   */
  ChassisScales getScales() override;

  /**
   * Odometry calculation loop
   */
  void loop() override;

protected:
  std::shared_ptr<ChassisModel> model {nullptr};
  ChassisScales chassisScales;

  State state {};
  std::valarray<std::int32_t> lastTicks {0, 0, 0};
};
} // namespace lib7842

pathFollower.cpp

#include "lib7842/api/purePursuit/pathFollower.hpp"
#include "lib7842/api/positioning/point/mathPoint.hpp"

namespace lib7842 {

PathFollower::PathFollower(std::shared_ptr<ChassisModel> imodel,
                           std::shared_ptr<Odometry> iodometry, const ChassisScales& ichassisScales,
                           const QAngularSpeed& igearset, const QLength& ilookahead,
                           const std::optional<QLength>& idriveRadius) :
  model(std::move(imodel)),
  odometry(std::move(iodometry)),
  chassisScales(ichassisScales),
  gearset(igearset),
  lookahead(ilookahead),
  driveRadius(idriveRadius.value_or(ilookahead)) {}

void PathFollower::followPath(const std::vector<Waypoint>& path, const PursuitLimits& limits,
                              bool backwards, const std::optional<QSpeed>& startSpeed) {
  resetPursuit();

  auto rate = global::getTimeUtil()->getRate();
  auto timer = global::getTimeUtil()->getTimer();

  // assume the robot starts at minimum velocity unless otherwise specified
  QSpeed lastVelocity = startSpeed.value_or(limits.minVel);

  bool isFinished = false; // loop until the robot is considered to have finished the path
  while (!isFinished) {
    // get the robot position and heading
    State pos = State(odometry->getState(StateMode::CARTESIAN));

    auto closest = findClosest(path, pos); // get an iterator to the closest point
    Vector lookPoint = findLookaheadPoint(path, pos);

    // the robot is on the path if the distance to the closest point is smaller than the lookahead
    bool onPath = Vector::dist(pos, *closest) <= lookahead;

    // REDACTED

    double left = (wheelVel[0] / gearset).convert(number);
    double right = (wheelVel[1] / gearset).convert(number);

    // normalize the sides
    double maxMag = std::max(std::abs(left), std::abs(right));
    if (maxMag > 1.0) {
      left /= maxMag;
      right /= maxMag;
    }

    if (mode == util::motorMode::voltage) {
      model->tank(left, right);
    } else {
      model->left(left);
      model->right(right);
    }

    rate->delayUntil(10_ms);
  }

  model->driveVector(0, 0); // apply velocity braking
}

void PathFollower::setMotorMode(util::motorMode imode) { mode = imode; }

PathFollower::pathIterator_t PathFollower::findClosest(const std::vector<Waypoint>& path,
                                                       const Vector& pos) {
  // REDACTED
}

Vector PathFollower::findLookaheadPoint(const std::vector<Waypoint>& path, const Vector& pos) {

  // REDACTED
}

std::optional<double> PathFollower::findIntersectT(const Vector& start, const Vector& end,
                                                   const Vector& pos, const QLength& lookahead) {
 // REDACTED
}

double PathFollower::calculateCurvature(const State& state, const Vector& lookPoint) {
 // REDACTED
}

std::valarray<QAngularSpeed> PathFollower::calculateVelocity(const QSpeed& vel, double curvature,
                                                             const ChassisScales& chassisScales,
                                                             const PursuitLimits& limits) {
  QSpeed leftVel = vel * (2.0 + chassisScales.wheelTrack.convert(meter) * curvature) / 2.0;
  QSpeed rightVel = vel * (2.0 - chassisScales.wheelTrack.convert(meter) * curvature) / 2.0;

  leftVel = std::clamp(leftVel, limits.maxVel * -1, limits.maxVel);
  rightVel = std::clamp(rightVel, limits.maxVel * -1, limits.maxVel);

  QAngularSpeed leftWheel = (leftVel / (1_pi * chassisScales.wheelDiameter)) * 360_deg;
  QAngularSpeed rightWheel = (rightVel / (1_pi * chassisScales.wheelDiameter)) * 360_deg;

  return {leftWheel, rightWheel};
}

void PathFollower::resetPursuit() {
  lastClosest = std::nullopt;
  lastLookIndex = 0;
  lastLookT = 0;
}

} // namespace lib7842

#include "lib7842/api/odometry/customOdometry.hpp"
#include "lib7842/test/mocks.hpp"
namespace test {
class MockPathFollower : public PathFollower {
public:
  using PathFollower::PathFollower;
  using PathFollower::lastLookIndex;
  using PathFollower::findClosest;
  using PathFollower::findLookaheadPoint;
  using PathFollower::calculateCurvature;
  using PathFollower::calculateVelocity;
};

TEST_CASE("PathFollower") {

  SUBCASE("given a model, odom, follower, and limits") {

    auto model = std::make_shared<MockThreeEncoderXDriveModel>();
    auto odom =
      std::make_shared<CustomOdometry>(model, ChassisScales({{4_in, 10_in, 5_in, 4_in}, 360}));

    auto follower = std::make_shared<MockPathFollower>(
      model, odom, ChassisScales({{4_in, 10_in}, 360}), 200_rpm, 6_in);

    PursuitLimits limits {0_mps, 0.5_mps2, 1_mps, 1_mps};

    SUBCASE("TestClosest") {
      std::vector<Waypoint> path(
        {{0_ft, 0_ft}, {1_ft, 1_ft}, {2_ft, 2_ft}, {3_ft, 3_ft}, {4_ft, 4_ft}});
      follower->lastLookIndex = 4;

      auto closest = follower->findClosest(path, {1_ft, 1_ft});
      CHECK(closest - path.begin() == 1);

      closest = follower->findClosest(path, {0_ft, 0_ft});
      CHECK(closest - path.begin() == 1);

      closest = follower->findClosest(path, {3_ft, 3.3_ft});
      CHECK(closest - path.begin() == 3);

      closest = follower->findClosest(path, {6_ft, 6_ft});
      CHECK(closest - path.begin() == 4);

      closest = follower->findClosest(path, {0_ft, 0_ft});
      CHECK(closest - path.begin() == 4);
    }

    SUBCASE("TestLookahead") {
      std::vector<Waypoint> path(
        {{0_ft, 0_ft}, {0_ft, 1_ft}, {0_ft, 2_ft}, {0_ft, 3_ft}, {0_ft, 4_ft}});

      Vector lookahead = follower->findLookaheadPoint(path, {0_ft, 1_ft});
      Vector estimated {0_ft, 1.5_ft};
      CHECK(lookahead == estimated);

      lookahead = follower->findLookaheadPoint(path, {0_ft, 1_ft});
      CHECK(lookahead == estimated);

      lookahead = follower->findLookaheadPoint(path, {0_ft, 2_ft});
      estimated = {0_ft, 2.5_ft};
      CHECK(lookahead == estimated);

      lookahead = follower->findLookaheadPoint(path, {0_ft, 0_ft});
      CHECK(lookahead == estimated);

      lookahead = follower->findLookaheadPoint(path, {0_ft, 3.5_ft});
      estimated = {0_ft, 4_ft};
      CHECK(lookahead.y.convert(foot) == Approx(estimated.y.convert(foot)));

      lookahead = follower->findLookaheadPoint(path, {0_ft, 4_ft});
      estimated = {0_ft, 4_ft};
      CHECK(lookahead.y.convert(foot) == Approx(estimated.y.convert(foot)));

      lookahead = follower->findLookaheadPoint(path, {0_ft, 2_ft});
      CHECK(lookahead.y.convert(foot) == Approx(estimated.y.convert(foot)));
    }

    SUBCASE("TestCurvature") {
      auto curvature = MockPathFollower::calculateCurvature({0_in, 0_in, 0_deg}, {0_in, 5_in});
      CHECK(std::abs(curvature) < 1e-4);

      curvature = MockPathFollower::calculateCurvature({0_in, 0_in, 90_deg}, {5_in, 0_in});
      CHECK(std::abs(curvature) < 1e-4);

      curvature = MockPathFollower::calculateCurvature({0_in, 0_in, 90_deg}, {-5_in, 0_in});
      CHECK(std::abs(curvature) < 1e-4);

      curvature = MockPathFollower::calculateCurvature({0_in, 0_in, 45_deg}, {5_in, 5_in});
      CHECK(std::abs(curvature) < 1e-4);

      curvature = MockPathFollower::calculateCurvature({0_in, 0_in, 45_deg}, {-5_in, -5_in});
      CHECK(std::abs(curvature) < 1e-4);

      curvature = MockPathFollower::calculateCurvature({0_in, 0_in, 45_deg}, {10_in, 5_in});
      CHECK(std::abs(curvature) > 2);

      curvature = MockPathFollower::calculateCurvature({0_in, 0_in, -45_deg}, {-5_in, 5_in});
      CHECK(std::abs(curvature) < 1e-4);

      curvature = MockPathFollower::calculateCurvature({0_in, 0_in, 200_deg}, {10_in, 5_in});
      CHECK(std::abs(curvature) > 4);
    }

    SUBCASE("TestVelocityStraight") {
      auto vel = MockPathFollower::calculateVelocity(
        1_mps, 0, ChassisScales({{1_m / 1_pi, 10_m}, 360}), {0_mps, 1_mps2, 10_mps, 1_mps});
      CHECK(vel[0] == 60_rpm);
      CHECK(vel[1] == 60_rpm);
    }

    SUBCASE("TestVelocityCurved") {
      auto vel = MockPathFollower::calculateVelocity(
        1_mps, 1, ChassisScales({{1_m / 1_pi, 10_m}, 360}), {0_mps, 1_mps2, 10_mps, 1_mps});
      CHECK(vel[0] > 60_rpm);
      CHECK(vel[1] < 60_rpm);
    }
  }

  SUBCASE("TestVelConversions") {
    QSpeed robotVel = 1_mps;
    QAngularSpeed leftWheel = (robotVel / 10_cm) * 360_deg;
    CHECK(leftWheel == 600_rpm);
  }

  SUBCASE("ReverseVelConversions") {
    QAngularSpeed wheel = (1_mps / (1_pi * 10_cm)) * 360_deg;
    QSpeed vel = (wheel * 1_pi * 10_cm) / 360_deg;
    CHECK(vel == 1_mps);
  }
}
} // namespace test

pathFollower.hpp

#pragma once
#include "lib7842/api/other/utility.hpp"
#include "okapi/api/chassis/model/chassisModel.hpp"
#include "okapi/api/odometry/odometry.hpp"
#include "pursuitLimits.hpp"
#include "waypoint.hpp"
#include <optional>

namespace lib7842 {

class PathFollower {
public:
  /**
   * PathFollower. Follows a generated path using the Pure Pursuit algorithm.
   *
   * @param imodel         The chassis model.
   * @param iodometry      The odometry.
   * @param ichassisScales The powered wheel scales.
   * @param igearset       The powered wheel gearset multiplied by any external gear ratio.
   * @param ilookahead     The lookahead distance.
   * @param idriveRadius   Optional. The radius from the end of the path to turn off angle
   *                       correction. Defaults to lookahead distance.
   */
  PathFollower(std::shared_ptr<ChassisModel> imodel, std::shared_ptr<Odometry> iodometry,
               const ChassisScales& ichassisScales, const QAngularSpeed& igearset,
               const QLength& ilookahead,
               const std::optional<QLength>& idriveRadius = std::nullopt);

  virtual ~PathFollower() = default;

  /**
   * Follow a pre-generated std::vector<Waypoint>.
   *
   * @param path       The path to follow. Must have velocity setpoints generated by PathGenerator.
   * @param limits     The pursuit limits.
   * @param backwards  Whether to follow the path while driving backwards. The robot will follow the
   *                   exact same path as when driving forward, except it will face 180 degrees and
   *                   drive backwards.
   * @param startSpeed Optional. The starting speed of the robot. Defaults to the min speed of the
   *                   path limits. Used to chain paths together without accelerating from zero.
   */
  void followPath(const std::vector<Waypoint>& path, const PursuitLimits& limits,
                  bool backwards = false, const std::optional<QSpeed>& startSpeed = std::nullopt);

  /**
   * Set the motor mode for the pursuit. Velocity mode is more accurate, but voltage mode is
   * smoother. Default is voltage.
   *
   * @param imode The motor mode
   */
  virtual void setMotorMode(util::motorMode imode);

protected:
  /**
   * Iterator type that points to a waypoint array member.
   */
  using pathIterator_t = std::vector<Waypoint>::const_iterator;

  /**
   * Return an iterator to the point on a path that is closest to a position. Considers all options
   * from the last closest point to one point ahead of the lookahead. Will consider to the end of
   * the path if the end is within the lookahead.
   *
   * @param  path The path
   * @param  pos  The position
   * @return iterator to the closest point
   */
  pathIterator_t findClosest(const std::vector<Waypoint>& path, const Vector& pos);

  /**
   * Return the lookahead point on the path. Updates lastLookIndex and lastLookT.
   *
   * @param  path The path
   * @param  pos  The pos
   * @return The lookahead point
   */
  Vector findLookaheadPoint(const std::vector<Waypoint>& path, const Vector& pos);

  /**
   * Calculate the intersection of a lookahead circle with two points and return the interpolation
   * ratio. Return nullopt if no intersection found.
   *
   * @param  start     The starting point
   * @param  end       The ending point
   * @param  pos       The robot position
   * @param  lookahead The lookahead distance
   * @return The intersection ratio, if found
   */
  static std::optional<double> findIntersectT(const Vector& start, const Vector& end,
                                              const Vector& pos, const QLength& lookahead);

  /**
   * Calculate the curvature from the robot position and heading to the lookahead point.
   *
   * @param  state     The robot state (position and heading)
   * @param  lookPoint The lookahead point
   * @return The curvature
   */
  static double calculateCurvature(const State& state, const Vector& lookPoint);

  /**
   * Calculate the rotational velocity of each wheel given a desired robot velocity and curvature.
   *
   * @param  vel           The robot velocity
   * @param  curvature     The curvature
   * @param  chassisScales The chassis scales
   * @param  limits        The pursuit limits
   * @return The rotational velocity for each wheel.
   */
  static std::valarray<QAngularSpeed> calculateVelocity(const QSpeed& vel, double curvature,
                                                        const ChassisScales& chassisScales,
                                                        const PursuitLimits& limits);

  /**
   * Reset the pursuit members
   */
  void resetPursuit();

  std::shared_ptr<ChassisModel> model {nullptr};
  std::shared_ptr<Odometry> odometry {nullptr};
  const ChassisScales chassisScales;
  const QAngularSpeed gearset;

  const QLength lookahead {0_in};
  const QLength driveRadius {0_in};

  util::motorMode mode {util::motorMode::voltage};

  std::optional<pathIterator_t> lastClosest {std::nullopt};
  size_t lastLookIndex {0};
  double lastLookT {0};
};
} // namespace lib7842