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/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 1da9db3..b08e96f 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -6,10 +6,20 @@
load("@npm_bazel_typescript//:defs.bzl", "ts_library")
flatbuffer_cc_library(
+ name = "spline_goal_fbs",
+ srcs = ["spline_goal.fbs"],
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+flatbuffer_cc_library(
name = "drivetrain_goal_fbs",
srcs = ["drivetrain_goal.fbs"],
gen_reflections = 1,
- includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+ includes = [
+ ":spline_goal_fbs_includes",
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
)
flatbuffer_cc_library(
@@ -31,6 +41,13 @@
includes = ["//frc971/control_loops:control_loops_fbs_includes"],
)
+flatbuffer_cc_library(
+ name = "trajectory_fbs",
+ srcs = ["trajectory.fbs"],
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
flatbuffer_ts_library(
name = "drivetrain_status_ts_fbs",
srcs = ["drivetrain_status.fbs"],
@@ -70,7 +87,10 @@
name = "drivetrain_goal_float_fbs",
srcs = ["drivetrain_goal_float.fbs"],
gen_reflections = 1,
- includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+ includes = [
+ ":spline_goal_fbs_includes",
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
)
flatbuffer_cc_library(
@@ -118,6 +138,8 @@
src = "drivetrain_config.json",
flatbuffers = [
":drivetrain_goal_fbs",
+ ":trajectory_fbs",
+ ":spline_goal_fbs",
":drivetrain_output_fbs",
":drivetrain_status_fbs",
":drivetrain_position_fbs",
@@ -220,6 +242,7 @@
":drivetrain_output_fbs",
":drivetrain_status_fbs",
":spline",
+ ":spline_goal_fbs",
":trajectory",
"//aos:condition",
"//aos:init",
@@ -243,6 +266,7 @@
":drivetrain_output_fbs",
":drivetrain_status_fbs",
":localizer",
+ ":spline_goal_fbs",
"//aos:math",
"//aos/util:math",
"//frc971/control_loops:c2d",
@@ -289,6 +313,7 @@
":drivetrain_status_fbs",
":gear",
":localizer",
+ ":spline_goal_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
@@ -322,6 +347,7 @@
"//aos/controls:polytope",
"//frc971/control_loops:coerce_goal",
"//frc971/control_loops:control_loops_fbs",
+ ":spline_goal_fbs",
"//frc971/control_loops:state_feedback_loop",
] + select({
"@platforms//os:linux": [
@@ -396,6 +422,7 @@
":localizer",
":localizer_fbs",
":polydrivetrain",
+ ":spline_goal_fbs",
":splinedrivetrain",
":ssdrivetrain",
"//aos/controls:control_loop",
@@ -462,6 +489,7 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
":drivetrain_config",
+ ":trajectory_generator",
":drivetrain_lib",
":localizer_fbs",
":drivetrain_goal_fbs",
@@ -568,8 +596,11 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
":spline",
+ ":trajectory_fbs",
"//aos/logging",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:fixed_quadrature",
+ "@com_github_google_glog//:glog",
"@org_tuxfamily_eigen//:eigen",
],
)
@@ -591,6 +622,7 @@
deps = [
":distance_spline",
"//aos/testing:googletest",
+ "//aos:flatbuffers",
"//aos/testing:test_shm",
"@com_github_gflags_gflags//:gflags",
] + cpu_select({
@@ -609,6 +641,8 @@
deps = [
":distance_spline",
":drivetrain_config",
+ ":spline_goal_fbs",
+ ":trajectory_fbs",
"//frc971/control_loops:c2d",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:hybrid_state_feedback_loop",
@@ -618,6 +652,20 @@
],
)
+cc_library(
+ name = "trajectory_generator",
+ srcs = ["trajectory_generator.cc"],
+ hdrs = ["trajectory_generator.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":distance_spline",
+ ":drivetrain_config",
+ ":spline_goal_fbs",
+ ":trajectory",
+ ":trajectory_fbs",
+ ],
+)
+
cc_binary(
name = "trajectory_plot",
srcs = [