commit f7c6520a3e12728b0804ddbaf110ba8f7f427c04
author Austin Schuh <austin.linux@gmail.com> Fri Nov 04 21:28:20 2022 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Dec 07 21:33:56 2022 -0800
tree 99a8874fc140b411dba123d340755b192df1d07a
parent f48f39ad5efbf20ae0159bf0ae459b613a4ae1c2

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>

frc971/control_loops/drivetrain/trajectory.h

diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 60695de..6bccc74 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -44,7 +44,20 @@
  public:
   BaseTrajectory(
       const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
-      const DrivetrainConfig<double> &config);
+      const DrivetrainConfig<double> &config)
+      : BaseTrajectory(constraints, config,
+                       std::make_shared<StateFeedbackLoop<
+                           2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+                           HybridKalman<2, 2, 2>>>(
+                           config.make_hybrid_drivetrain_velocity_loop())) {}
+
+  BaseTrajectory(
+      const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+      const DrivetrainConfig<double> &config,
+      std::shared_ptr<
+          StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+                            HybridKalman<2, 2, 2>>>
+          velocity_drivetrain);
 
   virtual ~BaseTrajectory() = default;
 
@@ -94,7 +107,7 @@
             Eigen::Matrix<double, 2, 1> *K4,
             Eigen::Matrix<double, 2, 1> *K5) const;
 
-  virtual const DistanceSpline &spline() const = 0;
+  virtual const DistanceSplineBase &spline() const = 0;
 
   // Returns the length of the path in meters.
   double length() const { return spline().length(); }
@@ -175,7 +188,7 @@
       const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
       ConstraintType type);
 
-  std::unique_ptr<
+  std::shared_ptr<
       StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
                         HybridKalman<2, 2, 2>>>
       velocity_drivetrain_;
@@ -196,8 +209,21 @@
  public:
   // Note: The lifetime of the supplied buffer is assumed to be greater than
   // that of this object.
+  explicit 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);
+
   explicit FinishedTrajectory(const DrivetrainConfig<double> &config,
-                              const fb::Trajectory *buffer);
+                              const fb::Trajectory *buffer)
+      : FinishedTrajectory(
+            config, buffer,
+            std::make_shared<StateFeedbackLoop<
+                2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+                HybridKalman<2, 2, 2>>>(
+                config.make_hybrid_drivetrain_velocity_loop())) {}
 
   virtual ~FinishedTrajectory() = default;
 
@@ -224,9 +250,9 @@
   const fb::Trajectory &trajectory() const { return *buffer_; }
 
  private:
-  const DistanceSpline &spline() const override { return spline_; }
+  const DistanceSplineBase &spline() const override { return spline_; }
   const fb::Trajectory *buffer_;
-  const DistanceSpline spline_;
+  const FinishedDistanceSpline spline_;
 };
 
 // Class to handle plannign a trajectory and producing a flatbuffer containing