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/autonomous/BUILD b/frc971/autonomous/BUILD
index 414f817..c9eb49e 100644
--- a/frc971/autonomous/BUILD
+++ b/frc971/autonomous/BUILD
@@ -38,6 +38,7 @@
"//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/control_loops/drivetrain:spline_goal_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
],
)
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 7df6998..bb81194 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -33,6 +33,8 @@
"/drivetrain")),
drivetrain_goal_sender_(
event_loop->MakeSender<drivetrain::Goal>("/drivetrain")),
+ spline_goal_sender_(
+ event_loop->MakeSender<drivetrain::SplineGoal>("/drivetrain")),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<drivetrain::Status>("/drivetrain")),
drivetrain_goal_fetcher_(
@@ -443,8 +445,8 @@
BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
std::function<flatbuffers::Offset<frc971::MultiSpline>(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder)>
- &&multispline_builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder)> &&multispline_builder,
SplineDirection direction) {
AOS_LOG(INFO, "Planning spline\n");
@@ -452,7 +454,7 @@
drivetrain_goal_fetcher_.Fetch();
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = spline_goal_sender_.MakeBuilder();
flatbuffers::Offset<frc971::MultiSpline> multispline_offset =
multispline_builder(&builder);
@@ -464,24 +466,7 @@
SplineDirection::kBackward);
spline_builder.add_spline(multispline_offset);
- flatbuffers::Offset<drivetrain::SplineGoal> spline_offset =
- spline_builder.Finish();
-
- drivetrain::Goal::Builder goal_builder =
- builder.MakeBuilder<drivetrain::Goal>();
-
- drivetrain::ControllerType controller_type =
- drivetrain::ControllerType::SPLINE_FOLLOWER;
- if (drivetrain_goal_fetcher_.get()) {
- controller_type = drivetrain_goal_fetcher_->controller_type();
- goal_builder.add_throttle(drivetrain_goal_fetcher_->throttle());
- }
- goal_builder.add_controller_type(controller_type);
- goal_builder.add_spline_handle(goal_spline_handle_);
-
- goal_builder.add_spline(spline_offset);
-
- builder.Send(goal_builder.Finish());
+ builder.Send(spline_builder.Finish());
return BaseAutonomousActor::SplineHandle(spline_handle, this);
}
@@ -492,13 +477,18 @@
base_autonomous_actor_->drivetrain_status_fetcher_.get());
if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
- ((base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
- ->planning_spline_idx() == spline_handle_ &&
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
- ->planning_state() == drivetrain::PlanningState::PLANNED) ||
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
- ->current_spline_idx() == spline_handle_)) {
- return true;
+ base_autonomous_actor_->drivetrain_status_fetcher_.get()
+ ->has_trajectory_logging() &&
+ base_autonomous_actor_->drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->has_available_splines()) {
+ const flatbuffers::Vector<int> *splines =
+ base_autonomous_actor_->drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->available_splines();
+
+ return std::find(splines->begin(), splines->end(), spline_handle_) !=
+ splines->end();
}
return false;
}
@@ -547,8 +537,7 @@
->is_executed() &&
base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
->current_spline_idx() == spline_handle_) ||
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
- ->planning_spline_idx() == spline_handle_)) {
+ IsPlanned())) {
return false;
}
return true;
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 9444389..2478fda 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -64,7 +64,7 @@
// it.
SplineHandle PlanSpline(
std::function<flatbuffers::Offset<frc971::MultiSpline>(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder)> &&multispline_builder,
SplineDirection direction);
@@ -123,6 +123,8 @@
target_selector_hint_sender_;
::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::SplineGoal>
+ spline_goal_sender_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>