mecha10_controllers/
lidar.rs

1//! LiDAR controller trait and types
2
3use crate::{Controller, ControllerError};
4use async_trait::async_trait;
5use serde::{Deserialize, Serialize};
6
7/// LiDAR controller trait
8///
9/// Provides a unified interface for different LiDAR types:
10/// - 2D scanning LiDARs (RPLidar, SICK TiM/LMS)
11/// - 3D spinning LiDARs (Velodyne, Ouster)
12/// - Solid-state LiDARs
13#[async_trait]
14pub trait LidarController: Controller {
15    /// Scan type produced by this LiDAR
16    type Scan: Send + Sync;
17
18    /// Get the next scan from the LiDAR
19    ///
20    /// This will block until a complete scan is available or an error occurs.
21    async fn get_scan(&mut self) -> Result<Self::Scan, Self::Error>;
22
23    /// Set motor speed (for spinning LiDARs)
24    ///
25    /// Sets the rotation speed in RPM. Not all LiDARs support variable speed.
26    async fn set_motor_speed(&mut self, _rpm: u16) -> Result<(), Self::Error>
27    where
28        Self::Error: From<ControllerError>,
29    {
30        Err(Self::Error::from(ControllerError::NotSupported(
31            "Motor speed control not supported".to_string(),
32        )))
33    }
34
35    /// Get current motor speed (for spinning LiDARs)
36    async fn get_motor_speed(&self) -> Result<u16, Self::Error>
37    where
38        Self::Error: From<ControllerError>,
39    {
40        Err(Self::Error::from(ControllerError::NotSupported(
41            "Motor speed reading not supported".to_string(),
42        )))
43    }
44
45    /// Get LiDAR information
46    fn info(&self) -> LidarInfo;
47
48    /// Get field of view
49    fn field_of_view(&self) -> FieldOfView;
50
51    /// Check if this is a 3D LiDAR
52    fn is_3d(&self) -> bool {
53        self.capabilities().features.get("3d").copied().unwrap_or(false)
54    }
55
56    /// Check if this LiDAR provides intensity data
57    fn has_intensity(&self) -> bool {
58        self.capabilities().features.get("intensity").copied().unwrap_or(false)
59    }
60}
61
62// ============================================================================
63// LiDAR Types
64// ============================================================================
65
66/// LiDAR information
67#[derive(Debug, Clone, Serialize, Deserialize)]
68pub struct LidarInfo {
69    /// LiDAR model name
70    pub model: String,
71
72    /// Scan type (2D or 3D)
73    pub scan_type: ScanType,
74
75    /// Minimum range in meters
76    pub range_min: f32,
77
78    /// Maximum range in meters
79    pub range_max: f32,
80
81    /// Angular resolution in degrees
82    pub angular_resolution: f32,
83
84    /// Scan frequency in Hz
85    pub scan_frequency: f32,
86
87    /// Number of measurement points per scan
88    pub points_per_scan: Option<usize>,
89}
90
91impl Default for LidarInfo {
92    fn default() -> Self {
93        Self {
94            model: "Unknown".to_string(),
95            scan_type: ScanType::Planar2D,
96            range_min: 0.15,
97            range_max: 12.0,
98            angular_resolution: 0.36,
99            scan_frequency: 10.0,
100            points_per_scan: None,
101        }
102    }
103}
104
105/// Scan type
106#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
107pub enum ScanType {
108    /// 2D planar scan (single horizontal plane)
109    Planar2D,
110
111    /// 3D volumetric scan (spinning or multi-beam)
112    Volumetric3D,
113
114    /// Multi-layer 2D (multiple horizontal planes)
115    MultiLayer,
116}
117
118/// Field of view
119#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
120pub struct FieldOfView {
121    /// Horizontal FOV in radians
122    pub horizontal: f32,
123
124    /// Vertical FOV in radians (for 3D LiDARs)
125    pub vertical: Option<f32>,
126
127    /// Minimum angle in radians
128    pub angle_min: f32,
129
130    /// Maximum angle in radians
131    pub angle_max: f32,
132}
133
134impl Default for FieldOfView {
135    fn default() -> Self {
136        Self {
137            horizontal: 2.0 * std::f32::consts::PI, // 360 degrees
138            vertical: None,
139            angle_min: 0.0,
140            angle_max: 2.0 * std::f32::consts::PI,
141        }
142    }
143}
144
145// ============================================================================
146// Scan Data Types
147// ============================================================================
148
149/// 2D laser scan data
150#[derive(Debug, Clone, Serialize, Deserialize)]
151pub struct LaserScan2D {
152    /// Timestamp (microseconds since epoch)
153    pub timestamp: u64,
154
155    /// Scan sequence number
156    pub scan_id: u64,
157
158    /// Minimum angle in radians
159    pub angle_min: f32,
160
161    /// Maximum angle in radians
162    pub angle_max: f32,
163
164    /// Angular increment between measurements
165    pub angle_increment: f32,
166
167    /// Minimum range in meters
168    pub range_min: f32,
169
170    /// Maximum range in meters
171    pub range_max: f32,
172
173    /// Range measurements in meters
174    pub ranges: Vec<f32>,
175
176    /// Intensity measurements (if available)
177    pub intensities: Vec<f32>,
178
179    /// Quality/reliability values (if available)
180    pub quality: Vec<u8>,
181}
182
183impl Default for LaserScan2D {
184    fn default() -> Self {
185        Self {
186            timestamp: 0,
187            scan_id: 0,
188            angle_min: 0.0,
189            angle_max: 2.0 * std::f32::consts::PI,
190            angle_increment: 0.01,
191            range_min: 0.15,
192            range_max: 12.0,
193            ranges: Vec::new(),
194            intensities: Vec::new(),
195            quality: Vec::new(),
196        }
197    }
198}
199
200impl LaserScan2D {
201    /// Get the number of points in this scan
202    pub fn num_points(&self) -> usize {
203        self.ranges.len()
204    }
205
206    /// Get cartesian coordinates for a specific point
207    pub fn point_at(&self, index: usize) -> Option<(f32, f32)> {
208        if index >= self.ranges.len() {
209            return None;
210        }
211
212        let range = self.ranges[index];
213        if range < self.range_min || range > self.range_max {
214            return None;
215        }
216
217        let angle = self.angle_min + (index as f32) * self.angle_increment;
218        let x = range * angle.cos();
219        let y = range * angle.sin();
220
221        Some((x, y))
222    }
223
224    /// Convert to point cloud
225    pub fn to_point_cloud(&self) -> Vec<(f32, f32)> {
226        (0..self.ranges.len()).filter_map(|i| self.point_at(i)).collect()
227    }
228}
229
230/// 3D point cloud data
231#[derive(Debug, Clone, Serialize, Deserialize)]
232pub struct PointCloud3D {
233    /// Timestamp (microseconds since epoch)
234    pub timestamp: u64,
235
236    /// Scan sequence number
237    pub scan_id: u64,
238
239    /// Points in 3D space [x, y, z]
240    pub points: Vec<[f32; 3]>,
241
242    /// Intensity values for each point (if available)
243    pub intensities: Vec<f32>,
244
245    /// Color values for each point (if available) [r, g, b]
246    pub colors: Vec<[u8; 3]>,
247
248    /// Point cloud frame ID
249    pub frame_id: String,
250}
251
252impl Default for PointCloud3D {
253    fn default() -> Self {
254        Self {
255            timestamp: 0,
256            scan_id: 0,
257            points: Vec::new(),
258            intensities: Vec::new(),
259            colors: Vec::new(),
260            frame_id: "lidar".to_string(),
261        }
262    }
263}
264
265impl PointCloud3D {
266    /// Get the number of points in this cloud
267    pub fn num_points(&self) -> usize {
268        self.points.len()
269    }
270
271    /// Filter points by distance range
272    pub fn filter_range(&self, min: f32, max: f32) -> Self {
273        let mut filtered = Self {
274            timestamp: self.timestamp,
275            scan_id: self.scan_id,
276            frame_id: self.frame_id.clone(),
277            ..Default::default()
278        };
279
280        for (i, point) in self.points.iter().enumerate() {
281            let distance = (point[0] * point[0] + point[1] * point[1] + point[2] * point[2]).sqrt();
282            if distance >= min && distance <= max {
283                filtered.points.push(*point);
284                if i < self.intensities.len() {
285                    filtered.intensities.push(self.intensities[i]);
286                }
287                if i < self.colors.len() {
288                    filtered.colors.push(self.colors[i]);
289                }
290            }
291        }
292
293        filtered
294    }
295}
296
297// ============================================================================
298// LiDAR Configuration
299// ============================================================================
300
301/// Common LiDAR configuration
302#[derive(Debug, Clone, Serialize, Deserialize)]
303pub struct LidarConfig {
304    /// Scan frequency in Hz
305    pub scan_frequency: f32,
306
307    /// Angular resolution in degrees
308    pub angular_resolution: f32,
309
310    /// Minimum range in meters
311    pub range_min: f32,
312
313    /// Maximum range in meters
314    pub range_max: f32,
315
316    /// Enable intensity data
317    pub enable_intensity: bool,
318
319    /// Motor speed in RPM (for spinning LiDARs)
320    pub motor_rpm: Option<u16>,
321
322    /// Filter invalid measurements
323    pub filter_invalid: bool,
324
325    /// Median filter window size (0 = disabled)
326    pub median_filter_size: usize,
327}
328
329impl Default for LidarConfig {
330    fn default() -> Self {
331        Self {
332            scan_frequency: 10.0,
333            angular_resolution: 0.36,
334            range_min: 0.15,
335            range_max: 12.0,
336            enable_intensity: true,
337            motor_rpm: None,
338            filter_invalid: true,
339            median_filter_size: 0,
340        }
341    }
342}
343
344// ============================================================================
345// Health and Diagnostics
346// ============================================================================
347
348/// LiDAR health status
349#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)]
350pub enum LidarHealthStatus {
351    /// LiDAR is healthy
352    Good,
353
354    /// LiDAR is operational but with warnings
355    Warning,
356
357    /// LiDAR has errors
358    Error,
359
360    /// Health status unknown
361    Unknown,
362}
363
364/// LiDAR diagnostic information
365#[derive(Debug, Clone, Serialize, Deserialize)]
366pub struct LidarDiagnostics {
367    /// Health status
368    pub health: LidarHealthStatus,
369
370    /// Current motor speed (RPM)
371    pub motor_rpm: Option<u16>,
372
373    /// Temperature in Celsius
374    pub temperature: Option<f32>,
375
376    /// Error code if any
377    pub error_code: Option<u32>,
378
379    /// Error message if any
380    pub error_message: Option<String>,
381
382    /// Number of valid points in last scan
383    pub valid_points: Option<usize>,
384
385    /// Number of invalid points in last scan
386    pub invalid_points: Option<usize>,
387
388    /// Scan rate in Hz
389    pub actual_scan_rate: Option<f32>,
390}
391
392impl Default for LidarDiagnostics {
393    fn default() -> Self {
394        Self {
395            health: LidarHealthStatus::Unknown,
396            motor_rpm: None,
397            temperature: None,
398            error_code: None,
399            error_message: None,
400            valid_points: None,
401            invalid_points: None,
402            actual_scan_rate: None,
403        }
404    }
405}
mecha10_controllers/lidar.rs

mecha10_controllers/
lidar.rs
