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/y2019/BUILD b/y2019/BUILD
index 3f361a7..0954157 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -14,6 +14,7 @@
":joystick_reader",
":wpilib_interface",
"//y2019/control_loops/drivetrain:drivetrain",
+ "//y2019/control_loops/drivetrain:trajectory_generator",
"//y2019/control_loops/superstructure:superstructure",
"//y2019/actors:binaries",
"//y2019/vision/server",
diff --git a/y2019/actors/auto_splines.cc b/y2019/actors/auto_splines.cc
index 397229c..67f9e85 100644
--- a/y2019/actors/auto_splines.cc
+++ b/y2019/actors/auto_splines.cc
@@ -6,10 +6,10 @@
namespace actors {
void MaybeFlipSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset,
bool is_left) {
-
flatbuffers::Vector<float> *spline_y =
GetMutableTemporaryPointer(*builder->fbb(), spline_y_offset);
@@ -22,7 +22,8 @@
// Path off of level 2 to the far side of the rocket with a panel
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HABToFarRocket(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
@@ -97,7 +98,8 @@
// Path from the far side of the rocket to the loading station to pickup
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToHP(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
@@ -170,7 +172,8 @@
// Path from the human player station to the far side of the rocket with a panel
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToFarRocket(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
@@ -245,7 +248,8 @@
// Path from the far side of the rocket to close to the loading station
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToNearHP(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
// TODO(theo): Add some real constraints.
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
@@ -269,8 +273,10 @@
}
// Path from level 2 to 2nd cargo ship bay with a hatch panel
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HABToSecondCargoShipBay(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::HABToSecondCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
@@ -335,8 +341,10 @@
}
// Path from 2nd cargo ship bay to loading station
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SecondCargoShipBayToHP(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::SecondCargoShipBayToHP(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
@@ -390,8 +398,10 @@
}
// Path from loading station to 3rd cargo ship bay with a hatch panel
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToThirdCargoShipBay(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::HPToThirdCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
@@ -456,8 +466,10 @@
}
// Path from 3rd cargo ship bay to near the loading station
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::ThirdCargoShipBayToNearHP(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::ThirdCargoShipBayToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
@@ -500,7 +512,8 @@
}
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HabToFarRocketTest(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
@@ -574,7 +587,8 @@
}
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToHPTest(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
@@ -642,7 +656,8 @@
}
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToNearRocketTest(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
@@ -710,7 +725,8 @@
}
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
@@ -772,7 +788,8 @@
}
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder->fbb()->CreateVector<float>(
{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
diff --git a/y2019/actors/auto_splines.h b/y2019/actors/auto_splines.h
index 5d79ba9..7b56c11 100644
--- a/y2019/actors/auto_splines.h
+++ b/y2019/actors/auto_splines.h
@@ -20,59 +20,72 @@
// Path off of level 2 to the far side of the rocket with a panel
static flatbuffers::Offset<frc971::MultiSpline> HABToFarRocket(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
// Path from the far side of the rocket to the loading station to pickup
static flatbuffers::Offset<frc971::MultiSpline> FarRocketToHP(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
// Path from the far side of the rocket to the loading station to pickup
static flatbuffers::Offset<frc971::MultiSpline> HPToFarRocket(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
// Path from the far side of the rocket to close to the loading station
static flatbuffers::Offset<frc971::MultiSpline> FarRocketToNearHP(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
// Splines for 2 Panels on the far reaches of the cargo ship
// Path from level 2 to 2nd cargo ship bay with a hatch panel
static flatbuffers::Offset<frc971::MultiSpline> HABToSecondCargoShipBay(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
// Path from 2nd cargo ship bay to loading station
static flatbuffers::Offset<frc971::MultiSpline> SecondCargoShipBayToHP(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
// Path from loading station to 3rd cargo ship bay with a hatch panel
static flatbuffers::Offset<frc971::MultiSpline> HPToThirdCargoShipBay(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
// Path from 3rd cargo ship bay to near the loading station
static flatbuffers::Offset<frc971::MultiSpline> ThirdCargoShipBayToNearHP(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
// Testing Splines
static flatbuffers::Offset<frc971::MultiSpline> HPToNearRocketTest(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder);
static flatbuffers::Offset<frc971::MultiSpline> HabToFarRocketTest(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
bool is_left);
static flatbuffers::Offset<frc971::MultiSpline> FarRocketToHPTest(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder);
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder);
static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder);
};
} // namespace actors
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index aa2c808..078ad64 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -144,11 +144,12 @@
template <typename Functor>
std::function<flatbuffers::Offset<frc971::MultiSpline>(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder)>
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder)>
BindIsLeft(Functor f, bool is_left) {
- return
- [is_left, f](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
- *builder) { return f(builder, is_left); };
+ return [is_left,
+ f](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) { return f(builder, is_left); };
}
bool AutonomousActor::RunAction(
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index c21433b..9a0e879 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -74,6 +74,21 @@
)
cc_binary(
+ name = "trajectory_generator",
+ srcs = [
+ "trajectory_generator_main.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":drivetrain_base",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/control_loops/drivetrain:trajectory_generator",
+ ],
+)
+
+cc_binary(
name = "drivetrain",
srcs = [
"drivetrain_main.cc",
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index b811688..87dbaee 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -142,10 +142,8 @@
}
void SetUp() {
- flatbuffers::DetachedBuffer goal_buffer;
{
flatbuffers::FlatBufferBuilder fbb;
-
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
fbb.CreateVector<float>(GetParam().control_pts_x.begin(),
GetParam().control_pts_x.size());
@@ -169,12 +167,31 @@
spline_builder.add_spline_idx(1);
spline_builder.add_spline(multispline_offset);
- flatbuffers::Offset<frc971::control_loops::drivetrain::SplineGoal>
- spline_offset = spline_builder.Finish();
+ fbb.Finish(spline_builder.Finish());
+ aos::FlatbufferDetachedBuffer<
+ frc971::control_loops::drivetrain::SplineGoal>
+ spline_goal_buffer(fbb.Release());
+
+ frc971::control_loops::drivetrain::Trajectory trajectory(
+ spline_goal_buffer.message(), dt_config_);
+ trajectory.Plan();
+
+ flatbuffers::FlatBufferBuilder traj_fbb;
+
+ traj_fbb.Finish(trajectory.Serialize(&traj_fbb));
+
+ trajectory_ = std::make_unique<aos::FlatbufferDetachedBuffer<
+ frc971::control_loops::drivetrain::fb::Trajectory>>(
+ traj_fbb.Release());
+ spline_drivetrain_.AddTrajectory(&trajectory_->message());
+ }
+
+ flatbuffers::DetachedBuffer goal_buffer;
+ {
+ flatbuffers::FlatBufferBuilder fbb;
frc971::control_loops::drivetrain::Goal::Builder goal_builder(fbb);
- goal_builder.add_spline(spline_offset);
goal_builder.add_controller_type(
frc971::control_loops::drivetrain::ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
@@ -186,33 +203,6 @@
aos::FlatbufferDetachedBuffer<frc971::control_loops::drivetrain::Goal> goal(
std::move(goal_buffer));
- // Let the spline drivetrain compute the spline.
- while (true) {
- // We need to keep sending the goal. There are conditions when the
- // trajectory lock isn't grabbed the first time, and we want to keep
- // banging on it to keep trying. Otherwise we deadlock.
- spline_drivetrain_.SetGoal(&goal.message());
-
- ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
-
- flatbuffers::FlatBufferBuilder fbb;
-
- flatbuffers::Offset<frc971::control_loops::drivetrain::TrajectoryLogging>
- trajectory_logging_offset =
- spline_drivetrain_.MakeTrajectoryLogging(&fbb);
-
- ::frc971::control_loops::drivetrain::Status::Builder status_builder(fbb);
- status_builder.add_trajectory_logging(trajectory_logging_offset);
- spline_drivetrain_.PopulateStatus(&status_builder);
- fbb.Finish(status_builder.Finish());
- aos::FlatbufferDetachedBuffer<::frc971::control_loops::drivetrain::Status>
- status(fbb.Release());
-
- if (status.message().trajectory_logging()->planning_state() ==
- ::frc971::control_loops::drivetrain::PlanningState::PLANNED) {
- break;
- }
- }
spline_drivetrain_.SetGoal(&goal.message());
}
@@ -357,6 +347,9 @@
TestLocalizer localizer_;
+ std::unique_ptr<aos::FlatbufferDetachedBuffer<
+ frc971::control_loops::drivetrain::fb::Trajectory>>
+ trajectory_;
::frc971::control_loops::drivetrain::SplineDrivetrain spline_drivetrain_;
// All the data we want to end up plotting.
diff --git a/y2019/control_loops/drivetrain/trajectory_generator_main.cc b/y2019/control_loops/drivetrain/trajectory_generator_main.cc
new file mode 100644
index 0000000..cc3d2bf
--- /dev/null
+++ b/y2019/control_loops/drivetrain/trajectory_generator_main.cc
@@ -0,0 +1,39 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::TrajectoryGenerator;
+
+DEFINE_bool(skip_renicing, false,
+ "If true, skip renicing the trajectory generator.");
+
+int main(int argc, char *argv[]) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ TrajectoryGenerator generator(
+ &event_loop, ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
+
+ event_loop.OnRun([]() {
+ if (FLAGS_skip_renicing) {
+ LOG(WARNING) << "Ignoring request to renice to -20 due to "
+ "--skip_renicing.";
+ } else {
+ errno = 0;
+ setpriority(PRIO_PROCESS, 0, -20);
+ PCHECK(errno == 0)
+ << ": Renicing to -20 failed, use --skip_renicing to skip renicing.";
+ }
+ });
+
+ event_loop.Run();
+
+ return 0;
+}