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/y2020/BUILD b/y2020/BUILD
index 024b6ac..90d976b 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -12,7 +12,7 @@
":config",
],
dirs = [
- "//y2020/actors:splines"
+ "//y2020/actors:splines",
],
start_binaries = [
"//aos/events/logging:logger_main",
@@ -22,6 +22,7 @@
"//aos/network:message_bridge_server",
"//y2020/actors:binaries",
"//y2020/control_loops/drivetrain:drivetrain",
+ "//y2020/control_loops/drivetrain:trajectory_generator",
"//y2020/control_loops/superstructure:superstructure",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index dbea31a..00e3bd0 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -6,7 +6,8 @@
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 =
@@ -20,7 +21,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,
aos::Alliance alliance) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
@@ -89,7 +91,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/y2020/actors/auto_splines.h b/y2020/actors/auto_splines.h
index 5f53651..891228b 100644
--- a/y2020/actors/auto_splines.h
+++ b/y2020/actors/auto_splines.h
@@ -43,63 +43,76 @@
"splines/autonav_slalom.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
aos::Alliance alliance);
static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder);
flatbuffers::Offset<frc971::MultiSpline> TestSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> SplineRedA(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(spline_red_a_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> SplineBlueA(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(spline_blue_a_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> SplineRedB(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(spline_red_b_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> SplineBlueB(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(spline_blue_b_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> AutoNavBounce1(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(autonav_bounce_1_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> AutoNavBounce2(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(autonav_bounce_2_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> AutoNavBounce3(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(autonav_bounce_3_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> AutoNavBounce4(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(autonav_bounce_4_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> AutoNavBarrel(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(autonav_barrel_,
builder->fbb());
}
flatbuffers::Offset<frc971::MultiSpline> AutoNavSlalom(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
return aos::CopyFlatBuffer<frc971::MultiSpline>(autonav_slalom_,
builder->fbb());
}
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 4f04961..c59aa7a 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -104,8 +104,9 @@
AOS_LOG(ERROR, "The galactic search path is unknown, doing nothing.");
} else {
SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
- *builder) {
+ [this](
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder) {
flatbuffers::Offset<frc971::MultiSpline> target_spline;
if (path_fetcher_->alliance() == y2020::vision::Alliance::kRed) {
if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
@@ -147,7 +148,7 @@
void AutonomousActor::AutoNavBounce() {
SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) {
flatbuffers::Offset<frc971::MultiSpline> target_spline =
auto_splines_.AutoNavBounce1(builder);
@@ -162,7 +163,7 @@
spline1.Start();
SplineHandle spline2 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) { return auto_splines_.AutoNavBounce2(builder); },
SplineDirection::kBackward);
@@ -172,7 +173,7 @@
spline2.Start();
SplineHandle spline3 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) { return auto_splines_.AutoNavBounce3(builder); },
SplineDirection::kForward);
@@ -182,7 +183,7 @@
spline3.Start();
SplineHandle spline4 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) { return auto_splines_.AutoNavBounce4(builder); },
SplineDirection::kBackward);
@@ -196,7 +197,7 @@
void AutonomousActor::AutoNavBarrel() {
SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) {
flatbuffers::Offset<frc971::MultiSpline> target_spline;
target_spline = auto_splines_.AutoNavBarrel(builder);
@@ -215,7 +216,7 @@
void AutonomousActor::AutoNavSlalom() {
SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) {
flatbuffers::Offset<frc971::MultiSpline> target_spline;
target_spline = auto_splines_.AutoNavSlalom(builder);
@@ -234,7 +235,7 @@
void AutonomousActor::SplineAuto() {
SplineHandle spline1 = PlanSpline(
- [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ [this](aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) {
flatbuffers::Offset<frc971::MultiSpline> target_spline;
target_spline = auto_splines_.TestSpline(builder);
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 75a8475..01dd1a3 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -91,6 +91,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",
@@ -174,6 +189,7 @@
"//aos/events/logging:log_reader",
"//aos/events/logging:log_writer",
"//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain:trajectory_generator",
"@com_github_gflags_gflags//:gflags",
"@com_github_google_glog//:glog",
],
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index 9b2375b..fc50556 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -12,6 +12,7 @@
#include "aos/json_to_flatbuffer.h"
#include "aos/network/team_number.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
#include "gflags/gflags.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/control_loops/drivetrain/localizer.h"
@@ -97,6 +98,14 @@
node = aos::configuration::GetNode(reader.configuration(), "roborio");
}
+ std::unique_ptr<aos::EventLoop> trajectory_generator_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("trajectory_generator", node);
+ trajectory_generator_event_loop->SkipTimingReport();
+
+ frc971::control_loops::drivetrain::TrajectoryGenerator trajectory_generator(
+ trajectory_generator_event_loop.get(),
+ y2020::control_loops::drivetrain::GetDrivetrainConfig());
+
std::unique_ptr<aos::EventLoop> drivetrain_event_loop =
reader.event_loop_factory()->MakeEventLoop("drivetrain", node);
drivetrain_event_loop->SkipTimingReport();
diff --git a/y2020/control_loops/drivetrain/trajectory_generator_main.cc b/y2020/control_loops/drivetrain/trajectory_generator_main.cc
new file mode 100644
index 0000000..adfa492
--- /dev/null
+++ b/y2020/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 "y2020/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, ::y2020::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;
+}
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 2a204ae..17666de 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -200,6 +200,21 @@
},
{
"name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.fb.Trajectory",
+ "source_node": "roborio",
+ "max_size": 200000,
+ "frequency": 10,
+ "read_method": "PIN",
+ "num_readers": 6
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.SplineGoal",
+ "source_node": "roborio",
+ "frequency": 10
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Goal",
"source_node": "roborio",
"frequency": 200
@@ -273,6 +288,11 @@
"nodes": ["roborio"]
},
{
+ "name": "trajectory_generator",
+ "executable_name": "trajectory_generator.stripped",
+ "nodes": ["roborio"]
+ },
+ {
"name": "superstructure",
"executable_name": "superstructure.stripped",
"nodes": ["roborio"]