Fetch latest SplineGoal in trajectory_generator
This makes log replay a bit easier when only replaying a single
spline, in case the goal was sent before the log started.
Change-Id: I3e2f420e969ab60c7356879ac808d0143b17a03f
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.cc b/frc971/control_loops/drivetrain/trajectory_generator.cc
index e531167..bb7a53b 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.cc
+++ b/frc971/control_loops/drivetrain/trajectory_generator.cc
@@ -7,10 +7,16 @@
: event_loop_(event_loop),
dt_config_(config),
trajectory_sender_(
- event_loop_->MakeSender<fb::Trajectory>("/drivetrain")) {
+ event_loop_->MakeSender<fb::Trajectory>("/drivetrain")),
+ spline_goal_fetcher_(event_loop->MakeFetcher<SplineGoal>("/drivetrain")) {
event_loop_->MakeWatcher("/drivetrain", [this](const SplineGoal &goal) {
HandleSplineGoal(goal);
});
+ event_loop_->OnRun([this]() {
+ if (spline_goal_fetcher_.Fetch()) {
+ HandleSplineGoal(*spline_goal_fetcher_.get());
+ }
+ });
}
void TrajectoryGenerator::HandleSplineGoal(const SplineGoal &goal) {
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.h b/frc971/control_loops/drivetrain/trajectory_generator.h
index 5d638d3..f3498bd 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.h
+++ b/frc971/control_loops/drivetrain/trajectory_generator.h
@@ -18,6 +18,7 @@
aos::EventLoop *const event_loop_;
const DrivetrainConfig<double> dt_config_;
aos::Sender<fb::Trajectory> trajectory_sender_;
+ aos::Fetcher<SplineGoal> spline_goal_fetcher_;
};
} // namespace frc971::control_loops::drivetrain