Refactor trajectory generation to separate process
This pulls all the trajectory planning into a TrajectoryGenerator class,
which produces a Trajectory spline that the drivetrain code can consume
and use to track the spline.
Broadly speaking, this change:
- Separates the Trajectory class into a generation class and a
FinishedTrajectory class, where the generator produces a flatbuffer
and the FinishedTrajectory reads all the required information from
the flatbuffer.
- Add an option for serialization/deserialization of a DistanceSpline.
- Removes some dead code from Trajectory class (mostly having to do with
the old feedback algorithm).
- Uses floats in more places, to keep the size of the Trajectory
flatbuffer under control
- Update the tests & autonomous code to use the new spline code.
Further work that may make sense:
- Experiment with alternatives to current structure of the Trajectory
flatbuffer to see whether (a) the size is a problem; and (b) if so,
what we should do about it.
- Add shims to allow replaying logfiles with old-style spline goals.
Change-Id: Ic80ce4e384ec4d1bd22940580e3652ecd305b352
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index ab88560..0feb902 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -5,12 +5,15 @@
#include "Eigen/Dense"
#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory_generated.h"
#include "frc971/control_loops/fixed_quadrature.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
+std::vector<Spline> FlatbufferToSplines(const MultiSpline *fb);
+
// Class to hold a spline as a function of distance.
class DistanceSpline {
public:
@@ -18,6 +21,15 @@
DistanceSpline(const Spline &spline, int num_alpha = 0);
DistanceSpline(::std::vector<Spline> &&splines, int num_alpha = 0);
+ DistanceSpline(const MultiSpline *fb, int num_alpha = 0);
+ // Copies the distances for the spline directly out of the provided buffer,
+ // rather than constructing the distances from the original splines.
+ DistanceSpline(const fb::DistanceSpline &fb);
+
+ flatbuffers::Offset<fb::DistanceSpline> Serialize(
+ flatbuffers::FlatBufferBuilder *fbb,
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Constraint>>>
+ constraints) const;
// Returns a point on the spline as a function of distance.
::Eigen::Matrix<double, 2, 1> XY(double distance) const {
@@ -61,6 +73,9 @@
// Returns the length of the path in meters.
double length() const { return distances_.back(); }
+ const std::vector<float> &distances() const { return distances_; }
+ const std::vector<Spline> &splines() const { return splines_; }
+
private:
struct AlphaAndIndex {
size_t index;
@@ -70,12 +85,12 @@
// Computes alpha for a distance
AlphaAndIndex DistanceToAlpha(double distance) const;
- ::std::vector<double> BuildDistances(size_t num_alpha);
+ ::std::vector<float> BuildDistances(size_t num_alpha);
// The spline we are converting to a distance.
const ::std::vector<Spline> splines_;
// An interpolation table of distances evenly distributed in alpha.
- const ::std::vector<double> distances_;
+ const ::std::vector<float> distances_;
};
} // namespace drivetrain