fix control interval return ref

include/trajopt/path/SwervePathBuilder.hpp

@@ -163,6 +163,13 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder {
    */
   const std::vector<size_t>& GetControlIntervalCounts() const;
 
+  /**
+   * Calculate a best guess of control interval counts
+   *
+   * @return the vector of best guess control interval counts
+   */
+  const std::vector<size_t> CalculateControlIntervalCounts() const;
+
   /**
    * Calculate a discrete, linear initial guess of the x, y, and heading
    * of the robot that goes through each segment.

include/trajopt/util/GenerateSplineInitialGuess.hpp

@@ -219,11 +219,10 @@ CalculateWaypointStatesWithControlIntervals(
   return waypoint_states;
 }
 
-std::vector<size_t> CalculateControlIntervalCounts(
+std::vector<size_t> CalculateControlIntervalCountsWithDt(
     const trajopt::SwervePath path,
     const std::vector<std::vector<Pose2d>> initialGuessPoints,
     const double desiredDt) {
-  const auto desiredDt = 0.1;
   const auto trajectoriesSamples =
       CalculateWaypointStatesWithDt(path, initialGuessPoints, desiredDt);
   std::vector<size_t> counts;
@@ -235,7 +234,7 @@ std::vector<size_t> CalculateControlIntervalCounts(
   return counts;
 }
 
-SwerveSolution CalculateSplineInitialGuessWithKinematicsAndConstraints(
+inline SwerveSolution CalculateSplineInitialGuessWithKinematicsAndConstraints(
     const trajopt::SwervePath path,
     const std::vector<std::vector<Pose2d>> initialGuessPoints,
     std::vector<size_t> controlIntervalCounts) {
@@ -244,7 +243,8 @@ SwerveSolution CalculateSplineInitialGuessWithKinematicsAndConstraints(
 
   SwerveSolution initialGuess;
   for (const auto& traj : trajectoriesSamples) {
-    /// FIXME: first segment is always 1 point long so always 0.1s to second sample
+    /// FIXME: first segment is always 1 point long so always 0.1s to second
+    /// sample
     auto dt = 0.1_s;
     if (traj.size() > 1) {
       dt = traj.at(1).t - traj.front().t;

src/path/SwervePathBuilder.cpp

@@ -211,6 +211,11 @@ const std::vector<size_t>& SwervePathBuilder::GetControlIntervalCounts() const {
   return controlIntervalCounts;
 }
 
+const std::vector<size_t> SwervePathBuilder::CalculateControlIntervalCounts()
+    const {
+  return CalculateControlIntervalCountsWithDt(path, initialGuessPoints, 0.1);
+}
+
 SwerveSolution SwervePathBuilder::CalculateInitialGuess() const {
   return GenerateLinearInitialGuess<SwerveSolution>(initialGuessPoints,
                                                     controlIntervalCounts);

src/trajoptlibrust.cpp

@@ -268,15 +268,14 @@ HolonomicTrajectory SwervePathBuilderImpl::calculate_linear_initial_guess()
 HolonomicTrajectory SwervePathBuilderImpl::calculate_spline_initial_guess()
     const {
   return _convert_sol_to_holonomic_trajectory(
-      path.CalculateSplineInitialGuessWithKinematicsAndConstraints());
+      path.CalculateSplineInitialGuess());
 }
 
 rust::Vec<rust::usize>
 SwervePathBuilderImpl::calculate_control_interval_counts() const {
-  const auto cppCounts = path.CalculateControlIntervalCounts();
-
+  auto cppCounts = path.CalculateControlIntervalCounts();
   rust::Vec<rust::usize> rustCounts;
-  for (const auto& count : cppCounts) {
+  for (const auto count : cppCounts) {
     rustCounts.emplace_back(count);
   }
   return rustCounts;