From f87c23d1d9b4c2995bc950b46cf2b719c3faf8fc Mon Sep 17 00:00:00 2001 From: Alex Dickhans Date: Wed, 21 Aug 2024 13:21:41 -0600 Subject: [PATCH] fix: indexing bug --- .pre-commit-config.yaml | 2 +- src/bezier.rs | 188 ++++++++++++++++++++++++--------- src/combined_mp.rs | 120 +++++++++++++-------- src/lib.rs | 6 +- src/motion_profile.rs | 2 +- src/mp_2d.rs | 224 ++++++++++++++++++++++++++++------------ src/path.rs | 6 +- 7 files changed, 381 insertions(+), 167 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 207e1d7..00f6d30 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -8,6 +8,6 @@ repos: hooks: - id: cargo-test name: Run Cargo Test - entry: cargo test --all-targets + entry: cargo test --all-targets -- language: system types: [ rust ] \ No newline at end of file diff --git a/src/bezier.rs b/src/bezier.rs index 6b0256d..a145de3 100644 --- a/src/bezier.rs +++ b/src/bezier.rs @@ -1,20 +1,21 @@ +use crate::bezier::BezierConstructionError::{TooFewPoints, TooManyPoints}; +use crate::path::PathSegment; use core::cmp::Ordering; use interp::interp; -use nalgebra::{Matrix2x4, Matrix3x4, Matrix4, RowVector2, RowVector3, RowVector4, Vector2, Vector3, Vector4}; -use crate::bezier::BezierConstructionError::{TooFewPoints, TooManyPoints}; -use crate::path::{PathSegment}; +use nalgebra::{ + Matrix2x4, Matrix3x4, Matrix4, RowVector2, RowVector3, RowVector4, Vector2, Vector3, Vector4, +}; -const BEZIER_MATRIX: Matrix4 = Matrix4::new(-1.0, 3.0, -3.0, 1.0, - 3.0, -6.0, 3.0, 0.0, - -3.0, 3.0, 0.0, 0.0, - 1.0, 0.0, 0.0, 0.0); +const BEZIER_MATRIX: Matrix4 = Matrix4::new( + -1.0, 3.0, -3.0, 1.0, 3.0, -6.0, 3.0, 0.0, -3.0, 3.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, +); -const BEZIER_D_MATRIX: Matrix3x4 = Matrix3x4::new(-3.0, 9.0, -9.0, 3.0, - 6.0, -12.0, 6.0, 0.0, - -3.0, 3.0, 0.0, 0.0); +const BEZIER_D_MATRIX: Matrix3x4 = Matrix3x4::new( + -3.0, 9.0, -9.0, 3.0, 6.0, -12.0, 6.0, 0.0, -3.0, 3.0, 0.0, 0.0, +); -const BEZIER_DD_MATRIX: Matrix2x4 = Matrix2x4::new(-6.0, 18.0, -18.0, 6.0, - 6.0, -12.0, 6.0, 0.0); +const BEZIER_DD_MATRIX: Matrix2x4 = + Matrix2x4::new(-6.0, 18.0, -18.0, 6.0, 6.0, -12.0, 6.0, 0.0); #[derive(Debug, Copy, Clone)] pub enum BezierConstructionError { @@ -30,12 +31,16 @@ pub struct Bezier { } impl Bezier { - pub fn new(points: (Vector2, Vector2, Vector2, Vector2), vel: f64, accel: f64) -> Self { + pub fn new( + points: (Vector2, Vector2, Vector2, Vector2), + vel: f64, + accel: f64, + ) -> Self { let mut res = Self { points, dist_to_t: ([0.0; D], [0.0; D]), vel, - accel + accel, }; res.compute_dist_to_t(); @@ -47,7 +52,7 @@ impl Bezier { let mut mag_sum = 0.0; for i in 0..D { - let t = i as f64 / (D-1) as f64; + let t = i as f64 / (D - 1) as f64; mag_sum += self.get_d(t).magnitude() / D as f64; self.dist_to_t.0[i] = t; self.dist_to_t.1[i] = mag_sum; @@ -60,8 +65,22 @@ impl Bezier { pub fn get(&self, t: f64) -> Vector3 { let t_vector = RowVector4::new(t.powi(3), t.powi(2), t, 1.0); - let x = t_vector * BEZIER_MATRIX * Vector4::new(self.points.0.x, self.points.1.x, self.points.2.x, self.points.3.x); - let y = t_vector * BEZIER_MATRIX * Vector4::new(self.points.0.y, self.points.1.y, self.points.2.y, self.points.3.y); + let x = t_vector + * BEZIER_MATRIX + * Vector4::new( + self.points.0.x, + self.points.1.x, + self.points.2.x, + self.points.3.x, + ); + let y = t_vector + * BEZIER_MATRIX + * Vector4::new( + self.points.0.y, + self.points.1.y, + self.points.2.y, + self.points.3.y, + ); let d = self.get_d(t); @@ -70,16 +89,44 @@ impl Bezier { pub fn get_d(&self, t: f64) -> Vector2 { let t_vector = RowVector3::new(t.powi(2), t, 1.0); - let x = t_vector * BEZIER_D_MATRIX * Vector4::new(self.points.0.x, self.points.1.x, self.points.2.x, self.points.3.x); - let y = t_vector * BEZIER_D_MATRIX * Vector4::new(self.points.0.y, self.points.1.y, self.points.2.y, self.points.3.y); + let x = t_vector + * BEZIER_D_MATRIX + * Vector4::new( + self.points.0.x, + self.points.1.x, + self.points.2.x, + self.points.3.x, + ); + let y = t_vector + * BEZIER_D_MATRIX + * Vector4::new( + self.points.0.y, + self.points.1.y, + self.points.2.y, + self.points.3.y, + ); Vector2::new(x.sum(), y.sum()) } pub fn get_dd(&self, t: f64) -> Vector2 { let t_vector = RowVector2::new(t, 1.0); - let x = t_vector * BEZIER_DD_MATRIX * Vector4::new(self.points.0.x, self.points.1.x, self.points.2.x, self.points.3.x); - let y = t_vector * BEZIER_DD_MATRIX * Vector4::new(self.points.0.y, self.points.1.y, self.points.2.y, self.points.3.y); + let x = t_vector + * BEZIER_DD_MATRIX + * Vector4::new( + self.points.0.x, + self.points.1.x, + self.points.2.x, + self.points.3.x, + ); + let y = t_vector + * BEZIER_DD_MATRIX + * Vector4::new( + self.points.0.y, + self.points.1.y, + self.points.2.y, + self.points.3.y, + ); Vector2::new(x.sum(), y.sum()) } @@ -102,9 +149,18 @@ impl TryFrom for Bezier { fn try_from(path: PathSegment) -> Result { match path.path.len().cmp(&4usize) { - Ordering::Less => {Err(TooFewPoints)} - Ordering::Equal => { Ok(Self::new((path.path[0].into(), path.path[1].into(), path.path[2].into(), path.path[3].into()), path.constraints.velocity, path.constraints.accel))} - Ordering::Greater => { Err(TooManyPoints)} + Ordering::Less => Err(TooFewPoints), + Ordering::Equal => Ok(Self::new( + ( + path.path[0].into(), + path.path[1].into(), + path.path[2].into(), + path.path[3].into(), + ), + path.constraints.velocity, + path.constraints.accel, + )), + Ordering::Greater => Err(TooManyPoints), } } } @@ -115,10 +171,10 @@ fn cross_2d(a: Vector2, b: Vector2) -> f64 { #[cfg(test)] mod tests { - use alloc::{vec}; use super::*; - use nalgebra::Vector2; use crate::path::{Constraints, Point}; + use alloc::vec; + use nalgebra::Vector2; #[test] fn test_bezier_new() { @@ -126,15 +182,21 @@ mod tests { Vector2::new(0.0, 0.0), Vector2::new(1.0, 2.0), Vector2::new(2.0, 3.0), - Vector2::new(3.0, 0.0) + Vector2::new(3.0, 0.0), ); let bezier = Bezier::<50>::new(points, 1.0, 0.5); assert_eq!(bezier.points.0, Vector2::new(0.0, 0.0)); assert_eq!(bezier.vel, 1.0); assert_eq!(bezier.accel, 0.5); - assert!(!bezier.dist_to_t.0.is_empty(), "dist_to_t.0 should not be empty after initialization"); - assert!(!bezier.dist_to_t.1.is_empty(), "dist_to_t.1 should not be empty after initialization"); + assert!( + !bezier.dist_to_t.0.is_empty(), + "dist_to_t.0 should not be empty after initialization" + ); + assert!( + !bezier.dist_to_t.1.is_empty(), + "dist_to_t.1 should not be empty after initialization" + ); } #[test] @@ -143,7 +205,7 @@ mod tests { Vector2::new(0.0, 0.0), Vector2::new(0.25, 0.0), Vector2::new(0.75, 0.0), - Vector2::new(1.0, 0.0) + Vector2::new(1.0, 0.0), ); let bezier = Bezier::<50>::new(points, 1.0, 0.5); @@ -165,7 +227,7 @@ mod tests { Vector2::new(0.0, 0.0), Vector2::new(1.0, 2.0), Vector2::new(2.0, 3.0), - Vector2::new(3.0, 0.0) + Vector2::new(3.0, 0.0), ); let bezier = Bezier::<50>::new(points, 1.0, 0.5); @@ -181,14 +243,20 @@ mod tests { Vector2::new(0.0, 0.0), Vector2::new(1.0, 2.0), Vector2::new(2.0, 3.0), - Vector2::new(3.0, 0.0) + Vector2::new(3.0, 0.0), ); let bezier = Bezier::<50>::new(points, 1.0, 0.5); let dd_vector = bezier.get_dd(0.5); - assert!(dd_vector.x.is_finite(), "X second derivative should be finite"); - assert!(dd_vector.y.is_finite(), "Y second derivative should be finite"); + assert!( + dd_vector.x.is_finite(), + "X second derivative should be finite" + ); + assert!( + dd_vector.y.is_finite(), + "Y second derivative should be finite" + ); } #[test] @@ -197,7 +265,7 @@ mod tests { Vector2::new(0.0, 0.0), Vector2::new(1.0, 2.0), Vector2::new(2.0, 3.0), - Vector2::new(3.0, 0.0) + Vector2::new(3.0, 0.0), ); let bezier = Bezier::<50>::new(points, 1.0, 0.5); @@ -212,7 +280,7 @@ mod tests { Vector2::new(0.0, 0.0), Vector2::new(0.25, 0.0), Vector2::new(0.75, 0.0), - Vector2::new(1.0, 0.0) + Vector2::new(1.0, 0.0), ); let bezier = Bezier::<50>::new(points, 1.0, 0.5); @@ -229,7 +297,7 @@ mod tests { Vector2::new(0.0, 0.0), Vector2::new(0.25, 0.0), Vector2::new(0.75, 0.0), - Vector2::new(1.0, 0.0) + Vector2::new(1.0, 0.0), ); let bezier = Bezier::<50>::new(points, 1.0, 0.5); @@ -238,7 +306,12 @@ mod tests { assert!(distance.is_finite(), "Distance should be finite"); assert!(distance >= 0.0, "Distance should be non-negative"); - assert!((distance - correct_distance).abs() < correct_distance * 0.05, "calculated: {}, correct: {}", distance, correct_distance); + assert!( + (distance - correct_distance).abs() < correct_distance * 0.05, + "calculated: {}, correct: {}", + distance, + correct_distance + ); } #[test] @@ -247,7 +320,7 @@ mod tests { Vector2::new(0.0, 0.0), Vector2::new(1.0, 2.0), Vector2::new(2.0, 3.0), - Vector2::new(3.0, 0.0) + Vector2::new(3.0, 0.0), ); let bezier = Bezier::<50>::new(points, 1.0, 0.5); @@ -266,31 +339,40 @@ mod tests { Point { x: 2.0, y: 3.0 }, Point { x: 3.0, y: 0.0 }, ], - constraints: Constraints{ velocity: 1.0, accel: 1.0 }, + constraints: Constraints { + velocity: 1.0, + accel: 1.0, + }, inverted: false, stop_end: false, }; let bezier_result = Bezier::<50>::try_from(path_segment); - assert!(bezier_result.is_ok(), "Should successfully create a Bezier from PathSegment"); + assert!( + bezier_result.is_ok(), + "Should successfully create a Bezier from PathSegment" + ); } #[test] fn test_bezier_try_from_too_few_points() { let path_segment = PathSegment { - path: vec![ - Point { x: 0.0, y: 0.0 }, - Point { x: 1.0, y: 2.0 }, - ], - constraints: Constraints{ velocity: 1.0, accel: 1.0 }, + path: vec![Point { x: 0.0, y: 0.0 }, Point { x: 1.0, y: 2.0 }], + constraints: Constraints { + velocity: 1.0, + accel: 1.0, + }, inverted: false, stop_end: false, }; let bezier_result = Bezier::<50>::try_from(path_segment); - assert!(matches!(bezier_result, Err(BezierConstructionError::TooFewPoints))); + assert!(matches!( + bezier_result, + Err(BezierConstructionError::TooFewPoints) + )); } #[test] @@ -301,15 +383,21 @@ mod tests { Point { x: 1.0, y: 2.0 }, Point { x: 2.0, y: 3.0 }, Point { x: 3.0, y: 0.0 }, - Point { x: 4.0, y: 1.0 } + Point { x: 4.0, y: 1.0 }, ], - constraints: Constraints{ velocity: 1.0, accel: 1.0 }, + constraints: Constraints { + velocity: 1.0, + accel: 1.0, + }, inverted: false, stop_end: false, }; let bezier_result = Bezier::<50>::try_from(path_segment); - assert!(matches!(bezier_result, Err(BezierConstructionError::TooManyPoints))); + assert!(matches!( + bezier_result, + Err(BezierConstructionError::TooManyPoints) + )); } } diff --git a/src/combined_mp.rs b/src/combined_mp.rs index d69dd4c..e29c69c 100644 --- a/src/combined_mp.rs +++ b/src/combined_mp.rs @@ -1,22 +1,23 @@ +use crate::motion_profile::{MotionCommand, MotionProfile}; use alloc::vec::Vec; use core::time::Duration; -use crate::motion_profile::{MotionCommand, MotionProfile}; pub struct CombinedMP { - motion_profiles: Vec + motion_profiles: Vec, } impl CombinedMP { pub fn new(motion_profiles: Vec) -> Self { - Self { - motion_profiles - } + Self { motion_profiles } } } impl MotionProfile for CombinedMP { fn duration(&self) -> Duration { - self.motion_profiles.iter().map(|profile| profile.duration()).sum() + self.motion_profiles + .iter() + .map(|profile| profile.duration()) + .sum() } fn get(&mut self, t: Duration) -> Option { @@ -25,15 +26,18 @@ impl MotionProfile for CombinedMP { } else { let mut accumulated_t = Duration::new(0, 0); - self.motion_profiles.iter_mut().find(|profile| { - let duration = profile.duration(); - if accumulated_t + duration >= t { - true - } else { - accumulated_t += duration; - false - } - })?.get(t - accumulated_t) + self.motion_profiles + .iter_mut() + .find(|profile| { + let duration = profile.duration(); + if accumulated_t + duration >= t { + true + } else { + accumulated_t += duration; + false + } + })? + .get(t - accumulated_t) } } } @@ -67,7 +71,7 @@ mod tests { fn get(&mut self, t: Duration) -> Option { if t <= self.duration { - Some(MotionCommand{ + Some(MotionCommand { desired_velocity: Velocity::new::(t.as_secs_f64()), desired_angular: AngularVelocity::new::(0.0), desired_pose: Vector3::new(self.command.value as f64, 0.0, 0.0), @@ -81,8 +85,14 @@ mod tests { #[test] fn test_combined_motion_profile_duration() { let profiles = vec![ - MockMotionProfile { duration: Duration::new(2, 0), command: MockMotionCommand { value: 10 } }, - MockMotionProfile { duration: Duration::new(3, 0), command: MockMotionCommand { value: 20 } }, + MockMotionProfile { + duration: Duration::new(2, 0), + command: MockMotionCommand { value: 10 }, + }, + MockMotionProfile { + duration: Duration::new(3, 0), + command: MockMotionCommand { value: 20 }, + }, ]; let combined_mp = CombinedMP::new(profiles); @@ -93,32 +103,47 @@ mod tests { #[test] fn test_combined_motion_profile_get() { let profiles = vec![ - MockMotionProfile { duration: Duration::new(2, 0), command: MockMotionCommand { value: 10 } }, - MockMotionProfile { duration: Duration::new(3, 0), command: MockMotionCommand { value: 20 } }, + MockMotionProfile { + duration: Duration::new(2, 0), + command: MockMotionCommand { value: 10 }, + }, + MockMotionProfile { + duration: Duration::new(3, 0), + command: MockMotionCommand { value: 20 }, + }, ]; let mut combined_mp = CombinedMP::new(profiles); // Test within the first profile - assert_eq!(combined_mp.get(Duration::new(1, 0)), Some(MotionCommand{ - desired_velocity: Velocity::new::(1.0), - desired_angular: AngularVelocity::new::(0.0), - desired_pose: Vector3::new(10.0, 0.0, 0.0), - })); + assert_eq!( + combined_mp.get(Duration::new(1, 0)), + Some(MotionCommand { + desired_velocity: Velocity::new::(1.0), + desired_angular: AngularVelocity::new::(0.0), + desired_pose: Vector3::new(10.0, 0.0, 0.0), + }) + ); // Test at the boundary between profiles - assert_eq!(combined_mp.get(Duration::new(2, 0)), Some(MotionCommand{ - desired_velocity: Velocity::new::(2.0), - desired_angular: AngularVelocity::new::(0.0), - desired_pose: Vector3::new(10.0, 0.0, 0.0), - })); + assert_eq!( + combined_mp.get(Duration::new(2, 0)), + Some(MotionCommand { + desired_velocity: Velocity::new::(2.0), + desired_angular: AngularVelocity::new::(0.0), + desired_pose: Vector3::new(10.0, 0.0, 0.0), + }) + ); // Test within the second profile - assert_eq!(combined_mp.get(Duration::new(4, 0)), Some(MotionCommand{ - desired_velocity: Velocity::new::(2.0), - desired_angular: AngularVelocity::new::(0.0), - desired_pose: Vector3::new(20.0, 0.0, 0.0), - })); + assert_eq!( + combined_mp.get(Duration::new(4, 0)), + Some(MotionCommand { + desired_velocity: Velocity::new::(2.0), + desired_angular: AngularVelocity::new::(0.0), + desired_pose: Vector3::new(20.0, 0.0, 0.0), + }) + ); // Test outside the duration assert_eq!(combined_mp.get(Duration::new(6, 0)), None); @@ -127,20 +152,29 @@ mod tests { #[test] fn test_combined_motion_profile_invalid_times() { let profiles = vec![ - MockMotionProfile { duration: Duration::new(2, 0), command: MockMotionCommand { value: 10 } }, - MockMotionProfile { duration: Duration::new(3, 0), command: MockMotionCommand { value: 20 } }, + MockMotionProfile { + duration: Duration::new(2, 0), + command: MockMotionCommand { value: 10 }, + }, + MockMotionProfile { + duration: Duration::new(3, 0), + command: MockMotionCommand { value: 20 }, + }, ]; let mut combined_mp = CombinedMP::new(profiles); // Test a negative time - assert_eq!(combined_mp.get(Duration::new(0, 0)), Some(MotionCommand{ - desired_velocity: Velocity::new::(0.0), - desired_angular: AngularVelocity::new::(0.0), - desired_pose: Vector3::new(10.0, 0.0, 0.0), - })); + assert_eq!( + combined_mp.get(Duration::new(0, 0)), + Some(MotionCommand { + desired_velocity: Velocity::new::(0.0), + desired_angular: AngularVelocity::new::(0.0), + desired_pose: Vector3::new(10.0, 0.0, 0.0), + }) + ); // Test a time greater than the total duration assert_eq!(combined_mp.get(Duration::new(10, 0)), None); } -} \ No newline at end of file +} diff --git a/src/lib.rs b/src/lib.rs index dbb4792..2aa7763 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -3,8 +3,8 @@ extern crate alloc; -pub mod path; +mod bezier; +pub mod combined_mp; pub mod motion_profile; pub mod mp_2d; -pub mod combined_mp; -mod bezier; +pub mod path; diff --git a/src/motion_profile.rs b/src/motion_profile.rs index a3fdfd6..b498853 100644 --- a/src/motion_profile.rs +++ b/src/motion_profile.rs @@ -12,4 +12,4 @@ pub struct MotionCommand { pub trait MotionProfile { fn duration(&self) -> Duration; fn get(&mut self, t: Duration) -> Option; -} \ No newline at end of file +} diff --git a/src/mp_2d.rs b/src/mp_2d.rs index 25ea405..91d3290 100644 --- a/src/mp_2d.rs +++ b/src/mp_2d.rs @@ -1,3 +1,7 @@ +use crate::bezier::Bezier; +use crate::combined_mp::CombinedMP; +use crate::motion_profile::{MotionCommand, MotionProfile}; +use crate::path::Path; use alloc::vec; use alloc::vec::Vec; use core::cmp::max; @@ -6,11 +10,6 @@ use interp::interp; use uom::si::angular_velocity::radian_per_second; use uom::si::f64::{AngularVelocity, Velocity}; use uom::si::velocity::meter_per_second; -use crate::bezier::Bezier; -use crate::combined_mp::CombinedMP; -use crate::motion_profile::{MotionCommand, MotionProfile}; -use crate::path::{Path}; - pub struct MotionProfile2d { path: Vec>, @@ -23,12 +22,16 @@ impl MotionProfile2d { fn new(path: Path, track_width: f64) -> Self { let mut res = Self { inverted: path.segments.last().unwrap().inverted, - path: path.segments.into_iter().filter_map(|path_segment| path_segment.try_into().ok()).collect(), + path: path + .segments + .into_iter() + .filter_map(|path_segment| path_segment.try_into().ok()) + .collect(), time_to_velocity: (vec![], vec![], vec![]), }; res.compute(path.start_speed, path.end_speed, track_width); - + res } @@ -46,44 +49,52 @@ impl MotionProfile2d { for item in 1..unlimited_speed.len() { let delta_distance = (unlimited_speed[item].0 - unlimited_speed[item - 1].0).abs(); velocity.push( - (velocity[item - 1].powi(2) + 2.0 * unlimited_speed[item].2 * delta_distance).sqrt() - .min(unlimited_speed[item].1)); + (velocity[item - 1].powi(2) + 2.0 * unlimited_speed[item].2 * delta_distance) + .sqrt() + .min(unlimited_speed[item].1), + ); } velocity } - + fn compute(&mut self, start_speed: f64, end_speed: f64, track_width: f64) { // distance, speed, accel - let mut unlimited_speed = vec![ - (0.0, - start_speed.min( - Self::limited_speed(self.path[0].get_curvature(0.0), track_width) - ), - self.path[0].accel)]; + let mut unlimited_speed = vec![( + 0.0, + start_speed.min(Self::limited_speed( + self.path[0].get_curvature(0.0), + track_width, + )), + self.path[0].accel, + )]; let mut distance = 0.0; self.time_to_velocity.2.push(0.0); - for bezier in self.path.iter() { + for (index, bezier) in self.path.iter().enumerate() { let length = bezier.get_length(); - let count = max(5, (length/0.02) as usize); + let count = max(5, (length / 0.02) as usize); for i in 1..=count { let t = i as f64 / count as f64; - self.time_to_velocity.2.push(i as f64 + t); + self.time_to_velocity.2.push(index as f64 + t); let curvature = bezier.get_curvature(t); - unlimited_speed.push((distance + bezier.get_distance_by_t(t), Self::limited_speed(curvature, track_width) * bezier.vel, bezier.accel)); + unlimited_speed.push(( + distance + bezier.get_distance_by_t(t), + Self::limited_speed(curvature, track_width) * bezier.vel, + bezier.accel, + )); } distance += length; } - let len = unlimited_speed.len()-1; + let len = unlimited_speed.len() - 1; unlimited_speed[len].1 = end_speed.min(unlimited_speed.last().unwrap().1); @@ -106,12 +117,12 @@ impl MotionProfile2d { for i in 1..unlimited_speed.len() { // add stuff to current time - let delta_distance = unlimited_speed[i].0 - unlimited_speed[i-1].0; - let change_v = unlimited_speed[i].1.powi(2) - unlimited_speed[i-1].1.powi(2); + let delta_distance = unlimited_speed[i].0 - unlimited_speed[i - 1].0; + let change_v = unlimited_speed[i].1.powi(2) - unlimited_speed[i - 1].1.powi(2); let a = change_v / (2.0 * delta_distance); if a.abs() > 0.01 { - time += (unlimited_speed[i].1 - unlimited_speed[i-1].1) / a; + time += (unlimited_speed[i].1 - unlimited_speed[i - 1].1) / a; } else { time += delta_distance / unlimited_speed[i].1; } @@ -128,30 +139,47 @@ impl MotionProfile2d { impl MotionProfile for MotionProfile2d { fn duration(&self) -> Duration { - assert!(self.time_to_velocity.0.last().unwrap().is_finite(), "Time is not finite"); - Duration::from_secs_f64(*self.time_to_velocity.0.last().expect("Array should not be empty")) + assert!( + self.time_to_velocity.0.last().unwrap().is_finite(), + "Time is not finite" + ); + Duration::from_secs_f64( + *self + .time_to_velocity + .0 + .last() + .expect("Array should not be empty"), + ) } fn get(&mut self, time: Duration) -> Option { if self.duration() < time { None } else { - let t = interp(&self.time_to_velocity.0, &self.time_to_velocity.2, time.as_secs_f64()); + let t = interp( + &self.time_to_velocity.0, + &self.time_to_velocity.2, + time.as_secs_f64(), + ); let bezier = self.get_bezier(t)?; let t_local = t % 1.0; let inverted_multiplier = if self.inverted { -1.0 } else { 1.0 }; - let desired_velocity = Velocity::new::(interp(&self.time_to_velocity.0, &self.time_to_velocity.1, time.as_secs_f64())); + let desired_velocity = Velocity::new::(interp( + &self.time_to_velocity.0, + &self.time_to_velocity.1, + time.as_secs_f64(), + )); let curvature = bezier.get_curvature(t_local); - Some( - MotionCommand { - desired_velocity: inverted_multiplier * desired_velocity, - desired_angular: AngularVelocity::new::(desired_velocity.get::() * curvature), - desired_pose: bezier.get(t_local), - } - ) + Some(MotionCommand { + desired_velocity: inverted_multiplier * desired_velocity, + desired_angular: AngularVelocity::new::( + desired_velocity.get::() * curvature, + ), + desired_pose: bezier.get(t_local), + }) } } } @@ -188,15 +216,20 @@ impl CombinedMP { commands: vec![], }); - Some(Self::new(paths.drain(..).map(|path: Path| MotionProfile2d::new(path, track_width)).collect())) + Some(Self::new( + paths + .drain(..) + .map(|path: Path| MotionProfile2d::new(path, track_width)) + .collect(), + )) } } #[cfg(test)] mod tests { use super::*; - use core::time::Duration; use crate::path::{Constraints, PathSegment, Point}; + use core::time::Duration; fn create_mock_path() -> Path { // Create a mock path with dummy segments @@ -204,18 +237,34 @@ mod tests { start_speed: 0.0, end_speed: 0.0, segments: vec![ - PathSegment{ inverted: false, stop_end: false, path: vec![ - Point{ x: 0.0, y: 0.0 }, - Point{ x: 1.0, y: 0.0 }, - Point{ x: 0.0, y: 1.0 }, - Point{ x: 1.0, y: 1.0 }], - constraints: Constraints { velocity: 1.0, accel: 1.0 } }, - PathSegment{ inverted: false, stop_end: false, path: vec![ - Point{ x: 0.0, y: 0.0 }, - Point{ x: 1.0, y: 0.0 }, - Point{ x: 0.0, y: 1.0 }, - Point{ x: 1.0, y: 1.0 }], - constraints: Constraints { velocity: 1.0, accel: 1.0 } } + PathSegment { + inverted: false, + stop_end: false, + path: vec![ + Point { x: 0.0, y: 0.0 }, + Point { x: 1.0, y: 0.0 }, + Point { x: 0.0, y: 1.0 }, + Point { x: 1.0, y: 1.0 }, + ], + constraints: Constraints { + velocity: 1.0, + accel: 1.0, + }, + }, + PathSegment { + inverted: false, + stop_end: false, + path: vec![ + Point { x: 0.0, y: 0.0 }, + Point { x: 1.0, y: 0.0 }, + Point { x: 0.0, y: 1.0 }, + Point { x: 1.0, y: 1.0 }, + ], + constraints: Constraints { + velocity: 1.0, + accel: 1.0, + }, + }, ], commands: vec![], } @@ -227,8 +276,14 @@ mod tests { let track_width = 1.0; let profile = MotionProfile2d::new(path, track_width); - assert!(!profile.path.is_empty(), "Path should not be empty after initialization"); - assert!(!profile.time_to_velocity.0.is_empty(), "Initial time_to_velocity vector should not be empty"); + assert!( + !profile.path.is_empty(), + "Path should not be empty after initialization" + ); + assert!( + !profile.time_to_velocity.0.is_empty(), + "Initial time_to_velocity vector should not be empty" + ); } #[test] @@ -246,7 +301,10 @@ mod tests { let curvature = 0.5; let speed = MotionProfile2d::limited_speed(curvature, track_width); - assert!(speed < 1.0, "Speed should be less than 1.0 when curvature is positive"); + assert!( + speed < 1.0, + "Speed should be less than 1.0 when curvature is positive" + ); } #[test] @@ -256,9 +314,18 @@ mod tests { let profile = MotionProfile2d::new(path, track_width); // Test that the computation is correctly run and values are calculated - assert!(!profile.time_to_velocity.0.is_empty(), "Computed time array should not be empty"); - assert!(!profile.time_to_velocity.1.is_empty(), "Computed velocity array should not be empty"); - assert!(!profile.time_to_velocity.2.is_empty(), "Computed t array should not be empty"); + assert!( + !profile.time_to_velocity.0.is_empty(), + "Computed time array should not be empty" + ); + assert!( + !profile.time_to_velocity.1.is_empty(), + "Computed velocity array should not be empty" + ); + assert!( + !profile.time_to_velocity.2.is_empty(), + "Computed t array should not be empty" + ); } #[test] @@ -269,9 +336,14 @@ mod tests { let duration = profile.duration(); - assert!(duration.as_secs_f64() > 0.0, "Duration should be greater than zero after computation"); - assert!(duration.as_secs_f64().is_finite(), "Duration should be finite"); - + assert!( + duration.as_secs_f64() > 0.0, + "Duration should be greater than zero after computation" + ); + assert!( + duration.as_secs_f64().is_finite(), + "Duration should be finite" + ); } #[test] @@ -283,7 +355,13 @@ mod tests { let time = Duration::from_secs_f64(0.5); let command = profile.get(time); - assert!(command.is_some(), "MotionCommand should be returned for valid time input"); + assert!( + command.is_some(), + "MotionCommand should be returned for valid time input, {:?}\n{:?}\n{:?}", + profile.time_to_velocity.0, + profile.time_to_velocity.1, + profile.time_to_velocity.2 + ); } #[test] @@ -295,7 +373,10 @@ mod tests { let time = Duration::from_secs_f64(10.0); // Assuming time exceeds duration let command = profile.get(time); - assert!(command.is_none(), "MotionCommand should return None for time exceeding the duration"); + assert!( + command.is_none(), + "MotionCommand should return None for time exceeding the duration" + ); } #[test] @@ -304,22 +385,33 @@ mod tests { let track_width = 1.0; let combined_profile = CombinedMP::try_new_2d(path, track_width); - assert!(combined_profile.is_some(), "Combined motion profile should be created successfully"); + assert!( + combined_profile.is_some(), + "Combined motion profile should be created successfully" + ); } #[test] #[cfg(feature = "serde_support")] fn test_json_decoding() { - let path: Path = serde_json::from_str(include_str!("test/test.json")).expect("Failed to decode json path file"); + let path: Path = serde_json::from_str(include_str!("test/test.json")) + .expect("Failed to decode json path file"); - let track_width = 10.0/39.37; + let track_width = 10.0 / 39.37; - let profile = CombinedMP::try_new_2d(path, track_width).expect("Failed to create motion profile"); + let profile = + CombinedMP::try_new_2d(path, track_width).expect("Failed to create motion profile"); let duration = profile.duration(); // Test that the computation is correctly run and values are calculated - assert!(duration.as_secs_f64() > 0.0, "Duration should be greater than zero after computation"); - assert!(duration.as_secs_f64().is_finite(), "Duration should be finite"); + assert!( + duration.as_secs_f64() > 0.0, + "Duration should be greater than zero after computation" + ); + assert!( + duration.as_secs_f64().is_finite(), + "Duration should be finite" + ); } } diff --git a/src/path.rs b/src/path.rs index b4d91cb..c0012a8 100644 --- a/src/path.rs +++ b/src/path.rs @@ -1,7 +1,7 @@ use alloc::string::String; use alloc::vec::Vec; use nalgebra::Vector2; -#[cfg(feature="serde_support")] +#[cfg(feature = "serde_support")] use serde::{Deserialize, Serialize}; #[derive(Debug, Copy, Clone, PartialEq)] @@ -51,8 +51,8 @@ pub struct Path { #[cfg(test)] mod tests { - use alloc::vec; use super::*; + use alloc::vec; #[cfg(feature = "serde_support")] use serde_json; @@ -210,4 +210,4 @@ mod tests { assert_eq!(path.segments.len(), deserialized.segments.len()); assert_eq!(path.commands.len(), deserialized.commands.len()); } -} \ No newline at end of file +}