phytrace_sdk/models/domains/
motion.rs

1//! Motion Domain - Velocity, acceleration, and movement state.
2//!
3//! Contains linear and angular velocities, accelerations, odometry, and motion state.
4
5use serde::{Deserialize, Serialize};
6use validator::Validate;
7
8use super::common::Vector3D;
9use crate::models::enums::MotionState;
10
11/// Motion domain containing velocity and movement information.
12#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
13pub struct MotionDomain {
14    // === Simplified Speed ===
15    /// Overall speed in meters per second
16    #[serde(skip_serializing_if = "Option::is_none")]
17    #[validate(range(min = 0.0))]
18    pub speed_mps: Option<f64>,
19
20    // === Linear Velocity ===
21    /// Linear velocity components
22    #[serde(skip_serializing_if = "Option::is_none")]
23    pub linear_velocity: Option<LinearVelocity>,
24
25    // === Angular Velocity ===
26    /// Angular velocity components
27    #[serde(skip_serializing_if = "Option::is_none")]
28    pub angular_velocity: Option<AngularVelocity>,
29
30    // === Acceleration ===
31    /// Linear acceleration components
32    #[serde(skip_serializing_if = "Option::is_none")]
33    pub linear_acceleration: Option<LinearAcceleration>,
34
35    /// Angular acceleration components
36    #[serde(skip_serializing_if = "Option::is_none")]
37    pub angular_acceleration: Option<AngularAcceleration>,
38
39    // === Odometry ===
40    /// Odometry data
41    #[serde(skip_serializing_if = "Option::is_none")]
42    pub odometry: Option<Odometry>,
43
44    // === Motion State ===
45    /// Current motion state
46    #[serde(skip_serializing_if = "Option::is_none")]
47    pub motion_state: Option<MotionState>,
48
49    // === Reference Frame ===
50    /// Reference frame for velocities (e.g., "base_link", "odom")
51    #[serde(skip_serializing_if = "Option::is_none")]
52    pub frame_id: Option<String>,
53
54    // === Commanded vs Actual ===
55    /// Commanded linear velocity (from controller)
56    #[serde(skip_serializing_if = "Option::is_none")]
57    pub commanded_linear_mps: Option<f64>,
58
59    /// Commanded angular velocity (from controller)
60    #[serde(skip_serializing_if = "Option::is_none")]
61    pub commanded_angular_dps: Option<f64>,
62}
63
64/// Linear velocity in 3D.
65#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
66pub struct LinearVelocity {
67    /// X component in m/s (forward)
68    #[serde(skip_serializing_if = "Option::is_none")]
69    pub x_mps: Option<f64>,
70
71    /// Y component in m/s (left)
72    #[serde(skip_serializing_if = "Option::is_none")]
73    pub y_mps: Option<f64>,
74
75    /// Z component in m/s (up)
76    #[serde(skip_serializing_if = "Option::is_none")]
77    pub z_mps: Option<f64>,
78}
79
80impl LinearVelocity {
81    /// Create a new linear velocity.
82    pub fn new(x: f64, y: f64, z: f64) -> Self {
83        Self {
84            x_mps: Some(x),
85            y_mps: Some(y),
86            z_mps: Some(z),
87        }
88    }
89
90    /// Create linear velocity for planar motion (x forward only).
91    pub fn forward(speed_mps: f64) -> Self {
92        Self {
93            x_mps: Some(speed_mps),
94            y_mps: Some(0.0),
95            z_mps: Some(0.0),
96        }
97    }
98
99    /// Calculate speed magnitude.
100    pub fn speed(&self) -> Option<f64> {
101        match (self.x_mps, self.y_mps, self.z_mps) {
102            (Some(x), Some(y), Some(z)) => Some((x * x + y * y + z * z).sqrt()),
103            (Some(x), Some(y), None) => Some((x * x + y * y).sqrt()),
104            (Some(x), None, None) => Some(x.abs()),
105            _ => None,
106        }
107    }
108
109    /// Convert to Vector3D.
110    pub fn to_vector(&self) -> Vector3D {
111        Vector3D {
112            x: self.x_mps,
113            y: self.y_mps,
114            z: self.z_mps,
115        }
116    }
117}
118
119/// Angular velocity in 3D.
120#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
121pub struct AngularVelocity {
122    /// Roll rate in degrees per second
123    #[serde(skip_serializing_if = "Option::is_none")]
124    pub roll_dps: Option<f64>,
125
126    /// Pitch rate in degrees per second
127    #[serde(skip_serializing_if = "Option::is_none")]
128    pub pitch_dps: Option<f64>,
129
130    /// Yaw rate in degrees per second
131    #[serde(skip_serializing_if = "Option::is_none")]
132    pub yaw_dps: Option<f64>,
133}
134
135impl AngularVelocity {
136    /// Create a new angular velocity.
137    pub fn new(roll: f64, pitch: f64, yaw: f64) -> Self {
138        Self {
139            roll_dps: Some(roll),
140            pitch_dps: Some(pitch),
141            yaw_dps: Some(yaw),
142        }
143    }
144
145    /// Create angular velocity for yaw-only rotation.
146    pub fn yaw_only(yaw_dps: f64) -> Self {
147        Self {
148            roll_dps: Some(0.0),
149            pitch_dps: Some(0.0),
150            yaw_dps: Some(yaw_dps),
151        }
152    }
153}
154
155/// Linear acceleration in 3D.
156#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
157pub struct LinearAcceleration {
158    /// X component in m/s²
159    #[serde(skip_serializing_if = "Option::is_none")]
160    pub x_mps2: Option<f64>,
161
162    /// Y component in m/s²
163    #[serde(skip_serializing_if = "Option::is_none")]
164    pub y_mps2: Option<f64>,
165
166    /// Z component in m/s² (includes gravity if not compensated)
167    #[serde(skip_serializing_if = "Option::is_none")]
168    pub z_mps2: Option<f64>,
169
170    /// Whether gravity has been compensated
171    #[serde(skip_serializing_if = "Option::is_none")]
172    pub gravity_compensated: Option<bool>,
173}
174
175impl LinearAcceleration {
176    /// Create a new linear acceleration.
177    pub fn new(x: f64, y: f64, z: f64) -> Self {
178        Self {
179            x_mps2: Some(x),
180            y_mps2: Some(y),
181            z_mps2: Some(z),
182            gravity_compensated: None,
183        }
184    }
185}
186
187/// Angular acceleration in 3D.
188#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
189pub struct AngularAcceleration {
190    /// Roll acceleration in deg/s²
191    #[serde(skip_serializing_if = "Option::is_none")]
192    pub roll_dps2: Option<f64>,
193
194    /// Pitch acceleration in deg/s²
195    #[serde(skip_serializing_if = "Option::is_none")]
196    pub pitch_dps2: Option<f64>,
197
198    /// Yaw acceleration in deg/s²
199    #[serde(skip_serializing_if = "Option::is_none")]
200    pub yaw_dps2: Option<f64>,
201}
202
203/// Odometry data.
204#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
205pub struct Odometry {
206    /// Total distance traveled in meters
207    #[serde(skip_serializing_if = "Option::is_none")]
208    #[validate(range(min = 0.0))]
209    pub distance_traveled_m: Option<f64>,
210
211    /// Distance traveled in current session
212    #[serde(skip_serializing_if = "Option::is_none")]
213    #[validate(range(min = 0.0))]
214    pub session_distance_m: Option<f64>,
215
216    /// Total rotation accumulated in degrees
217    #[serde(skip_serializing_if = "Option::is_none")]
218    pub total_rotation_deg: Option<f64>,
219
220    /// Wheel odometry (if available)
221    #[serde(skip_serializing_if = "Option::is_none")]
222    pub wheel_odometry: Option<WheelOdometry>,
223}
224
225/// Wheel-level odometry data.
226#[derive(Debug, Clone, Default, Serialize, Deserialize)]
227pub struct WheelOdometry {
228    /// Left wheel distance in meters
229    #[serde(skip_serializing_if = "Option::is_none")]
230    pub left_m: Option<f64>,
231
232    /// Right wheel distance in meters
233    #[serde(skip_serializing_if = "Option::is_none")]
234    pub right_m: Option<f64>,
235
236    /// Left wheel velocity in m/s
237    #[serde(skip_serializing_if = "Option::is_none")]
238    pub left_velocity_mps: Option<f64>,
239
240    /// Right wheel velocity in m/s
241    #[serde(skip_serializing_if = "Option::is_none")]
242    pub right_velocity_mps: Option<f64>,
243}
244
245impl MotionDomain {
246    /// Create a motion domain with just speed.
247    pub fn from_speed(speed_mps: f64) -> Self {
248        Self {
249            speed_mps: Some(speed_mps),
250            ..Default::default()
251        }
252    }
253
254    /// Create a motion domain with linear velocity components.
255    pub fn from_linear(x_mps: f64, y_mps: f64, z_mps: f64) -> Self {
256        Self {
257            linear_velocity: Some(LinearVelocity::new(x_mps, y_mps, z_mps)),
258            speed_mps: Some((x_mps * x_mps + y_mps * y_mps + z_mps * z_mps).sqrt()),
259            ..Default::default()
260        }
261    }
262
263    /// Builder method to add angular velocity.
264    pub fn with_angular(mut self, yaw_dps: f64) -> Self {
265        self.angular_velocity = Some(AngularVelocity::yaw_only(yaw_dps));
266        self
267    }
268
269    /// Builder method to add motion state.
270    pub fn with_state(mut self, state: MotionState) -> Self {
271        self.motion_state = Some(state);
272        self
273    }
274
275    /// Builder method to add odometry.
276    pub fn with_odometry(mut self, distance_m: f64) -> Self {
277        self.odometry = Some(Odometry {
278            distance_traveled_m: Some(distance_m),
279            ..Default::default()
280        });
281        self
282    }
283}
284
285#[cfg(test)]
286mod tests {
287    use super::*;
288
289    #[test]
290    fn test_linear_velocity_speed() {
291        let vel = LinearVelocity::new(3.0, 4.0, 0.0);
292        assert!((vel.speed().unwrap() - 5.0).abs() < 1e-10);
293    }
294
295    #[test]
296    fn test_motion_from_speed() {
297        let motion = MotionDomain::from_speed(1.5)
298            .with_angular(10.0)
299            .with_state(MotionState::Moving);
300
301        assert_eq!(motion.speed_mps, Some(1.5));
302        assert_eq!(motion.angular_velocity.unwrap().yaw_dps, Some(10.0));
303        assert_eq!(motion.motion_state, Some(MotionState::Moving));
304    }
305
306    #[test]
307    fn test_motion_serialization() {
308        let motion = MotionDomain::from_speed(2.0);
309        let json = serde_json::to_string(&motion).unwrap();
310        assert!(json.contains("2.0") || json.contains("2"));
311    }
312
313    // ==========================================================================
314    // MotionDomain Additional Tests
315    // ==========================================================================
316
317    #[test]
318    fn test_motion_domain_default() {
319        let motion = MotionDomain::default();
320        assert!(motion.speed_mps.is_none());
321        assert!(motion.linear_velocity.is_none());
322        assert!(motion.angular_velocity.is_none());
323        assert!(motion.linear_acceleration.is_none());
324        assert!(motion.angular_acceleration.is_none());
325        assert!(motion.odometry.is_none());
326        assert!(motion.motion_state.is_none());
327        assert!(motion.frame_id.is_none());
328    }
329
330    #[test]
331    fn test_motion_domain_from_linear() {
332        let motion = MotionDomain::from_linear(1.0, 2.0, 0.0);
333
334        assert!(motion.linear_velocity.is_some());
335        let lv = motion.linear_velocity.unwrap();
336        assert_eq!(lv.x_mps, Some(1.0));
337        assert_eq!(lv.y_mps, Some(2.0));
338
339        // Speed should be calculated
340        let expected_speed = (1.0_f64.powi(2) + 2.0_f64.powi(2)).sqrt();
341        assert!((motion.speed_mps.unwrap() - expected_speed).abs() < 1e-10);
342    }
343
344    #[test]
345    fn test_motion_domain_with_odometry() {
346        let motion = MotionDomain::from_speed(1.0).with_odometry(1500.0);
347
348        assert!(motion.odometry.is_some());
349        assert_eq!(
350            motion.odometry.as_ref().unwrap().distance_traveled_m,
351            Some(1500.0)
352        );
353    }
354
355    #[test]
356    fn test_motion_domain_chained_builders() {
357        let motion = MotionDomain::from_speed(2.0)
358            .with_angular(45.0)
359            .with_state(MotionState::Accelerating)
360            .with_odometry(100.0);
361
362        assert!(motion.speed_mps.is_some());
363        assert!(motion.angular_velocity.is_some());
364        assert!(motion.motion_state.is_some());
365        assert!(motion.odometry.is_some());
366    }
367
368    // ==========================================================================
369    // LinearVelocity Tests
370    // ==========================================================================
371
372    #[test]
373    fn test_linear_velocity_new() {
374        let vel = LinearVelocity::new(1.0, 2.0, 3.0);
375        assert_eq!(vel.x_mps, Some(1.0));
376        assert_eq!(vel.y_mps, Some(2.0));
377        assert_eq!(vel.z_mps, Some(3.0));
378    }
379
380    #[test]
381    fn test_linear_velocity_forward() {
382        let vel = LinearVelocity::forward(5.0);
383        assert_eq!(vel.x_mps, Some(5.0));
384        assert_eq!(vel.y_mps, Some(0.0));
385        assert_eq!(vel.z_mps, Some(0.0));
386    }
387
388    #[test]
389    fn test_linear_velocity_speed_2d() {
390        let vel = LinearVelocity {
391            x_mps: Some(3.0),
392            y_mps: Some(4.0),
393            z_mps: None,
394        };
395        assert!((vel.speed().unwrap() - 5.0).abs() < 1e-10);
396    }
397
398    #[test]
399    fn test_linear_velocity_speed_1d() {
400        let vel = LinearVelocity {
401            x_mps: Some(7.0),
402            y_mps: None,
403            z_mps: None,
404        };
405        assert!((vel.speed().unwrap() - 7.0).abs() < 1e-10);
406    }
407
408    #[test]
409    fn test_linear_velocity_speed_none() {
410        let vel = LinearVelocity::default();
411        assert!(vel.speed().is_none());
412    }
413
414    #[test]
415    fn test_linear_velocity_to_vector() {
416        let vel = LinearVelocity::new(1.0, 2.0, 3.0);
417        let vec = vel.to_vector();
418        assert_eq!(vec.x, Some(1.0));
419        assert_eq!(vec.y, Some(2.0));
420        assert_eq!(vec.z, Some(3.0));
421    }
422
423    // ==========================================================================
424    // AngularVelocity Tests
425    // ==========================================================================
426
427    #[test]
428    fn test_angular_velocity_new() {
429        let av = AngularVelocity::new(10.0, 20.0, 30.0);
430        assert_eq!(av.roll_dps, Some(10.0));
431        assert_eq!(av.pitch_dps, Some(20.0));
432        assert_eq!(av.yaw_dps, Some(30.0));
433    }
434
435    #[test]
436    fn test_angular_velocity_yaw_only() {
437        let av = AngularVelocity::yaw_only(45.0);
438        assert_eq!(av.roll_dps, Some(0.0));
439        assert_eq!(av.pitch_dps, Some(0.0));
440        assert_eq!(av.yaw_dps, Some(45.0));
441    }
442
443    #[test]
444    fn test_angular_velocity_default() {
445        let av = AngularVelocity::default();
446        assert!(av.roll_dps.is_none());
447        assert!(av.pitch_dps.is_none());
448        assert!(av.yaw_dps.is_none());
449    }
450
451    // ==========================================================================
452    // LinearAcceleration Tests
453    // ==========================================================================
454
455    #[test]
456    fn test_linear_acceleration_new() {
457        let acc = LinearAcceleration::new(1.0, 2.0, 9.8);
458        assert_eq!(acc.x_mps2, Some(1.0));
459        assert_eq!(acc.y_mps2, Some(2.0));
460        assert_eq!(acc.z_mps2, Some(9.8));
461        assert!(acc.gravity_compensated.is_none());
462    }
463
464    #[test]
465    fn test_linear_acceleration_default() {
466        let acc = LinearAcceleration::default();
467        assert!(acc.x_mps2.is_none());
468        assert!(acc.y_mps2.is_none());
469        assert!(acc.z_mps2.is_none());
470        assert!(acc.gravity_compensated.is_none());
471    }
472
473    // ==========================================================================
474    // AngularAcceleration Tests
475    // ==========================================================================
476
477    #[test]
478    fn test_angular_acceleration_default() {
479        let acc = AngularAcceleration::default();
480        assert!(acc.roll_dps2.is_none());
481        assert!(acc.pitch_dps2.is_none());
482        assert!(acc.yaw_dps2.is_none());
483    }
484
485    #[test]
486    fn test_angular_acceleration_full() {
487        let acc = AngularAcceleration {
488            roll_dps2: Some(5.0),
489            pitch_dps2: Some(10.0),
490            yaw_dps2: Some(15.0),
491        };
492        assert_eq!(acc.yaw_dps2, Some(15.0));
493    }
494
495    // ==========================================================================
496    // Odometry Tests
497    // ==========================================================================
498
499    #[test]
500    fn test_odometry_default() {
501        let odom = Odometry::default();
502        assert!(odom.distance_traveled_m.is_none());
503        assert!(odom.session_distance_m.is_none());
504        assert!(odom.total_rotation_deg.is_none());
505        assert!(odom.wheel_odometry.is_none());
506    }
507
508    #[test]
509    fn test_odometry_full() {
510        let odom = Odometry {
511            distance_traveled_m: Some(15000.0),
512            session_distance_m: Some(500.0),
513            total_rotation_deg: Some(36000.0),
514            wheel_odometry: Some(WheelOdometry {
515                left_m: Some(7500.0),
516                right_m: Some(7500.0),
517                left_velocity_mps: Some(1.0),
518                right_velocity_mps: Some(1.0),
519            }),
520        };
521
522        assert_eq!(odom.distance_traveled_m, Some(15000.0));
523        assert!(odom.wheel_odometry.is_some());
524    }
525
526    // ==========================================================================
527    // WheelOdometry Tests
528    // ==========================================================================
529
530    #[test]
531    fn test_wheel_odometry_default() {
532        let wo = WheelOdometry::default();
533        assert!(wo.left_m.is_none());
534        assert!(wo.right_m.is_none());
535        assert!(wo.left_velocity_mps.is_none());
536        assert!(wo.right_velocity_mps.is_none());
537    }
538
539    #[test]
540    fn test_wheel_odometry_full() {
541        let wo = WheelOdometry {
542            left_m: Some(100.0),
543            right_m: Some(100.5),
544            left_velocity_mps: Some(0.5),
545            right_velocity_mps: Some(0.52),
546        };
547
548        assert_eq!(wo.left_m, Some(100.0));
549        assert_eq!(wo.right_velocity_mps, Some(0.52));
550    }
551
552    // ==========================================================================
553    // Serialization Roundtrip Tests
554    // ==========================================================================
555
556    #[test]
557    fn test_motion_domain_serialization_roundtrip() {
558        let motion = MotionDomain {
559            speed_mps: Some(2.5),
560            linear_velocity: Some(LinearVelocity::new(2.0, 1.5, 0.0)),
561            angular_velocity: Some(AngularVelocity::yaw_only(30.0)),
562            motion_state: Some(MotionState::Moving),
563            frame_id: Some("base_link".to_string()),
564            ..Default::default()
565        };
566
567        let json = serde_json::to_string(&motion).unwrap();
568        let deserialized: MotionDomain = serde_json::from_str(&json).unwrap();
569
570        assert_eq!(deserialized.speed_mps, Some(2.5));
571        assert_eq!(deserialized.motion_state, Some(MotionState::Moving));
572        assert!(deserialized.linear_velocity.is_some());
573    }
574}
phytrace_sdk/models/domains/motion.rs

phytrace_sdk/models/domains/
motion.rs
