Fix Doxygen errors

trajoptlib/include/trajopt/path/PathBuilder.hpp

@@ -222,6 +222,12 @@ class TRAJOPT_DLLEXPORT PathBuilder {
                                                 controlIntervalCounts);
   }
 
+  /**
+   * Calculate a discrete, spline initial guess of the x, y, and heading of the
+   * robot that goes through each segment.
+   *
+   * @return the initial guess, as a solution
+   */
   Solution CalculateSplineInitialGuess() const {
     return GenerateSplineInitialGuess<Solution>(initialGuessPoints,
                                                 controlIntervalCounts);

trajoptlib/include/trajopt/spline/CubicHermitePoseSplineHolonomic.hpp

@@ -2,9 +2,7 @@
 
 #pragma once
 
-#include <algorithm>
 #include <utility>
-#include <vector>
 
 #include "trajopt/geometry/Pose2.hpp"
 #include "trajopt/geometry/Rotation2.hpp"
@@ -14,14 +12,30 @@
 #include "trajopt/util/SymbolExports.hpp"
 
 namespace frc {
+
 /**
  * Represents a cubic pose spline, which is a specific implementation of a cubic
  * hermite spline.
  */
 class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
  public:
+  /// Pose2d with curvature.
   using PoseWithCurvature = std::pair<trajopt::Pose2d, double>;
 
+  /**
+   * Constructs a cubic pose spline.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   *     the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   *     the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   *     the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   *     the y dimension.
+   * @param r0 Initial heading.
+   * @param r1 Final heading.
+   */
   CubicHermitePoseSplineHolonomic(wpi::array<double, 2> xInitialControlVector,
                                   wpi::array<double, 2> xFinalControlVector,
                                   wpi::array<double, 2> yInitialControlVector,
@@ -33,22 +47,41 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
         r0(r0),
         theta(0.0, (-r0).RotateBy(r1).Radians(), 0, 0) {}
 
-  trajopt::Rotation2d getCourse(double t) const {
+  /**
+   * Return course at point t.
+   *
+   * @param t The point t
+   * @return The course at point t.
+   */
+  trajopt::Rotation2d GetCourse(double t) const {
     const PoseWithCurvature splinePoint =
         CubicHermiteSpline::GetPoint(t).value();
     return splinePoint.first.Rotation();
   }
 
-  trajopt::Rotation2d getHeading(double t) const {
-    return r0.RotateBy(trajopt::Rotation2d(theta.getPosition(t)));
+  /**
+   * Return heading at point t.
+   *
+   * @param t The point t
+   * @return The heading at point t.
+   */
+  trajopt::Rotation2d GetHeading(double t) const {
+    return r0.RotateBy(trajopt::Rotation2d(theta.GetPosition(t)));
   }
 
-  double getDHeading(double t) const { return theta.getVelocity(t); }
+  /**
+   * Return heading rate at point t.
+   *
+   * @param t The point t
+   * @return The heading rate at point t.
+   */
+  double getDHeading(double t) const { return theta.GetVelocity(t); }
 
   /**
    * Gets the pose and curvature at some point t on the spline.
    *
    * @param t The point t
+   * @param isDifferential Whether the drivetrain is a differential drive.
    * @return The pose and curvature at that point.
    */
   std::optional<PoseWithCurvature> GetPoint(double t,
@@ -57,7 +90,7 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
       return CubicHermiteSpline::GetPoint(t);
     } else {
       const auto splinePoint = CubicHermiteSpline::GetPoint(t).value();
-      return PoseWithCurvature{{splinePoint.first.Translation(), getHeading(t)},
+      return PoseWithCurvature{{splinePoint.first.Translation(), GetHeading(t)},
                                splinePoint.second};
     }
   }
@@ -66,4 +99,5 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
   trajopt::Rotation2d r0;
   trajopt::CubicHermiteSpline1d theta;
 };
+
 }  // namespace frc

trajoptlib/include/trajopt/spline/CubicHermiteSpline1d.hpp

@@ -9,28 +9,43 @@
 
 namespace trajopt {
 
+/**
+ * 1D cubic hermite spline.
+ */
 class TRAJOPT_DLLEXPORT CubicHermiteSpline1d : public Spline1d {
  public:
-  // Coefficients of the cubic polynomial
-  const double a, b, c, d;
-
+  /**
+   * Constructs a 1D cubic hermite spline.
+   *
+   * @param p0 The initial position.
+   * @param p1 The final position.
+   * @param v0 The initial velocity.
+   * @param v1 The final velocity.
+   */
   CubicHermiteSpline1d(double p0, double p1, double v0, double v1)
       : a(v0 + v1 + 2 * p0 - 2 * p1),
         b(-2 * v0 - v1 - 3 * p0 + 3 * p1),
         c(v0),
         d(p0) {}
 
-  double getPosition(double t) const override {
+  double GetPosition(double t) const override {
     return a * std::pow(t, 3) + b * std::pow(t, 2) + c * t + d;
   }
 
-  double getVelocity(double t) const override {
+  double GetVelocity(double t) const override {
     return 3 * a * std::pow(t, 2) + 2 * b * t + c;
   }
 
-  double getAcceleration(double t) const override { return 6 * a * t + 2 * b; }
+  double GetAcceleration(double t) const override { return 6 * a * t + 2 * b; }
 
-  double getJerk([[maybe_unused]] double t) const override { return 6 * a; }
+  double GetJerk([[maybe_unused]] double t) const override { return 6 * a; }
+
+ private:
+  // Coefficients of the cubic polynomial
+  double a;
+  double b;
+  double c;
+  double d;
 };
 
 }  // namespace trajopt

trajoptlib/include/trajopt/spline/Spline.hpp

@@ -19,16 +19,9 @@ namespace frc {
 template <int Degree>
 class Spline {
  public:
+  /// Pose2d with curvature
   using PoseWithCurvature = std::pair<trajopt::Pose2d, double>;
 
-  Spline() = default;
-
-  Spline(const Spline&) = default;
-  Spline& operator=(const Spline&) = default;
-
-  Spline(Spline&&) = default;
-  Spline& operator=(Spline&&) = default;
-
   virtual ~Spline() = default;
 
   /**

trajoptlib/include/trajopt/spline/Spline1d.hpp

@@ -6,19 +6,44 @@
 
 namespace trajopt {
 
+/**
+ * 1D spline.
+ */
 class TRAJOPT_DLLEXPORT Spline1d {
  public:
-  virtual ~Spline1d() {}
-
-  virtual double getPosition(double t) const = 0;
-
-  // ds/dt
-  virtual double getVelocity(double t) const = 0;
-
-  // ds²/dt
-  virtual double getAcceleration(double t) const = 0;
-
-  // ds³/dt
-  virtual double getJerk(double t) const = 0;
+  virtual ~Spline1d() = default;
+
+  /**
+   * Return the position at point t.
+   *
+   * @param t The point t
+   * @return The position at point t.
+   */
+  virtual double GetPosition(double t) const = 0;
+
+  /**
+   * Return the velocity at point t.
+   *
+   * @param t The point t
+   * @return The velocity at point t.
+   */
+  virtual double GetVelocity(double t) const = 0;
+
+  /**
+   * Return the acceleration at point t.
+   *
+   * @param t The point t
+   * @return The acceleration at point t.
+   */
+  virtual double GetAcceleration(double t) const = 0;
+
+  /**
+   * Return the jerk at point t.
+   *
+   * @param t The point t
+   * @return The jerk at point t.
+   */
+  virtual double GetJerk(double t) const = 0;
 };
+
 }  // namespace trajopt

trajoptlib/include/trajopt/spline/array.hpp

@@ -23,31 +23,81 @@ constexpr empty_array_t empty_array;
 template <typename T, size_t N>
 class array : public std::array<T, N> {
  public:
+  /**
+   * Constructs an uninitialized array.
+   */
   constexpr explicit array(empty_array_t) {}
 
+  /**
+   * Constructs an array initialized with the given elements.
+   *
+   * @param arg The first element.
+   * @param args The remaining elements.
+   */
   template <std::convertible_to<T>... Ts>
     requires(1 + sizeof...(Ts) == N)
   constexpr array(T arg, Ts&&... args)  // NOLINT
       : std::array<T, N>{std::forward<T>(arg), std::forward<Ts>(args)...} {}
 
+  /**
+   * Copy constructor.
+   */
   constexpr array(const array<T, N>&) = default;
+
+  /**
+   * Copy assignment operator.
+   *
+   * @return The left-hand side.
+   */
   constexpr array& operator=(const array<T, N>&) = default;
+
+  /**
+   * Move constructor.
+   */
   constexpr array(array<T, N>&&) = default;
+
+  /**
+   * Move assignment operator.
+   *
+   * @return The left-hand side.
+   */
   constexpr array& operator=(array<T, N>&&) = default;
 
+  /**
+   * Copy constructor for std::array.
+   *
+   * @param rhs The std::array to copy.
+   */
   constexpr array(const std::array<T, N>& rhs) {  // NOLINT
     *static_cast<std::array<T, N>*>(this) = rhs;
   }
 
+  /**
+   * Copy assignment operator for std::array.
+   *
+   * @param rhs The std::array to copy.
+   * @return The left-hand side.
+   */
   constexpr array& operator=(const std::array<T, N>& rhs) {
     *static_cast<std::array<T, N>*>(this) = rhs;
     return *this;
   }
 
+  /**
+   * Move constructor for std::array.
+   *
+   * @param rhs The std::array to move.
+   */
   constexpr array(std::array<T, N>&& rhs) {  // NOLINT
     *static_cast<std::array<T, N>*>(this) = rhs;
   }
 
+  /**
+   * Move assignment operator for std::array.
+   *
+   * @param rhs The std::array to move.
+   * @return The left-hand side.
+   */
   constexpr array& operator=(std::array<T, N>&& rhs) {
     *static_cast<std::array<T, N>*>(this) = rhs;
     return *this;
@@ -85,14 +135,19 @@ constexpr const T&& get(const wpi::array<T, N>&& arr) noexcept {
 
 // Enables structured bindings
 namespace std {  // NOLINT
-// Partial specialization for wpi::array
+/**
+ * Partial specialization of tuple_size for wpi::array.
+ */
 template <typename T, size_t N>
 struct tuple_size<wpi::array<T, N>> : public integral_constant<size_t, N> {};
 
-// Partial specialization for wpi::array
+/**
+ * Partial specialization of tuple_element for wpi::array.
+ */
 template <size_t I, typename T, size_t N>
   requires(I < N)
 struct tuple_element<I, wpi::array<T, N>> {
+  /// Type trait.
   using type = T;
 };
 }  // namespace std

trajoptlib/src/DifferentialTrajectoryGenerator.cpp

@@ -5,6 +5,7 @@
 #include <algorithm>
 #include <cmath>
 #include <ranges>
+#include <thread>
 #include <utility>
 #include <vector>
 
@@ -298,6 +299,7 @@ DifferentialTrajectoryGenerator::Generate(bool diagnostics) {
     for (auto& callback : callbacks) {
       callback();
     }
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
     return trajopt::GetCancellationFlag();
   });