Reduce mallocs when constructing trajectories
We are mostly allocating because we are lazy. Allocate some of the data
up front and pass around pointers, and use spans to represent the rest
of the data and either point it at vectors, or at the underlying
flatbuffer.
Change-Id: I819dfaa85bc1ba9895eb92efc74a1540d93a6c38
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index c92a81b..27b31a0 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -33,11 +33,15 @@
}
} // namespace
-FinishedTrajectory::FinishedTrajectory(const DrivetrainConfig<double> &config,
- const fb::Trajectory *buffer)
+FinishedTrajectory::FinishedTrajectory(
+ const DrivetrainConfig<double> &config, const fb::Trajectory *buffer,
+ std::shared_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain)
: BaseTrajectory(CHECK_NOTNULL(CHECK_NOTNULL(buffer->spline())->spline())
->constraints(),
- config),
+ config, std::move(velocity_drivetrain)),
buffer_(buffer),
spline_(*buffer_->spline()) {}
@@ -77,15 +81,12 @@
BaseTrajectory::BaseTrajectory(
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
- const DrivetrainConfig<double> &config)
- : velocity_drivetrain_(
- std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
- StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>>(
- new StateFeedbackLoop<2, 2, 2, double,
- StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>(
- config.make_hybrid_drivetrain_velocity_loop()))),
+ const DrivetrainConfig<double> &config,
+ std::shared_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain)
+ : velocity_drivetrain_(std::move(velocity_drivetrain)),
config_(config),
robot_radius_l_(config.robot_radius),
robot_radius_r_(config.robot_radius),