commit 5c4ccf626ec3d6144f466859cf3f5a7598e77120
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Mar 03 17:29:49 2024 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Mar 15 21:46:10 2024 -0700
tree 651b1846178d3a486f1d9030180252986167eb72
parent ee52599f0b4349ba58330b9eb80b058995e81b1a

Remove mallocs in realtime trajectory code

Modifications to GetDrivetrainConfig for y2024 (which uses the constants
flatbuffer as a source of the drivetrain config) make it
non-realtime-safe.

Change-Id: Iad160acf3cf09d0feacb4498d03884f1065acc94
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

frc971/control_loops/drivetrain/trajectory.cc

diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 36d0947..a876ae3 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -33,7 +33,7 @@
 }  // namespace
 
 FinishedTrajectory::FinishedTrajectory(
-    const DrivetrainConfig<double> &config, const fb::Trajectory *buffer,
+    const DrivetrainConfig<double> *config, const fb::Trajectory *buffer,
     std::shared_ptr<
         StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
                           HybridKalman<2, 2, 2>>>
@@ -80,15 +80,15 @@
 
 BaseTrajectory::BaseTrajectory(
     const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
-    const DrivetrainConfig<double> &config,
+    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),
+      robot_radius_l_(config->robot_radius),
+      robot_radius_r_(config->robot_radius),
       lateral_acceleration_(
           ConstraintValue(constraints, ConstraintType::LATERAL_ACCELERATION)),
       longitudinal_acceleration_(ConstraintValue(
@@ -96,7 +96,7 @@
       voltage_limit_(ConstraintValue(constraints, ConstraintType::VOLTAGE)) {}
 
 Trajectory::Trajectory(const SplineGoal &spline_goal,
-                       const DrivetrainConfig<double> &config)
+                       const DrivetrainConfig<double> *config)
     : Trajectory(DistanceSpline{spline_goal.spline()}, config,
                  spline_goal.spline()->constraints(),
                  spline_goal.spline_idx()) {
@@ -104,7 +104,7 @@
 }
 
 Trajectory::Trajectory(
-    DistanceSpline &&input_spline, const DrivetrainConfig<double> &config,
+    DistanceSpline &&input_spline, const DrivetrainConfig<double> *config,
     const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
     int spline_idx, double vmax, int num_distance)
     : BaseTrajectory(constraints, config),
@@ -127,6 +127,15 @@
   }
 }
 
+Trajectory::Trajectory(
+    DistanceSpline &&spline, std::unique_ptr<DrivetrainConfig<double>> config,
+    const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+    int spline_idx, double vmax, int num_distance)
+    : Trajectory(std::move(spline), config.get(), constraints, spline_idx, vmax,
+                 num_distance) {
+  owned_config_ = std::move(config);
+}
+
 void Trajectory::LateralAccelPass() {
   for (size_t i = 0; i < plan_.size(); ++i) {
     const double distance = Distance(i);
@@ -751,7 +760,8 @@
 // finite-horizon much longer (albeit with the extension just using the
 // linearization for the infal point).
 void Trajectory::CalculatePathGains() {
-  const std::vector<Eigen::Matrix<double, 3, 1>> xva_plan = PlanXVA(config_.dt);
+  const std::vector<Eigen::Matrix<double, 3, 1>> xva_plan =
+      PlanXVA(config_->dt);
   if (xva_plan.empty()) {
     LOG(ERROR) << "Plan is empty--unable to plan trajectory.";
     return;
@@ -783,7 +793,7 @@
     PathRelativeContinuousSystem(distance, &A_continuous, &B_continuous);
     Eigen::Matrix<double, 5, 5> A_discrete;
     Eigen::Matrix<double, 5, 2> B_discrete;
-    controls::C2D(A_continuous, B_continuous, config_.dt, &A_discrete,
+    controls::C2D(A_continuous, B_continuous, config_->dt, &A_discrete,
                   &B_discrete);
 
     if (i == max_index) {
@@ -898,9 +908,9 @@
   result(2, 0) = spline().Theta(distance);
 
   result.block<2, 1>(3, 0) =
-      config_.Tla_to_lr() * (Eigen::Matrix<double, 2, 1>() << velocity,
-                             spline().DThetaDt(distance, velocity))
-                                .finished();
+      config_->Tla_to_lr() * (Eigen::Matrix<double, 2, 1>() << velocity,
+                              spline().DThetaDt(distance, velocity))
+                                 .finished();
   return result;
 }