phytrace_sdk/models/domains/
location.rs

1//! Location Domain - Position and coordinates.
2//!
3//! Contains global (WGS84), local, grid, and semantic location information.
4
5use serde::{Deserialize, Serialize};
6use validator::Validate;
7
8use super::common::{CovarianceMatrix, Position2D, Position3D};
9
10/// Location domain containing position information.
11#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
12pub struct LocationDomain {
13    // === Global Coordinates (WGS84) ===
14    /// Latitude in decimal degrees (-90 to 90)
15    #[serde(skip_serializing_if = "Option::is_none")]
16    #[validate(range(min = -90.0, max = 90.0))]
17    pub latitude: Option<f64>,
18
19    /// Longitude in decimal degrees (-180 to 180)
20    #[serde(skip_serializing_if = "Option::is_none")]
21    #[validate(range(min = -180.0, max = 180.0))]
22    pub longitude: Option<f64>,
23
24    /// Altitude in meters above sea level
25    #[serde(skip_serializing_if = "Option::is_none")]
26    pub altitude_m: Option<f64>,
27
28    /// Heading/bearing in degrees (0-360, 0=North, clockwise)
29    #[serde(skip_serializing_if = "Option::is_none")]
30    #[validate(range(min = 0.0, max = 360.0))]
31    pub heading_deg: Option<f64>,
32
33    // === Local Coordinates ===
34    /// Local coordinate system position
35    #[serde(skip_serializing_if = "Option::is_none")]
36    pub local: Option<LocalCoordinates>,
37
38    // === Grid Coordinates ===
39    /// Grid-based position (for warehouses, etc.)
40    #[serde(skip_serializing_if = "Option::is_none")]
41    pub grid: Option<GridCoordinates>,
42
43    // === Semantic Location ===
44    /// Semantic location information
45    #[serde(skip_serializing_if = "Option::is_none")]
46    pub semantic: Option<SemanticLocation>,
47
48    // === Uncertainty ===
49    /// Horizontal position accuracy in meters (CEP or 1-sigma)
50    #[serde(skip_serializing_if = "Option::is_none")]
51    #[validate(range(min = 0.0))]
52    pub horizontal_accuracy_m: Option<f64>,
53
54    /// Vertical position accuracy in meters
55    #[serde(skip_serializing_if = "Option::is_none")]
56    #[validate(range(min = 0.0))]
57    pub vertical_accuracy_m: Option<f64>,
58
59    /// Full pose covariance matrix
60    #[serde(skip_serializing_if = "Option::is_none")]
61    pub covariance: Option<CovarianceMatrix>,
62
63    // === Reference Frame ===
64    /// Reference frame ID (e.g., "map", "odom", "world")
65    #[serde(skip_serializing_if = "Option::is_none")]
66    pub frame_id: Option<String>,
67
68    /// Map identifier (for multi-map deployments)
69    #[serde(skip_serializing_if = "Option::is_none")]
70    pub map_id: Option<String>,
71
72    /// Floor number (for multi-floor buildings)
73    #[serde(skip_serializing_if = "Option::is_none")]
74    pub floor: Option<i32>,
75}
76
77/// Local coordinate system position.
78#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
79pub struct LocalCoordinates {
80    /// X position in meters
81    #[serde(skip_serializing_if = "Option::is_none")]
82    pub x_m: Option<f64>,
83
84    /// Y position in meters
85    #[serde(skip_serializing_if = "Option::is_none")]
86    pub y_m: Option<f64>,
87
88    /// Z position in meters (height above floor)
89    #[serde(skip_serializing_if = "Option::is_none")]
90    pub z_m: Option<f64>,
91
92    /// Yaw angle in degrees (rotation about Z)
93    #[serde(skip_serializing_if = "Option::is_none")]
94    #[validate(range(min = -180.0, max = 180.0))]
95    pub yaw_deg: Option<f64>,
96
97    /// Roll angle in degrees
98    #[serde(skip_serializing_if = "Option::is_none")]
99    #[validate(range(min = -180.0, max = 180.0))]
100    pub roll_deg: Option<f64>,
101
102    /// Pitch angle in degrees
103    #[serde(skip_serializing_if = "Option::is_none")]
104    #[validate(range(min = -90.0, max = 90.0))]
105    pub pitch_deg: Option<f64>,
106}
107
108impl LocalCoordinates {
109    /// Create local coordinates from x, y, yaw.
110    pub fn new(x_m: f64, y_m: f64, yaw_deg: f64) -> Self {
111        Self {
112            x_m: Some(x_m),
113            y_m: Some(y_m),
114            z_m: None,
115            yaw_deg: Some(yaw_deg),
116            roll_deg: None,
117            pitch_deg: None,
118        }
119    }
120
121    /// Create full 3D local coordinates.
122    pub fn new_3d(x_m: f64, y_m: f64, z_m: f64, yaw_deg: f64) -> Self {
123        Self {
124            x_m: Some(x_m),
125            y_m: Some(y_m),
126            z_m: Some(z_m),
127            yaw_deg: Some(yaw_deg),
128            roll_deg: None,
129            pitch_deg: None,
130        }
131    }
132
133    /// Convert to Position2D.
134    pub fn to_position_2d(&self) -> Position2D {
135        Position2D {
136            x_m: self.x_m,
137            y_m: self.y_m,
138        }
139    }
140
141    /// Convert to Position3D.
142    pub fn to_position_3d(&self) -> Position3D {
143        Position3D {
144            x_m: self.x_m,
145            y_m: self.y_m,
146            z_m: self.z_m,
147        }
148    }
149}
150
151/// Grid-based coordinates for structured environments.
152#[derive(Debug, Clone, Default, Serialize, Deserialize)]
153pub struct GridCoordinates {
154    /// Row index
155    #[serde(skip_serializing_if = "Option::is_none")]
156    pub row: Option<i32>,
157
158    /// Column index
159    #[serde(skip_serializing_if = "Option::is_none")]
160    pub column: Option<i32>,
161
162    /// Aisle identifier
163    #[serde(skip_serializing_if = "Option::is_none")]
164    pub aisle: Option<String>,
165
166    /// Bay identifier
167    #[serde(skip_serializing_if = "Option::is_none")]
168    pub bay: Option<String>,
169
170    /// Level/shelf number
171    #[serde(skip_serializing_if = "Option::is_none")]
172    pub level: Option<i32>,
173
174    /// Cell/slot identifier
175    #[serde(skip_serializing_if = "Option::is_none")]
176    pub cell: Option<String>,
177}
178
179/// Semantic location information.
180#[derive(Debug, Clone, Default, Serialize, Deserialize)]
181pub struct SemanticLocation {
182    /// Zone name (e.g., "picking_zone_a", "charging_area")
183    #[serde(skip_serializing_if = "Option::is_none")]
184    pub zone: Option<String>,
185
186    /// Zone type (e.g., "work_area", "transit", "restricted")
187    #[serde(skip_serializing_if = "Option::is_none")]
188    pub zone_type: Option<String>,
189
190    /// Area name (e.g., "warehouse_east", "loading_dock")
191    #[serde(skip_serializing_if = "Option::is_none")]
192    pub area: Option<String>,
193
194    /// Building name
195    #[serde(skip_serializing_if = "Option::is_none")]
196    pub building: Option<String>,
197
198    /// Room name or identifier
199    #[serde(skip_serializing_if = "Option::is_none")]
200    pub room: Option<String>,
201
202    /// Nearest landmark or point of interest
203    #[serde(skip_serializing_if = "Option::is_none")]
204    pub landmark: Option<String>,
205
206    /// Named waypoint identifier
207    #[serde(skip_serializing_if = "Option::is_none")]
208    pub waypoint: Option<String>,
209
210    /// Node ID in navigation graph
211    #[serde(skip_serializing_if = "Option::is_none")]
212    pub node_id: Option<String>,
213
214    /// Edge ID in navigation graph (if on an edge)
215    #[serde(skip_serializing_if = "Option::is_none")]
216    pub edge_id: Option<String>,
217}
218
219impl LocationDomain {
220    /// Create a location with global coordinates.
221    pub fn from_global(latitude: f64, longitude: f64) -> Self {
222        Self {
223            latitude: Some(latitude),
224            longitude: Some(longitude),
225            ..Default::default()
226        }
227    }
228
229    /// Create a location with local coordinates.
230    pub fn from_local(x_m: f64, y_m: f64, yaw_deg: f64) -> Self {
231        Self {
232            local: Some(LocalCoordinates::new(x_m, y_m, yaw_deg)),
233            ..Default::default()
234        }
235    }
236
237    /// Builder method to add heading.
238    pub fn with_heading(mut self, heading_deg: f64) -> Self {
239        self.heading_deg = Some(heading_deg);
240        self
241    }
242
243    /// Builder method to add local coordinates.
244    pub fn with_local(mut self, local: LocalCoordinates) -> Self {
245        self.local = Some(local);
246        self
247    }
248
249    /// Builder method to add semantic location.
250    pub fn with_semantic(mut self, zone: impl Into<String>) -> Self {
251        self.semantic = Some(SemanticLocation {
252            zone: Some(zone.into()),
253            ..Default::default()
254        });
255        self
256    }
257
258    /// Builder method to add accuracy.
259    pub fn with_accuracy(mut self, horizontal_m: f64) -> Self {
260        self.horizontal_accuracy_m = Some(horizontal_m);
261        self
262    }
263
264    /// Builder method to set frame ID.
265    pub fn with_frame(mut self, frame_id: impl Into<String>) -> Self {
266        self.frame_id = Some(frame_id.into());
267        self
268    }
269}
270
271#[cfg(test)]
272mod tests {
273    use super::*;
274
275    #[test]
276    fn test_location_from_global() {
277        let loc = LocationDomain::from_global(41.8781, -87.6298).with_heading(45.0);
278
279        assert_eq!(loc.latitude, Some(41.8781));
280        assert_eq!(loc.longitude, Some(-87.6298));
281        assert_eq!(loc.heading_deg, Some(45.0));
282    }
283
284    #[test]
285    fn test_location_from_local() {
286        let loc = LocationDomain::from_local(10.5, 20.3, 90.0);
287
288        let local = loc.local.unwrap();
289        assert_eq!(local.x_m, Some(10.5));
290        assert_eq!(local.y_m, Some(20.3));
291        assert_eq!(local.yaw_deg, Some(90.0));
292    }
293
294    #[test]
295    fn test_location_serialization() {
296        let loc = LocationDomain::from_global(41.8781, -87.6298);
297        let json = serde_json::to_string(&loc).unwrap();
298
299        assert!(json.contains("41.8781"));
300        assert!(json.contains("-87.6298"));
301    }
302
303    // ==========================================================================
304    // LocationDomain Additional Tests
305    // ==========================================================================
306
307    #[test]
308    fn test_location_domain_default() {
309        let loc = LocationDomain::default();
310        assert!(loc.latitude.is_none());
311        assert!(loc.longitude.is_none());
312        assert!(loc.altitude_m.is_none());
313        assert!(loc.heading_deg.is_none());
314        assert!(loc.local.is_none());
315        assert!(loc.grid.is_none());
316        assert!(loc.semantic.is_none());
317    }
318
319    #[test]
320    fn test_location_domain_with_local() {
321        let local = LocalCoordinates::new(5.0, 10.0, 45.0);
322        let loc = LocationDomain::default().with_local(local);
323
324        assert!(loc.local.is_some());
325        assert_eq!(loc.local.as_ref().unwrap().x_m, Some(5.0));
326    }
327
328    #[test]
329    fn test_location_domain_with_semantic() {
330        let loc = LocationDomain::default().with_semantic("warehouse_zone_a");
331
332        assert!(loc.semantic.is_some());
333        assert_eq!(
334            loc.semantic.as_ref().unwrap().zone,
335            Some("warehouse_zone_a".to_string())
336        );
337    }
338
339    #[test]
340    fn test_location_domain_with_accuracy() {
341        let loc = LocationDomain::default().with_accuracy(0.5);
342        assert_eq!(loc.horizontal_accuracy_m, Some(0.5));
343    }
344
345    #[test]
346    fn test_location_domain_with_frame() {
347        let loc = LocationDomain::default().with_frame("map");
348        assert_eq!(loc.frame_id, Some("map".to_string()));
349    }
350
351    #[test]
352    fn test_location_domain_chained_builders() {
353        let loc = LocationDomain::from_global(40.0, -74.0)
354            .with_heading(180.0)
355            .with_local(LocalCoordinates::new(0.0, 0.0, 0.0))
356            .with_semantic("parking")
357            .with_accuracy(1.0)
358            .with_frame("odom");
359
360        assert!(loc.latitude.is_some());
361        assert!(loc.heading_deg.is_some());
362        assert!(loc.local.is_some());
363        assert!(loc.semantic.is_some());
364        assert!(loc.horizontal_accuracy_m.is_some());
365        assert!(loc.frame_id.is_some());
366    }
367
368    // ==========================================================================
369    // LocalCoordinates Tests
370    // ==========================================================================
371
372    #[test]
373    fn test_local_coordinates_new() {
374        let local = LocalCoordinates::new(1.0, 2.0, 90.0);
375        assert_eq!(local.x_m, Some(1.0));
376        assert_eq!(local.y_m, Some(2.0));
377        assert!(local.z_m.is_none());
378        assert_eq!(local.yaw_deg, Some(90.0));
379    }
380
381    #[test]
382    fn test_local_coordinates_new_3d() {
383        let local = LocalCoordinates::new_3d(1.0, 2.0, 3.0, 45.0);
384        assert_eq!(local.x_m, Some(1.0));
385        assert_eq!(local.y_m, Some(2.0));
386        assert_eq!(local.z_m, Some(3.0));
387        assert_eq!(local.yaw_deg, Some(45.0));
388    }
389
390    #[test]
391    fn test_local_coordinates_to_position_2d() {
392        let local = LocalCoordinates::new(10.0, 20.0, 0.0);
393        let pos2d = local.to_position_2d();
394
395        assert_eq!(pos2d.x_m, Some(10.0));
396        assert_eq!(pos2d.y_m, Some(20.0));
397    }
398
399    #[test]
400    fn test_local_coordinates_to_position_3d() {
401        let local = LocalCoordinates::new_3d(1.0, 2.0, 3.0, 0.0);
402        let pos3d = local.to_position_3d();
403
404        assert_eq!(pos3d.x_m, Some(1.0));
405        assert_eq!(pos3d.y_m, Some(2.0));
406        assert_eq!(pos3d.z_m, Some(3.0));
407    }
408
409    #[test]
410    fn test_local_coordinates_default() {
411        let local = LocalCoordinates::default();
412        assert!(local.x_m.is_none());
413        assert!(local.y_m.is_none());
414        assert!(local.z_m.is_none());
415        assert!(local.yaw_deg.is_none());
416        assert!(local.roll_deg.is_none());
417        assert!(local.pitch_deg.is_none());
418    }
419
420    // ==========================================================================
421    // GridCoordinates Tests
422    // ==========================================================================
423
424    #[test]
425    fn test_grid_coordinates_default() {
426        let grid = GridCoordinates::default();
427        assert!(grid.row.is_none());
428        assert!(grid.column.is_none());
429        assert!(grid.aisle.is_none());
430        assert!(grid.bay.is_none());
431        assert!(grid.level.is_none());
432        assert!(grid.cell.is_none());
433    }
434
435    #[test]
436    fn test_grid_coordinates_full() {
437        let grid = GridCoordinates {
438            row: Some(5),
439            column: Some(10),
440            aisle: Some("A".to_string()),
441            bay: Some("B3".to_string()),
442            level: Some(2),
443            cell: Some("A-B3-5-10".to_string()),
444        };
445
446        assert_eq!(grid.row, Some(5));
447        assert_eq!(grid.aisle, Some("A".to_string()));
448    }
449
450    // ==========================================================================
451    // SemanticLocation Tests
452    // ==========================================================================
453
454    #[test]
455    fn test_semantic_location_default() {
456        let sem = SemanticLocation::default();
457        assert!(sem.zone.is_none());
458        assert!(sem.zone_type.is_none());
459        assert!(sem.area.is_none());
460        assert!(sem.building.is_none());
461        assert!(sem.room.is_none());
462    }
463
464    #[test]
465    fn test_semantic_location_full() {
466        let sem = SemanticLocation {
467            zone: Some("picking_zone".to_string()),
468            zone_type: Some("work_area".to_string()),
469            area: Some("east_wing".to_string()),
470            building: Some("Warehouse A".to_string()),
471            room: Some("Storage 101".to_string()),
472            landmark: Some("Charger Station 1".to_string()),
473            waypoint: Some("wp_001".to_string()),
474            node_id: Some("node_42".to_string()),
475            edge_id: Some("edge_42_43".to_string()),
476        };
477
478        assert_eq!(sem.zone, Some("picking_zone".to_string()));
479        assert_eq!(sem.waypoint, Some("wp_001".to_string()));
480    }
481
482    // ==========================================================================
483    // Serialization Roundtrip Tests
484    // ==========================================================================
485
486    #[test]
487    fn test_location_domain_serialization_roundtrip() {
488        let loc = LocationDomain {
489            latitude: Some(41.8781),
490            longitude: Some(-87.6298),
491            altitude_m: Some(200.0),
492            heading_deg: Some(90.0),
493            local: Some(LocalCoordinates::new_3d(10.0, 20.0, 0.0, 90.0)),
494            horizontal_accuracy_m: Some(0.5),
495            frame_id: Some("map".to_string()),
496            floor: Some(1),
497            ..Default::default()
498        };
499
500        let json = serde_json::to_string(&loc).unwrap();
501        let deserialized: LocationDomain = serde_json::from_str(&json).unwrap();
502
503        assert_eq!(deserialized.latitude, Some(41.8781));
504        assert_eq!(deserialized.floor, Some(1));
505        assert!(deserialized.local.is_some());
506    }
507}
phytrace_sdk/models/domains/location.rs

phytrace_sdk/models/domains/
location.rs
