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/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 6336386..1395e5d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -309,6 +309,41 @@
dt_spline_(dt_config_),
dt_line_follow_(dt_config_, localizer->target_selector()) {
event_loop->SetRuntimeRealtimePriority(30);
+ for (size_t ii = 0; ii < trajectory_fetchers_.size(); ++ii) {
+ trajectory_fetchers_[ii].fetcher =
+ event_loop->MakeFetcher<fb::Trajectory>("/drivetrain");
+ }
+}
+
+void DrivetrainLoop::UpdateTrajectoryFetchers() {
+ for (auto &fetcher : trajectory_fetchers_) {
+ const fb::Trajectory *trajectory = fetcher.fetcher.get();
+ // If the current fetcher is already being used by the SplineDrivetrain,
+ // don't touch it.
+ // We have to check both in_use and HasTrajectory because if
+ // in_use is true and HasTrajectory() is false, that implies that the
+ // SplineDrivetrain has finished executing the trajectory and disposed of
+ // it; if in_use is false and HasTrajectory() is true, that implies that
+ // this fetcher is at the same point in the queue as another fetcher, and
+ // that the other fetcher is the one that we are using to keep the message
+ // pinned.
+ // TODO(james): Consider garbage-collecting splines once we run out of
+ // fetchers.
+ if (fetcher.in_use && dt_spline_.HasTrajectory(trajectory)) {
+ continue;
+ }
+ fetcher.in_use = false;
+ // Go through and find the next Trajectory that isn't already held by the
+ // SplineDrivetrain, and add it.
+ while (fetcher.fetcher.FetchNext()) {
+ trajectory = fetcher.fetcher.get();
+ if (!dt_spline_.HasTrajectory(trajectory)) {
+ fetcher.in_use = true;
+ dt_spline_.AddTrajectory(trajectory);
+ break;
+ }
+ }
+ }
}
void DrivetrainLoop::RunIteration(
@@ -326,6 +361,8 @@
filters_.Reset(monotonic_now, position);
}
+ UpdateTrajectoryFetchers();
+
filters_.Correct(monotonic_now, position);
// Set the gear-logging parts of the status