Pre-plan auto splines
(a) Make it so that the drivetrain automatically evicts old splines
(b) Set up auto to preplan splines at construction and after every auto.
Change-Id: I96ddb3a38947da02ad9ddc6fe933b7e85727dc18
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 1395e5d..91f1513 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -316,6 +316,27 @@
}
void DrivetrainLoop::UpdateTrajectoryFetchers() {
+ if (dt_spline_.trajectory_count() >= trajectory_fetchers_.size()) {
+ aos::monotonic_clock::time_point min_time = aos::monotonic_clock::max_time;
+ size_t min_fetcher_index = 0;
+ size_t fetcher_index = 0;
+ // Find the oldest spline to forget.
+ for (auto &fetcher : trajectory_fetchers_) {
+ CHECK_NE(fetcher.fetcher.context().monotonic_event_time,
+ monotonic_clock::min_time);
+ if (fetcher.fetcher.context().monotonic_event_time < min_time &&
+ !dt_spline_.IsCurrentTrajectory(fetcher.fetcher.get())) {
+ min_time = fetcher.fetcher.context().monotonic_event_time;
+ min_fetcher_index = fetcher_index;
+ }
+ ++fetcher_index;
+ }
+
+ dt_spline_.DeleteTrajectory(
+ trajectory_fetchers_[min_fetcher_index].fetcher.get());
+ trajectory_fetchers_[min_fetcher_index].in_use = false;
+ }
+
for (auto &fetcher : trajectory_fetchers_) {
const fb::Trajectory *trajectory = fetcher.fetcher.get();
// If the current fetcher is already being used by the SplineDrivetrain,
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 070cd03..325715f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -144,7 +144,9 @@
class DrivetrainLoop
: public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- static constexpr size_t kNumSplineFetchers = 4;
+ // Note that we only actually store N - 1 splines, since we need to keep one
+ // fetcher free to check whether there are any new splines.
+ static constexpr size_t kNumSplineFetchers = 5;
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index 86318ca..3438fe5 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -11,8 +11,9 @@
{
"name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.fb.Trajectory",
- "max_size": 200000,
- "frequency": 10,
+ "max_size": 400000,
+ "frequency": 2,
+ "num_senders": 2,
"read_method": "PIN",
"num_readers": 6
},
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 7dd5a52..26428ba 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -1399,14 +1399,25 @@
VerifyNearPosition(1.0, 1.0);
}
-// Tests that when we send a bunch of splines we can fill up the internal
-// buffers and that we handle that case sanely.
+// Tests that when we send a bunch of splines we properly evict old splines from
+// the internal buffers.
TEST_F(DrivetrainTest, FillSplineBuffer) {
SetEnabled(true);
- std::vector<int> sent_spline_indices;
- constexpr size_t kExtraSplines = 4;
- for (size_t spline_index = 1;
- spline_index < DrivetrainLoop::kNumSplineFetchers + 1 + kExtraSplines;
+ std::vector<int> expected_splines;
+ constexpr size_t kExtraSplines = 10;
+ constexpr size_t kNumStoredSplines = DrivetrainLoop::kNumSplineFetchers - 1;
+ constexpr int kRunSpline = 1;
+ {
+ // Tell the drivetrain to execute spline 1; we then will check that that
+ // spline never gets evicted.
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(kRunSpline);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+ for (size_t spline_index = 0;
+ spline_index < DrivetrainLoop::kNumSplineFetchers + kExtraSplines;
++spline_index) {
auto builder = trajectory_goal_sender_.MakeBuilder();
@@ -1430,65 +1441,34 @@
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
- RunFor(dt());
+ // Run for at least 2 iterations. Because of how the logic works, there will
+ // actually typically be a single iteration where we store kNumStoredSplines
+ // + 1.
+ RunFor(2 * dt());
- sent_spline_indices.push_back(spline_index);
- }
-
- drivetrain_status_fetcher_.Fetch();
-
- ASSERT_EQ(DrivetrainLoop::kNumSplineFetchers,
- CHECK_NOTNULL(drivetrain_status_fetcher_.get()
- ->trajectory_logging()
- ->available_splines())
- ->size());
- for (size_t ii = 0; ii < DrivetrainLoop::kNumSplineFetchers; ++ii) {
- EXPECT_EQ(sent_spline_indices[ii],
- CHECK_NOTNULL(drivetrain_status_fetcher_.get()
- ->trajectory_logging()
- ->available_splines())
- ->Get(ii));
- }
-
- // Next, start going through and executing all the splines; we should
- // gradually work through the splines.
- for (size_t ii = 0; ii < sent_spline_indices.size(); ++ii) {
- const int current_spline = sent_spline_indices[ii];
- {
- auto builder = drivetrain_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline_handle(current_spline);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ expected_splines.push_back(spline_index);
+ if (expected_splines.size() > kNumStoredSplines) {
+ if (expected_splines.front() != kRunSpline) {
+ expected_splines.erase(expected_splines.begin());
+ } else {
+ expected_splines.erase(expected_splines.begin() + 1);
+ }
}
- // Run for two iterations to give the drivetrain time to resolve all of its
- // internal state for handling splines (coordinating the fetchers and goal
- // message to get this two all happen in 1 message is entirely possible, but
- // has no practical need).
- RunFor(2 * dt());
+ // We should always just have the past kNumStoredSplines available.
drivetrain_status_fetcher_.Fetch();
- ASSERT_EQ(current_spline, drivetrain_status_fetcher_.get()
- ->trajectory_logging()
- ->current_spline_idx())
- << aos::FlatbufferToJson(drivetrain_status_fetcher_.get());
-
- const int num_available_splines = std::min(
- sent_spline_indices.size() - ii, DrivetrainLoop::kNumSplineFetchers);
-
- ASSERT_EQ(num_available_splines,
+ ASSERT_EQ(expected_splines.size(),
CHECK_NOTNULL(drivetrain_status_fetcher_.get()
->trajectory_logging()
->available_splines())
- ->size())
- << aos::FlatbufferToJson(drivetrain_status_fetcher_.get());
- for (int jj = 0; jj < num_available_splines; ++jj) {
- EXPECT_EQ(sent_spline_indices[ii + jj],
+ ->size());
+ for (size_t ii = 0; ii < expected_splines.size(); ++ii) {
+ EXPECT_EQ(expected_splines[ii],
CHECK_NOTNULL(drivetrain_status_fetcher_.get()
->trajectory_logging()
->available_splines())
- ->Get(jj));
+ ->Get(ii));
}
}
}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index cdedce0..1ebda6c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -41,6 +41,12 @@
UpdateSplineHandles();
}
+bool SplineDrivetrain::IsCurrentTrajectory(
+ const fb::Trajectory *trajectory) const {
+ return (current_trajectory_ != nullptr &&
+ current_trajectory().spline_handle() == trajectory->handle());
+}
+
bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
if (trajectory == nullptr) {
return false;
@@ -53,6 +59,19 @@
return false;
}
+void SplineDrivetrain::DeleteTrajectory(const fb::Trajectory *trajectory) {
+ CHECK(trajectory != nullptr);
+
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+ trajectories_.erase(trajectories_.begin() + ii);
+ return;
+ }
+ }
+
+ LOG(FATAL) << "Trying to remove unknown trajectory " << trajectory->handle();
+}
+
void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
trajectories_.emplace_back(
std::make_unique<FinishedTrajectory>(dt_config_, trajectory));
@@ -60,11 +79,9 @@
}
void SplineDrivetrain::DeleteCurrentSpline() {
- CHECK(current_trajectory_index_);
- CHECK_LT(*current_trajectory_index_, trajectories_.size());
- trajectories_.erase(trajectories_.begin() + *current_trajectory_index_);
+ DeleteTrajectory(&CHECK_NOTNULL(current_trajectory_)->trajectory());
executing_spline_ = false;
- current_trajectory_index_.reset();
+ current_trajectory_ = nullptr;
current_xva_.setZero();
}
@@ -92,7 +109,7 @@
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
if (trajectories_[ii]->spline_handle() == *commanded_spline_) {
executing_spline_ = true;
- current_trajectory_index_ = ii;
+ current_trajectory_ = trajectories_[ii].get();
}
}
// If we didn't find the commanded spline in the list of available splines,
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 0170151..303701f 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -32,6 +32,8 @@
// returns false.
bool HasTrajectory(const fb::Trajectory *trajectory) const;
void AddTrajectory(const fb::Trajectory *trajectory);
+ bool IsCurrentTrajectory(const fb::Trajectory *trajectory) const;
+ void DeleteTrajectory(const fb::Trajectory *trajectory);
void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state,
const ::Eigen::Matrix<double, 2, 1> &voltage_error);
@@ -58,6 +60,8 @@
: true;
}
+ size_t trajectory_count() const { return trajectories_.size(); }
+
// Returns true if the splinedrivetrain is enabled.
bool enable() const { return enable_; }
@@ -79,10 +83,7 @@
void DeleteCurrentSpline();
const FinishedTrajectory ¤t_trajectory() const {
- CHECK(current_trajectory_index_);
- CHECK_LE(0u, *current_trajectory_index_);
- CHECK_LT(*current_trajectory_index_, trajectories_.size());
- return *trajectories_[*current_trajectory_index_];
+ return *CHECK_NOTNULL(current_trajectory_);
}
const DrivetrainConfig<double> dt_config_;
@@ -92,7 +93,7 @@
// TODO(james): Sort out construction to avoid so much dynamic memory
// allocation...
std::vector<std::unique_ptr<FinishedTrajectory>> trajectories_;
- std::optional<size_t> current_trajectory_index_;
+ const FinishedTrajectory *current_trajectory_ = nullptr;
std::optional<int> commanded_spline_;