mecha10_core/messages/
sensor.rs

1// Sensor Message Types
2
3use serde::{Deserialize, Serialize};
4
5/// Image data from a camera
6///
7/// Supports various encodings (rgb8, bgr8, mono8, depth, etc.)
8#[derive(Debug, Clone, Serialize, Deserialize)]
9pub struct Image {
10    /// Timestamp in microseconds since epoch
11    pub timestamp: u64,
12
13    /// Image width in pixels
14    pub width: u32,
15
16    /// Image height in pixels
17    pub height: u32,
18
19    /// Encoding format (e.g., "rgb8", "bgr8", "mono8", "16UC1" for depth)
20    pub encoding: String,
21
22    /// Raw image data
23    pub data: Vec<u8>,
24
25    /// Optional camera frame ID
26    #[serde(skip_serializing_if = "Option::is_none")]
27    pub frame_id: Option<String>,
28}
29
30/// Compressed image data for network-efficient transmission
31///
32/// Used for streaming over network to remote nodes or dashboards.
33/// Supports JPEG, PNG, H.264, and other compressed formats.
34#[derive(Debug, Clone, Serialize, Deserialize)]
35pub struct CompressedImage {
36    /// Timestamp in microseconds since epoch
37    pub timestamp: u64,
38
39    /// Original image width in pixels
40    pub width: u32,
41
42    /// Original image height in pixels
43    pub height: u32,
44
45    /// Compression format (e.g., "jpeg", "png", "h264")
46    pub format: String,
47
48    /// Compressed image data
49    pub data: Vec<u8>,
50
51    /// Optional JPEG quality (1-100) or compression settings
52    #[serde(skip_serializing_if = "Option::is_none")]
53    pub quality: Option<u8>,
54
55    /// Optional camera frame ID
56    #[serde(skip_serializing_if = "Option::is_none")]
57    pub frame_id: Option<String>,
58}
59
60/// 2D laser scan data
61///
62/// Represents a planar laser scan (e.g., from RPLidar, Hokuyo)
63#[derive(Debug, Clone, Serialize, Deserialize)]
64pub struct LaserScan {
65    /// Timestamp in microseconds since epoch
66    pub timestamp: u64,
67
68    /// Start angle of the scan in radians
69    pub angle_min: f32,
70
71    /// End angle of the scan in radians
72    pub angle_max: f32,
73
74    /// Angular distance between measurements in radians
75    pub angle_increment: f32,
76
77    /// Time between measurements in seconds
78    pub time_increment: f32,
79
80    /// Time between scans in seconds
81    pub scan_time: f32,
82
83    /// Minimum range value in meters
84    pub range_min: f32,
85
86    /// Maximum range value in meters
87    pub range_max: f32,
88
89    /// Range data in meters (one per angle)
90    pub ranges: Vec<f32>,
91
92    /// Intensity data (optional)
93    #[serde(skip_serializing_if = "Option::is_none")]
94    pub intensities: Option<Vec<f32>>,
95
96    /// Optional frame ID
97    #[serde(skip_serializing_if = "Option::is_none")]
98    pub frame_id: Option<String>,
99}
100
101/// Odometry data (position and velocity)
102///
103/// Represents the robot's estimated position and velocity
104#[derive(Debug, Clone, Serialize, Deserialize)]
105pub struct Odometry {
106    /// Timestamp in microseconds since epoch
107    pub timestamp: u64,
108
109    /// Position (x, y, z) in meters
110    pub position: [f32; 3],
111
112    /// Orientation as quaternion (x, y, z, w)
113    pub orientation: [f32; 4],
114
115    /// Linear velocity (x, y, z) in m/s
116    pub linear_velocity: [f32; 3],
117
118    /// Angular velocity (x, y, z) in rad/s
119    pub angular_velocity: [f32; 3],
120
121    /// Optional frame ID
122    #[serde(skip_serializing_if = "Option::is_none")]
123    pub frame_id: Option<String>,
124
125    /// Optional child frame ID
126    #[serde(skip_serializing_if = "Option::is_none")]
127    pub child_frame_id: Option<String>,
128}
129
130/// IMU (Inertial Measurement Unit) data
131///
132/// Provides orientation, angular velocity, and linear acceleration
133#[derive(Debug, Clone, Serialize, Deserialize)]
134pub struct Imu {
135    /// Timestamp in microseconds since epoch
136    pub timestamp: u64,
137
138    /// Orientation as quaternion (x, y, z, w)
139    pub orientation: [f32; 4],
140
141    /// Orientation covariance (9 elements, row-major)
142    #[serde(skip_serializing_if = "Option::is_none")]
143    pub orientation_covariance: Option<[f32; 9]>,
144
145    /// Angular velocity (x, y, z) in rad/s
146    pub angular_velocity: [f32; 3],
147
148    /// Angular velocity covariance (9 elements, row-major)
149    #[serde(skip_serializing_if = "Option::is_none")]
150    pub angular_velocity_covariance: Option<[f32; 9]>,
151
152    /// Linear acceleration (x, y, z) in m/s²
153    pub linear_acceleration: [f32; 3],
154
155    /// Linear acceleration covariance (9 elements, row-major)
156    #[serde(skip_serializing_if = "Option::is_none")]
157    pub linear_acceleration_covariance: Option<[f32; 9]>,
158
159    /// Optional frame ID
160    #[serde(skip_serializing_if = "Option::is_none")]
161    pub frame_id: Option<String>,
162}
163
164/// GPS data
165///
166/// Global positioning data with optional accuracy estimates
167#[derive(Debug, Clone, Serialize, Deserialize)]
168pub struct GpsData {
169    /// Timestamp in microseconds since epoch
170    pub timestamp: u64,
171
172    /// Latitude in degrees
173    pub latitude: f64,
174
175    /// Longitude in degrees
176    pub longitude: f64,
177
178    /// Altitude above mean sea level in meters
179    pub altitude: f64,
180
181    /// Horizontal position accuracy in meters (optional)
182    #[serde(skip_serializing_if = "Option::is_none")]
183    pub horizontal_accuracy: Option<f32>,
184
185    /// Vertical position accuracy in meters (optional)
186    #[serde(skip_serializing_if = "Option::is_none")]
187    pub vertical_accuracy: Option<f32>,
188
189    /// Number of satellites used
190    #[serde(skip_serializing_if = "Option::is_none")]
191    pub satellites: Option<u8>,
192
193    /// GPS fix type (0=no fix, 1=GPS, 2=DGPS, 3=PPS, 4=RTK, 5=Float RTK)
194    #[serde(skip_serializing_if = "Option::is_none")]
195    pub fix_type: Option<u8>,
196}
197
198// Helper methods for Image
199impl Image {
200    /// Creates a new RGB8 image
201    pub fn new_rgb8(width: u32, height: u32, data: Vec<u8>) -> Self {
202        Self {
203            timestamp: crate::prelude::now_micros(),
204            width,
205            height,
206            encoding: "rgb8".to_string(),
207            data,
208            frame_id: None,
209        }
210    }
211
212    /// Creates a new depth image (16-bit unsigned)
213    pub fn new_depth(width: u32, height: u32, data: Vec<u8>) -> Self {
214        Self {
215            timestamp: crate::prelude::now_micros(),
216            width,
217            height,
218            encoding: "16UC1".to_string(),
219            data,
220            frame_id: None,
221        }
222    }
223
224    /// Returns the expected data size in bytes
225    pub fn expected_size(&self) -> usize {
226        let bytes_per_pixel = match self.encoding.as_str() {
227            "rgb8" | "bgr8" => 3,
228            "rgba8" | "bgra8" => 4,
229            "mono8" => 1,
230            "mono16" | "16UC1" => 2,
231            _ => 1, // default
232        };
233        (self.width * self.height) as usize * bytes_per_pixel
234    }
235
236    /// Checks if the image data size is valid
237    pub fn is_valid(&self) -> bool {
238        self.data.len() == self.expected_size()
239    }
240
241    /// Returns the age of this image in microseconds
242    pub fn age_micros(&self) -> u64 {
243        crate::prelude::now_micros().saturating_sub(self.timestamp)
244    }
245}
246
247// Helper methods for LaserScan
248impl LaserScan {
249    /// Returns the number of scan points
250    pub fn num_points(&self) -> usize {
251        self.ranges.len()
252    }
253
254    /// Returns the angle for a given index in radians
255    pub fn angle_at(&self, index: usize) -> Option<f32> {
256        if index < self.ranges.len() {
257            Some(self.angle_min + (index as f32) * self.angle_increment)
258        } else {
259            None
260        }
261    }
262
263    /// Converts scan point to (x, y) coordinates in meters
264    pub fn point_at(&self, index: usize) -> Option<(f32, f32)> {
265        if index >= self.ranges.len() {
266            return None;
267        }
268
269        let range = self.ranges[index];
270        if !self.is_valid_range(range) {
271            return None;
272        }
273
274        let angle = self.angle_min + (index as f32) * self.angle_increment;
275        Some((range * angle.cos(), range * angle.sin()))
276    }
277
278    /// Checks if a range value is valid
279    pub fn is_valid_range(&self, range: f32) -> bool {
280        range.is_finite() && range >= self.range_min && range <= self.range_max
281    }
282
283    /// Returns all valid points as (x, y) coordinates
284    pub fn valid_points(&self) -> Vec<(f32, f32)> {
285        (0..self.ranges.len()).filter_map(|i| self.point_at(i)).collect()
286    }
287
288    /// Returns the age of this scan in microseconds
289    pub fn age_micros(&self) -> u64 {
290        crate::prelude::now_micros().saturating_sub(self.timestamp)
291    }
292}
293
294// Helper methods for Odometry
295impl Odometry {
296    /// Creates a new odometry message at the origin
297    pub fn new() -> Self {
298        Self {
299            timestamp: crate::prelude::now_micros(),
300            position: [0.0, 0.0, 0.0],
301            orientation: [0.0, 0.0, 0.0, 1.0], // Identity quaternion
302            linear_velocity: [0.0, 0.0, 0.0],
303            angular_velocity: [0.0, 0.0, 0.0],
304            frame_id: None,
305            child_frame_id: None,
306        }
307    }
308
309    /// Returns the 2D position (x, y)
310    pub fn position_2d(&self) -> (f32, f32) {
311        (self.position[0], self.position[1])
312    }
313
314    /// Returns the 2D linear velocity (vx, vy)
315    pub fn velocity_2d(&self) -> (f32, f32) {
316        (self.linear_velocity[0], self.linear_velocity[1])
317    }
318
319    /// Returns the speed (magnitude of velocity) in m/s
320    pub fn speed(&self) -> f32 {
321        let vx = self.linear_velocity[0];
322        let vy = self.linear_velocity[1];
323        let vz = self.linear_velocity[2];
324        (vx * vx + vy * vy + vz * vz).sqrt()
325    }
326
327    /// Returns the yaw angle in radians (rotation around z-axis)
328    pub fn yaw(&self) -> f32 {
329        let q = self.orientation;
330        (2.0 * (q[3] * q[2] + q[0] * q[1])).atan2(1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]))
331    }
332
333    /// Calculates distance to another position in meters
334    pub fn distance_to(&self, other: &Odometry) -> f32 {
335        let dx = self.position[0] - other.position[0];
336        let dy = self.position[1] - other.position[1];
337        let dz = self.position[2] - other.position[2];
338        (dx * dx + dy * dy + dz * dz).sqrt()
339    }
340
341    /// Calculates 2D distance to another position in meters
342    pub fn distance_2d_to(&self, other: &Odometry) -> f32 {
343        let dx = self.position[0] - other.position[0];
344        let dy = self.position[1] - other.position[1];
345        (dx * dx + dy * dy).sqrt()
346    }
347}
348
349impl Default for Odometry {
350    fn default() -> Self {
351        Self::new()
352    }
353}
354
355// Helper methods for Imu
356impl Imu {
357    /// Returns the magnitude of linear acceleration in m/s²
358    pub fn acceleration_magnitude(&self) -> f32 {
359        let a = self.linear_acceleration;
360        (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]).sqrt()
361    }
362
363    /// Returns the magnitude of angular velocity in rad/s
364    pub fn angular_speed(&self) -> f32 {
365        let w = self.angular_velocity;
366        (w[0] * w[0] + w[1] * w[1] + w[2] * w[2]).sqrt()
367    }
368
369    /// Normalizes the orientation quaternion
370    pub fn normalize_orientation(&mut self) {
371        let q = self.orientation;
372        let mag = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
373        if mag > 0.0 {
374            self.orientation = [q[0] / mag, q[1] / mag, q[2] / mag, q[3] / mag];
375        }
376    }
377
378    /// Returns the age of this measurement in microseconds
379    pub fn age_micros(&self) -> u64 {
380        crate::prelude::now_micros().saturating_sub(self.timestamp)
381    }
382}
383
384// Helper methods for GpsData
385impl GpsData {
386    /// Calculates distance to another GPS point using Haversine formula (in meters)
387    pub fn distance_to(&self, other: &GpsData) -> f64 {
388        const EARTH_RADIUS_M: f64 = 6371000.0;
389
390        let lat1 = self.latitude.to_radians();
391        let lat2 = other.latitude.to_radians();
392        let dlat = (other.latitude - self.latitude).to_radians();
393        let dlon = (other.longitude - self.longitude).to_radians();
394
395        let a = (dlat / 2.0).sin().powi(2) + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
396        let c = 2.0 * a.sqrt().atan2((1.0 - a).sqrt());
397
398        EARTH_RADIUS_M * c
399    }
400
401    /// Calculates bearing to another GPS point in degrees (0-360)
402    pub fn bearing_to(&self, other: &GpsData) -> f64 {
403        let lat1 = self.latitude.to_radians();
404        let lat2 = other.latitude.to_radians();
405        let dlon = (other.longitude - self.longitude).to_radians();
406
407        let y = dlon.sin() * lat2.cos();
408        let x = lat1.cos() * lat2.sin() - lat1.sin() * lat2.cos() * dlon.cos();
409        let bearing = y.atan2(x).to_degrees();
410
411        (bearing + 360.0) % 360.0
412    }
413
414    /// Checks if the GPS fix is valid
415    pub fn has_valid_fix(&self) -> bool {
416        self.fix_type.is_some_and(|t| t > 0)
417    }
418
419    /// Checks if this is an RTK fix (high precision)
420    pub fn is_rtk_fix(&self) -> bool {
421        self.fix_type.is_some_and(|t| t >= 4)
422    }
423
424    /// Returns the age of this measurement in microseconds
425    pub fn age_micros(&self) -> u64 {
426        crate::prelude::now_micros().saturating_sub(self.timestamp)
427    }
428}
mecha10_core/messages/sensor.rs

mecha10_core/messages/
sensor.rs
