Rust control interval plumbing

rust/include/trajoptlib.h

@@ -18,6 +18,7 @@ class SwervePathBuilderImpl {
  public:
   void set_drivetrain(const SwerveDrivetrain& drivetrain);
   void set_bumpers(double length, double width);
+  void set_control_interval_counts(const rust::Vec<size_t> counts);
 
   void pose_wpt(size_t idx, double x, double y, double heading);
   void translation_wpt(size_t idx, double x, double y, double heading_guess);

rust/src/lib.rs

@@ -46,6 +46,7 @@ mod ffi {
 
         fn set_drivetrain(self: Pin<&mut SwervePathBuilderImpl>, drivetrain: &SwerveDrivetrain);
         fn set_bumpers(self: Pin<&mut SwervePathBuilderImpl>, length: f64, width: f64);
+        fn set_control_interval_counts(self: Pin<&mut SwervePathBuilderImpl>, counts: Vec<usize>);
 
         fn pose_wpt(
             self: Pin<&mut SwervePathBuilderImpl>,
@@ -168,6 +169,10 @@ impl SwervePathBuilder {
         crate::ffi::SwervePathBuilderImpl::set_bumpers(self.path.pin_mut(), length, width);
     }
 
+    pub fn set_control_interval_counts(&mut self, counts: Vec<usize>) {
+        crate::ffi::SwervePathBuilderImpl::set_control_interval_counts(self.path.pin_mut(), counts);
+    }
+
     pub fn pose_wpt(&mut self, idx: usize, x: f64, y: f64, heading: f64) {
         crate::ffi::SwervePathBuilderImpl::pose_wpt(self.path.pin_mut(), idx, x, y, heading);
     }

rust/src/trajoptlib.cc

@@ -83,6 +83,18 @@ void SwervePathBuilderImpl::set_drivetrain(const SwerveDrivetrain& drivetrain) {
   path.SetDrivetrain(_convert_swerve_drivetrain(drivetrain));
 }
 
+size_t _convert_count(const size_t& count) {
+  return count;
+}
+
+void SwervePathBuilderImpl::set_control_interval_counts(
+  const rust::Vec<size_t> counts) {
+  std::vector<size_t> converted_counts =
+    _rust_vec_to_cpp_vector<size_t, size_t, &_convert_count>
+    (counts);
+  path.ControlIntervalCounts(std::move(converted_counts));
+}
+
 void SwervePathBuilderImpl::set_bumpers(double length, double width) {
   path.AddBumpers(trajopt::Bumpers{
     .safetyDistance = 0.0,
@@ -107,7 +119,8 @@ void SwervePathBuilderImpl::empty_wpt(size_t idx, double x_guess, double y_guess
   path.WptInitialGuessPoint(idx, {x_guess, y_guess, heading_guess});
 }
 
-void SwervePathBuilderImpl::sgmt_initial_guess_points(size_t from_idx, const rust::Vec<InitialGuessPoint>& guess_points) {
+void SwervePathBuilderImpl::sgmt_initial_guess_points(
+  size_t from_idx, const rust::Vec<InitialGuessPoint>& guess_points) {
   std::vector<trajopt::InitialGuessPoint> convertedGuessPoints =
       _rust_vec_to_cpp_vector<InitialGuessPoint,
                               trajopt::InitialGuessPoint,