diff --git a/rust/include/trajoptlib.h b/rust/include/trajoptlib.h index e93a9632..149b108d 100644 --- a/rust/include/trajoptlib.h +++ b/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 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); diff --git a/rust/src/lib.rs b/rust/src/lib.rs index 451613a0..f48480da 100644 --- a/rust/src/lib.rs +++ b/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); 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) { + 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); } diff --git a/rust/src/trajoptlib.cc b/rust/src/trajoptlib.cc index dc2e4a6f..a2875107 100644 --- a/rust/src/trajoptlib.cc +++ b/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 counts) { + std::vector converted_counts = + _rust_vec_to_cpp_vector + (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& guess_points) { +void SwervePathBuilderImpl::sgmt_initial_guess_points( + size_t from_idx, const rust::Vec& guess_points) { std::vector convertedGuessPoints = _rust_vec_to_cpp_vector