diff --git a/src/lib.rs b/src/lib.rs index 3ac81567..a19fa03c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -46,7 +46,6 @@ mod ffi { type SwervePathBuilderImpl; - fn cancel_all(self: Pin<&mut SwervePathBuilderImpl>); 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); @@ -89,12 +88,6 @@ mod ffi { index: usize, magnitude: f64, ); - fn wpt_linear_velocity_polar( - self: Pin<&mut SwervePathBuilderImpl>, - index: usize, - magnitude: f64, - angle: f64, - ); fn wpt_angular_velocity( self: Pin<&mut SwervePathBuilderImpl>, index: usize, @@ -105,9 +98,6 @@ mod ffi { index: usize, angular_velocity: f64, ); - fn wpt_x(self: Pin<&mut SwervePathBuilderImpl>, index: usize, x: f64); - fn wpt_y(self: Pin<&mut SwervePathBuilderImpl>, index: usize, y: f64); - fn wpt_heading(self: Pin<&mut SwervePathBuilderImpl>, index: usize, heading: f64); fn wpt_point_at( self: Pin<&mut SwervePathBuilderImpl>, index: usize, @@ -128,13 +118,6 @@ mod ffi { to_index: usize, magnitude: f64, ); - fn sgmt_linear_velocity_polar( - self: Pin<&mut SwervePathBuilderImpl>, - from_index: usize, - to_index: usize, - magnitude: f64, - angle: f64, - ); fn sgmt_angular_velocity( self: Pin<&mut SwervePathBuilderImpl>, from_index: usize, @@ -147,24 +130,6 @@ mod ffi { to_index: usize, angular_velocity: f64, ); - fn sgmt_x( - self: Pin<&mut SwervePathBuilderImpl>, - from_index: usize, - to_index: usize, - x: f64, - ); - fn sgmt_y( - self: Pin<&mut SwervePathBuilderImpl>, - from_index: usize, - to_index: usize, - y: f64, - ); - fn sgmt_heading( - self: Pin<&mut SwervePathBuilderImpl>, - from_index: usize, - to_index: usize, - heading: f64, - ); fn sgmt_point_at( self: Pin<&mut SwervePathBuilderImpl>, from_index: usize, @@ -197,11 +162,14 @@ mod ffi { diagnostics: bool, uuid: i64, ) -> Result; + fn add_progress_callback( self: Pin<&mut SwervePathBuilderImpl>, callback: fn(HolonomicTrajectory, i64), ); + fn cancel_all(self: Pin<&mut SwervePathBuilderImpl>); + fn new_swerve_path_builder_impl() -> UniquePtr; } } @@ -281,15 +249,6 @@ impl SwervePathBuilder { ); } - pub fn wpt_linear_velocity_polar(&mut self, index: usize, magnitude: f64, angle: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_linear_velocity_polar( - self.path.pin_mut(), - index, - magnitude, - angle, - ); - } - pub fn wpt_angular_velocity(&mut self, index: usize, angular_velocity: f64) { crate::ffi::SwervePathBuilderImpl::wpt_angular_velocity( self.path.pin_mut(), @@ -306,18 +265,6 @@ impl SwervePathBuilder { ); } - pub fn wpt_x(&mut self, index: usize, x: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_x(self.path.pin_mut(), index, x); - } - - pub fn wpt_y(&mut self, index: usize, y: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_y(self.path.pin_mut(), index, y); - } - - pub fn wpt_heading(&mut self, index: usize, heading: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_heading(self.path.pin_mut(), index, heading); - } - pub fn wpt_point_at( &mut self, index: usize, @@ -362,22 +309,6 @@ impl SwervePathBuilder { ); } - pub fn sgmt_linear_velocity_polar( - &mut self, - from_index: usize, - to_index: usize, - magnitude: f64, - angle: f64, - ) { - crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_polar( - self.path.pin_mut(), - from_index, - to_index, - magnitude, - angle, - ); - } - pub fn sgmt_angular_velocity( &mut self, from_index: usize, @@ -406,23 +337,6 @@ impl SwervePathBuilder { ); } - pub fn sgmt_x(&mut self, from_index: usize, to_index: usize, x: f64) { - crate::ffi::SwervePathBuilderImpl::sgmt_x(self.path.pin_mut(), from_index, to_index, x); - } - - pub fn sgmt_y(&mut self, from_index: usize, to_index: usize, y: f64) { - crate::ffi::SwervePathBuilderImpl::sgmt_y(self.path.pin_mut(), from_index, to_index, y); - } - - pub fn sgmt_heading(&mut self, from_index: usize, to_index: usize, heading: f64) { - crate::ffi::SwervePathBuilderImpl::sgmt_heading( - self.path.pin_mut(), - from_index, - to_index, - heading, - ); - } - pub fn sgmt_point_at( &mut self, from_index: usize, diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp index 09b8e289..f1fe3fc3 100644 --- a/src/trajoptlibrust.cpp +++ b/src/trajoptlibrust.cpp @@ -123,12 +123,6 @@ void SwervePathBuilderImpl::wpt_linear_velocity_max_magnitude( path.WptVelocityMagnitude(index, magnitude); } -void SwervePathBuilderImpl::wpt_linear_velocity_polar(size_t index, - double magnitude, - double angle) { - path.WptVelocityPolar(index, magnitude, angle); -} - void SwervePathBuilderImpl::wpt_angular_velocity(size_t index, double angular_velocity) { // this probably ought to be added to SwervePathBuilder in the C++ API @@ -140,22 +134,6 @@ void SwervePathBuilderImpl::wpt_angular_velocity_max_magnitude( path.WptAngularVelocityMaxMagnitude(index, angular_velocity); } -void SwervePathBuilderImpl::wpt_x(size_t index, double x) { - path.WptConstraint(index, - trajopt::TranslationConstraint{trajopt::RectangularSet2d{ - .xBound = x, .yBound = trajopt::IntervalSet1d::R1()}}); -} - -void SwervePathBuilderImpl::wpt_y(size_t index, double y) { - path.WptConstraint(index, - trajopt::TranslationConstraint{trajopt::RectangularSet2d{ - .xBound = trajopt::IntervalSet1d::R1(), .yBound = y}}); -} - -void SwervePathBuilderImpl::wpt_heading(size_t index, double heading) { - path.WptConstraint(index, trajopt::HeadingConstraint{heading}); -} - void SwervePathBuilderImpl::wpt_point_at(size_t index, double field_point_x, double field_point_y, double heading_tolerance) { @@ -176,17 +154,6 @@ void SwervePathBuilderImpl::sgmt_linear_velocity_max_magnitude( path.SgmtVelocityMagnitude(from_index, to_index, magnitude); } -void SwervePathBuilderImpl::sgmt_linear_velocity_polar(size_t from_index, - size_t to_index, - double magnitude, - double angle) { - path.SgmtConstraint( - from_index, to_index, - trajopt::HolonomicVelocityConstraint{ - trajopt::RectangularSet2d::PolarExactSet2d(magnitude, angle), - trajopt::CoordinateSystem::kField}); -} - void SwervePathBuilderImpl::sgmt_angular_velocity(size_t from_index, size_t to_index, double angular_velocity) { @@ -198,28 +165,6 @@ void SwervePathBuilderImpl::sgmt_angular_velocity_max_magnitude( path.SgmtAngularVelocityMaxMagnitude(from_index, to_index, angular_velocity); } -void SwervePathBuilderImpl::sgmt_x(size_t from_index, size_t to_index, - double x) { - path.SgmtConstraint( - from_index, to_index, - trajopt::TranslationConstraint{trajopt::RectangularSet2d{ - .xBound = x, .yBound = trajopt::IntervalSet1d::R1()}}); -} - -void SwervePathBuilderImpl::sgmt_y(size_t from_index, size_t to_index, - double y) { - path.SgmtConstraint( - from_index, to_index, - trajopt::TranslationConstraint{trajopt::RectangularSet2d{ - .xBound = trajopt::IntervalSet1d::R1(), .yBound = y}}); -} - -void SwervePathBuilderImpl::sgmt_heading(size_t from_index, size_t to_index, - double heading) { - path.SgmtConstraint(from_index, to_index, - trajopt::HeadingConstraint{heading}); -} - void SwervePathBuilderImpl::sgmt_point_at(size_t from_index, size_t to_index, double field_point_x, double field_point_y, diff --git a/src/trajoptlibrust.hpp b/src/trajoptlibrust.hpp index 9bc55c02..c728ce11 100644 --- a/src/trajoptlibrust.hpp +++ b/src/trajoptlibrust.hpp @@ -17,6 +17,8 @@ struct SwerveDrivetrain; class SwervePathBuilderImpl { public: + SwervePathBuilderImpl() = default; + void set_drivetrain(const SwerveDrivetrain& drivetrain); void set_bumpers(double length, double width); void set_control_interval_counts(const rust::Vec counts); @@ -31,13 +33,9 @@ class SwervePathBuilderImpl { void wpt_linear_velocity_direction(size_t index, double angle); void wpt_linear_velocity_max_magnitude(size_t index, double magnitude); - void wpt_linear_velocity_polar(size_t index, double magnitude, double angle); void wpt_angular_velocity(size_t index, double angular_velocity); void wpt_angular_velocity_max_magnitude(size_t index, double angular_velocity); - void wpt_x(size_t index, double x); - void wpt_y(size_t index, double y); - void wpt_heading(size_t index, double heading); void wpt_point_at(size_t index, double field_point_x, double field_point_y, double heading_tolerance); @@ -45,15 +43,10 @@ class SwervePathBuilderImpl { double angle); void sgmt_linear_velocity_max_magnitude(size_t from_index, size_t to_index, double magnitude); - void sgmt_linear_velocity_polar(size_t from_index, size_t to_index, - double magnitude, double angle); void sgmt_angular_velocity(size_t from_index, size_t to_index, double angular_velocity); void sgmt_angular_velocity_max_magnitude(size_t from_index, size_t to_index, double angular_velocity); - void sgmt_x(size_t from_index, size_t to_index, double x); - void sgmt_y(size_t from_index, size_t to_index, double y); - void sgmt_heading(size_t from_index, size_t to_index, double heading); void sgmt_point_at(size_t from_index, size_t to_index, double field_point_x, double field_point_y, double heading_tolerance); @@ -70,8 +63,6 @@ class SwervePathBuilderImpl { rust::Fn callback); void cancel_all(); - SwervePathBuilderImpl() = default; - private: trajopt::SwervePathBuilder path; };