phytrace_sdk/models/domains/
navigation.rs

1//! Navigation Domain - Localization, path planning, and goals.
2//!
3//! Contains localization data, SLAM status, path information, and navigation goals.
4
5use serde::{Deserialize, Serialize};
6use validator::Validate;
7
8use super::common::Position2D;
9use crate::models::enums::{LocalizationQuality, ObstacleType, PathState};
10
11/// Navigation domain containing path planning and localization information.
12#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
13pub struct NavigationDomain {
14    // === Localization ===
15    /// Localization status
16    #[serde(skip_serializing_if = "Option::is_none")]
17    pub localization: Option<Localization>,
18
19    // === SLAM ===
20    /// SLAM status
21    #[serde(skip_serializing_if = "Option::is_none")]
22    pub slam: Option<SlamStatus>,
23
24    // === Path Planning ===
25    /// Current path information
26    #[serde(skip_serializing_if = "Option::is_none")]
27    pub path: Option<PathInfo>,
28
29    // === Goal ===
30    /// Current navigation goal
31    #[serde(skip_serializing_if = "Option::is_none")]
32    pub goal: Option<NavigationGoal>,
33
34    // === Obstacles ===
35    /// Detected obstacles
36    #[serde(skip_serializing_if = "Option::is_none")]
37    pub obstacles: Option<Vec<Obstacle>>,
38
39    /// Number of obstacles detected
40    #[serde(skip_serializing_if = "Option::is_none")]
41    pub obstacle_count: Option<u32>,
42
43    // === Semantic Map ===
44    /// Semantic map information
45    #[serde(skip_serializing_if = "Option::is_none")]
46    pub semantic_map: Option<SemanticMapInfo>,
47
48    // === Costmap ===
49    /// Costmap status
50    #[serde(skip_serializing_if = "Option::is_none")]
51    pub costmap: Option<CostmapInfo>,
52}
53
54/// Localization status and quality.
55#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
56pub struct Localization {
57    /// Localization quality
58    #[serde(skip_serializing_if = "Option::is_none")]
59    pub quality: Option<LocalizationQuality>,
60
61    /// Confidence score (0-1)
62    #[serde(skip_serializing_if = "Option::is_none")]
63    #[validate(range(min = 0.0, max = 1.0))]
64    pub confidence: Option<f64>,
65
66    /// Localization method (e.g., "amcl", "cartographer", "slam_toolbox")
67    #[serde(skip_serializing_if = "Option::is_none")]
68    pub method: Option<String>,
69
70    /// Whether robot is localized
71    #[serde(skip_serializing_if = "Option::is_none")]
72    pub is_localized: Option<bool>,
73
74    /// Position covariance (meters squared)
75    #[serde(skip_serializing_if = "Option::is_none")]
76    pub position_covariance_m2: Option<f64>,
77
78    /// Orientation covariance (radians squared)
79    #[serde(skip_serializing_if = "Option::is_none")]
80    pub orientation_covariance_rad2: Option<f64>,
81
82    /// Particle count (for particle filter methods)
83    #[serde(skip_serializing_if = "Option::is_none")]
84    pub particle_count: Option<u32>,
85
86    /// Number of features matched
87    #[serde(skip_serializing_if = "Option::is_none")]
88    pub features_matched: Option<u32>,
89
90    /// Time since last good localization (seconds)
91    #[serde(skip_serializing_if = "Option::is_none")]
92    pub time_since_good_loc_sec: Option<f64>,
93}
94
95/// SLAM (Simultaneous Localization and Mapping) status.
96#[derive(Debug, Clone, Default, Serialize, Deserialize)]
97pub struct SlamStatus {
98    /// Whether SLAM is active
99    #[serde(skip_serializing_if = "Option::is_none")]
100    pub is_active: Option<bool>,
101
102    /// SLAM mode (e.g., "mapping", "localization", "idle")
103    #[serde(skip_serializing_if = "Option::is_none")]
104    pub mode: Option<String>,
105
106    /// SLAM algorithm name
107    #[serde(skip_serializing_if = "Option::is_none")]
108    pub algorithm: Option<String>,
109
110    /// Map size in nodes/poses
111    #[serde(skip_serializing_if = "Option::is_none")]
112    pub map_size: Option<u32>,
113
114    /// Map coverage area (square meters)
115    #[serde(skip_serializing_if = "Option::is_none")]
116    pub map_area_m2: Option<f64>,
117
118    /// Number of loop closures detected
119    #[serde(skip_serializing_if = "Option::is_none")]
120    pub loop_closures: Option<u32>,
121
122    /// Last loop closure time
123    #[serde(skip_serializing_if = "Option::is_none")]
124    pub last_loop_closure: Option<chrono::DateTime<chrono::Utc>>,
125}
126
127/// Current path information.
128#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
129pub struct PathInfo {
130    /// Path state
131    #[serde(skip_serializing_if = "Option::is_none")]
132    pub state: Option<PathState>,
133
134    /// Path length in meters
135    #[serde(skip_serializing_if = "Option::is_none")]
136    #[validate(range(min = 0.0))]
137    pub length_m: Option<f64>,
138
139    /// Estimated time to complete path (seconds)
140    #[serde(skip_serializing_if = "Option::is_none")]
141    #[validate(range(min = 0.0))]
142    pub eta_sec: Option<f64>,
143
144    /// Remaining distance in meters
145    #[serde(skip_serializing_if = "Option::is_none")]
146    #[validate(range(min = 0.0))]
147    pub remaining_m: Option<f64>,
148
149    /// Progress percentage (0-100)
150    #[serde(skip_serializing_if = "Option::is_none")]
151    #[validate(range(min = 0.0, max = 100.0))]
152    pub progress_pct: Option<f64>,
153
154    /// Number of waypoints in path
155    #[serde(skip_serializing_if = "Option::is_none")]
156    pub waypoint_count: Option<u32>,
157
158    /// Current waypoint index
159    #[serde(skip_serializing_if = "Option::is_none")]
160    pub current_waypoint: Option<u32>,
161
162    /// Path waypoints (simplified, may be sampled)
163    #[serde(skip_serializing_if = "Option::is_none")]
164    pub waypoints: Option<Vec<Position2D>>,
165
166    /// Planner used (e.g., "navfn", "teb", "dwa")
167    #[serde(skip_serializing_if = "Option::is_none")]
168    pub planner: Option<String>,
169
170    /// Whether path is valid
171    #[serde(skip_serializing_if = "Option::is_none")]
172    pub is_valid: Option<bool>,
173
174    /// Replan count for current goal
175    #[serde(skip_serializing_if = "Option::is_none")]
176    pub replan_count: Option<u32>,
177}
178
179/// Navigation goal.
180#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
181pub struct NavigationGoal {
182    /// Goal ID
183    #[serde(skip_serializing_if = "Option::is_none")]
184    pub goal_id: Option<String>,
185
186    /// Goal position
187    #[serde(skip_serializing_if = "Option::is_none")]
188    pub position: Option<Position2D>,
189
190    /// Goal orientation (yaw) in degrees
191    #[serde(skip_serializing_if = "Option::is_none")]
192    #[validate(range(min = -180.0, max = 180.0))]
193    pub orientation_deg: Option<f64>,
194
195    /// Goal name/label
196    #[serde(skip_serializing_if = "Option::is_none")]
197    pub name: Option<String>,
198
199    /// Goal type (e.g., "waypoint", "dock", "work_location")
200    #[serde(skip_serializing_if = "Option::is_none")]
201    pub goal_type: Option<String>,
202
203    /// Goal frame ID
204    #[serde(skip_serializing_if = "Option::is_none")]
205    pub frame_id: Option<String>,
206
207    /// Distance to goal in meters
208    #[serde(skip_serializing_if = "Option::is_none")]
209    #[validate(range(min = 0.0))]
210    pub distance_m: Option<f64>,
211
212    /// Tolerance for goal position (meters)
213    #[serde(skip_serializing_if = "Option::is_none")]
214    pub position_tolerance_m: Option<f64>,
215
216    /// Tolerance for goal orientation (degrees)
217    #[serde(skip_serializing_if = "Option::is_none")]
218    pub orientation_tolerance_deg: Option<f64>,
219}
220
221/// Detected obstacle.
222#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
223pub struct Obstacle {
224    /// Obstacle ID
225    #[serde(skip_serializing_if = "Option::is_none")]
226    pub obstacle_id: Option<String>,
227
228    /// Obstacle type
229    #[serde(skip_serializing_if = "Option::is_none")]
230    pub obstacle_type: Option<ObstacleType>,
231
232    /// Position relative to robot
233    #[serde(skip_serializing_if = "Option::is_none")]
234    pub position: Option<Position2D>,
235
236    /// Distance to obstacle in meters
237    #[serde(skip_serializing_if = "Option::is_none")]
238    #[validate(range(min = 0.0))]
239    pub distance_m: Option<f64>,
240
241    /// Bearing to obstacle in degrees
242    #[serde(skip_serializing_if = "Option::is_none")]
243    pub bearing_deg: Option<f64>,
244
245    /// Obstacle velocity (for dynamic obstacles)
246    #[serde(skip_serializing_if = "Option::is_none")]
247    pub velocity_mps: Option<f64>,
248
249    /// Obstacle heading (for dynamic obstacles)
250    #[serde(skip_serializing_if = "Option::is_none")]
251    pub heading_deg: Option<f64>,
252
253    /// Whether obstacle is on current path
254    #[serde(skip_serializing_if = "Option::is_none")]
255    pub on_path: Option<bool>,
256
257    /// Time to collision if applicable (seconds)
258    #[serde(skip_serializing_if = "Option::is_none")]
259    pub ttc_sec: Option<f64>,
260
261    /// Confidence score (0-1)
262    #[serde(skip_serializing_if = "Option::is_none")]
263    #[validate(range(min = 0.0, max = 1.0))]
264    pub confidence: Option<f64>,
265}
266
267/// Semantic map information.
268#[derive(Debug, Clone, Default, Serialize, Deserialize)]
269pub struct SemanticMapInfo {
270    /// Map ID
271    #[serde(skip_serializing_if = "Option::is_none")]
272    pub map_id: Option<String>,
273
274    /// Map name
275    #[serde(skip_serializing_if = "Option::is_none")]
276    pub name: Option<String>,
277
278    /// Map version
279    #[serde(skip_serializing_if = "Option::is_none")]
280    pub version: Option<String>,
281
282    /// Number of zones
283    #[serde(skip_serializing_if = "Option::is_none")]
284    pub zone_count: Option<u32>,
285
286    /// Number of waypoints
287    #[serde(skip_serializing_if = "Option::is_none")]
288    pub waypoint_count: Option<u32>,
289
290    /// Number of edges
291    #[serde(skip_serializing_if = "Option::is_none")]
292    pub edge_count: Option<u32>,
293
294    /// Current zone IDs
295    #[serde(skip_serializing_if = "Option::is_none")]
296    pub current_zones: Option<Vec<String>>,
297}
298
299/// Costmap information.
300#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
301pub struct CostmapInfo {
302    /// Costmap width in cells
303    #[serde(skip_serializing_if = "Option::is_none")]
304    pub width: Option<u32>,
305
306    /// Costmap height in cells
307    #[serde(skip_serializing_if = "Option::is_none")]
308    pub height: Option<u32>,
309
310    /// Resolution in meters per cell
311    #[serde(skip_serializing_if = "Option::is_none")]
312    pub resolution_m: Option<f64>,
313
314    /// Percentage of unknown cells
315    #[serde(skip_serializing_if = "Option::is_none")]
316    #[validate(range(min = 0.0, max = 100.0))]
317    pub unknown_pct: Option<f64>,
318
319    /// Percentage of occupied cells
320    #[serde(skip_serializing_if = "Option::is_none")]
321    #[validate(range(min = 0.0, max = 100.0))]
322    pub occupied_pct: Option<f64>,
323
324    /// Last update time
325    #[serde(skip_serializing_if = "Option::is_none")]
326    pub last_update: Option<chrono::DateTime<chrono::Utc>>,
327}
328
329impl NavigationDomain {
330    /// Create with localization quality.
331    pub fn from_localization(quality: LocalizationQuality, confidence: f64) -> Self {
332        Self {
333            localization: Some(Localization {
334                quality: Some(quality),
335                confidence: Some(confidence),
336                is_localized: Some(quality != LocalizationQuality::Lost),
337                ..Default::default()
338            }),
339            ..Default::default()
340        }
341    }
342
343    /// Builder to add path.
344    pub fn with_path(mut self, path: PathInfo) -> Self {
345        self.path = Some(path);
346        self
347    }
348
349    /// Builder to add goal.
350    pub fn with_goal(mut self, goal: NavigationGoal) -> Self {
351        self.goal = Some(goal);
352        self
353    }
354
355    /// Builder to add obstacle.
356    pub fn with_obstacle(mut self, obstacle: Obstacle) -> Self {
357        let obstacles = self.obstacles.get_or_insert_with(Vec::new);
358        obstacles.push(obstacle);
359        self.obstacle_count = Some(obstacles.len() as u32);
360        self
361    }
362}
363
364impl NavigationGoal {
365    /// Create a goal from coordinates.
366    pub fn new(x: f64, y: f64, yaw_deg: f64) -> Self {
367        Self {
368            position: Some(Position2D::new(x, y)),
369            orientation_deg: Some(yaw_deg),
370            ..Default::default()
371        }
372    }
373
374    /// Builder to add name.
375    pub fn with_name(mut self, name: impl Into<String>) -> Self {
376        self.name = Some(name.into());
377        self
378    }
379}
380
381impl PathInfo {
382    /// Create a new path.
383    pub fn new(state: PathState, length_m: f64) -> Self {
384        Self {
385            state: Some(state),
386            length_m: Some(length_m),
387            is_valid: Some(state == PathState::Valid || state == PathState::Executing),
388            ..Default::default()
389        }
390    }
391
392    /// Builder to add progress.
393    pub fn with_progress(mut self, remaining_m: f64, progress_pct: f64) -> Self {
394        self.remaining_m = Some(remaining_m);
395        self.progress_pct = Some(progress_pct);
396        self
397    }
398}
399
400#[cfg(test)]
401mod tests {
402    use super::*;
403
404    #[test]
405    fn test_navigation_domain() {
406        let nav = NavigationDomain::from_localization(LocalizationQuality::Good, 0.95)
407            .with_goal(NavigationGoal::new(10.0, 5.0, 90.0).with_name("pick_station"));
408
409        assert_eq!(nav.localization.as_ref().unwrap().confidence, Some(0.95));
410        assert_eq!(
411            nav.goal.as_ref().unwrap().name,
412            Some("pick_station".to_string())
413        );
414    }
415
416    #[test]
417    fn test_path_info() {
418        let path = PathInfo::new(PathState::Executing, 25.0).with_progress(10.0, 60.0);
419
420        assert_eq!(path.length_m, Some(25.0));
421        assert_eq!(path.remaining_m, Some(10.0));
422        assert_eq!(path.progress_pct, Some(60.0));
423    }
424}
phytrace_sdk/models/domains/navigation.rs

phytrace_sdk/models/domains/
navigation.rs
