From b9be9ab3a41ab1e29dbe38adab79baf4d9126721 Mon Sep 17 00:00:00 2001
From: camaj <joshua.g.camarena@gmail.com>
Date: Sat, 29 Jun 2024 16:54:52 -0700
Subject: [PATCH] fix control interval return ref

---
 include/trajopt/path/SwervePathBuilder.hpp          | 7 +++++++
 include/trajopt/util/GenerateSplineInitialGuess.hpp | 8 ++++----
 src/path/SwervePathBuilder.cpp                      | 5 +++++
 src/trajoptlibrust.cpp                              | 7 +++----
 4 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/include/trajopt/path/SwervePathBuilder.hpp b/include/trajopt/path/SwervePathBuilder.hpp
index 4dd5a21f..ccee1800 100644
--- a/include/trajopt/path/SwervePathBuilder.hpp
+++ b/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.
diff --git a/include/trajopt/util/GenerateSplineInitialGuess.hpp b/include/trajopt/util/GenerateSplineInitialGuess.hpp
index 89b19bb7..b6c04fed 100644
--- a/include/trajopt/util/GenerateSplineInitialGuess.hpp
+++ b/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;
diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp
index 36aca690..bde69d4b 100644
--- a/src/path/SwervePathBuilder.cpp
+++ b/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);
diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp
index 5ef4fb67..019a6efd 100644
--- a/src/trajoptlibrust.cpp
+++ b/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;