Move the trajectory computation into another thread.
Change-Id: I9dff7a20752e6cdfe05ec71d3435f3006ae45353
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 45ae714..3b09e64 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -150,6 +150,16 @@
::std::copy(GetParam().control_pts_y.begin(),
GetParam().control_pts_y.end(), goal.spline.spline_y.begin());
spline_drivetrain_.SetGoal(goal);
+
+ // Let the spline drivetrain compute the spline.
+ ::frc971::control_loops::DrivetrainQueue::Status status;
+ do {
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
+ spline_drivetrain_.PopulateStatus(&status);
+ } while (status.trajectory_logging.planning_state !=
+ (int8_t)::frc971::control_loops::drivetrain::SplineDrivetrain::
+ PlanState::kPlannedTrajectory);
+ spline_drivetrain_.SetGoal(goal);
}
void TearDown() {