mecha10_controllers/mock/
lidar.rs

1//! Mock LiDAR controller for testing
2
3use crate::lidar::*;
4use crate::{Controller, ControllerCapabilities, ControllerError, ControllerHealth, ControllerState};
5use async_trait::async_trait;
6use std::time::{SystemTime, UNIX_EPOCH};
7
8#[derive(Debug, Clone)]
9pub struct MockLidarConfig {
10    pub scan_frequency: f32,
11    pub angular_resolution: f32,
12    pub range_max: f32,
13}
14
15impl Default for MockLidarConfig {
16    fn default() -> Self {
17        Self {
18            scan_frequency: 10.0,
19            angular_resolution: 0.36,
20            range_max: 12.0,
21        }
22    }
23}
24
25pub struct MockLidarController {
26    config: MockLidarConfig,
27    state: ControllerState,
28    scan_count: u64,
29}
30
31#[async_trait]
32impl Controller for MockLidarController {
33    type Config = MockLidarConfig;
34    type Error = ControllerError;
35
36    async fn init(config: Self::Config) -> Result<Self, Self::Error> {
37        Ok(Self {
38            config,
39            state: ControllerState::Initialized,
40            scan_count: 0,
41        })
42    }
43
44    async fn start(&mut self) -> Result<(), Self::Error> {
45        self.state = ControllerState::Running;
46        Ok(())
47    }
48
49    async fn stop(&mut self) -> Result<(), Self::Error> {
50        self.state = ControllerState::Stopped;
51        Ok(())
52    }
53
54    async fn health_check(&self) -> ControllerHealth {
55        match self.state {
56            ControllerState::Running => ControllerHealth::Healthy,
57            _ => ControllerHealth::Unknown,
58        }
59    }
60
61    fn capabilities(&self) -> ControllerCapabilities {
62        ControllerCapabilities::new("lidar", "mock")
63            .with_vendor("Mecha10")
64            .with_model("Mock LiDAR")
65            .with_feature("intensity", true)
66    }
67}
68
69#[async_trait]
70impl LidarController for MockLidarController {
71    type Scan = LaserScan2D;
72
73    async fn get_scan(&mut self) -> Result<Self::Scan, Self::Error> {
74        if self.state != ControllerState::Running {
75            return Err(ControllerError::InvalidState("LiDAR not running".to_string()));
76        }
77
78        self.scan_count += 1;
79
80        let num_points = (360.0 / self.config.angular_resolution) as usize;
81        let mut ranges = Vec::with_capacity(num_points);
82        let mut intensities = Vec::with_capacity(num_points);
83
84        // Generate synthetic scan (circular obstacle at 2m)
85        for i in 0..num_points {
86            let angle = (i as f32) * self.config.angular_resolution.to_radians();
87            let base_range = 2.0 + (angle.sin() * 0.5);
88            ranges.push(base_range);
89            intensities.push(((i % 100) as f32) / 100.0);
90        }
91
92        Ok(LaserScan2D {
93            timestamp: SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_micros() as u64,
94            scan_id: self.scan_count,
95            angle_min: 0.0,
96            angle_max: 2.0 * std::f32::consts::PI,
97            angle_increment: self.config.angular_resolution.to_radians(),
98            range_min: 0.15,
99            range_max: self.config.range_max,
100            ranges,
101            intensities,
102            quality: vec![],
103        })
104    }
105
106    fn info(&self) -> LidarInfo {
107        LidarInfo {
108            model: "Mock LiDAR".to_string(),
109            scan_type: ScanType::Planar2D,
110            range_min: 0.15,
111            range_max: self.config.range_max,
112            angular_resolution: self.config.angular_resolution,
113            scan_frequency: self.config.scan_frequency,
114            points_per_scan: Some((360.0 / self.config.angular_resolution) as usize),
115        }
116    }
117
118    fn field_of_view(&self) -> FieldOfView {
119        FieldOfView {
120            horizontal: 2.0 * std::f32::consts::PI,
121            vertical: None,
122            angle_min: 0.0,
123            angle_max: 2.0 * std::f32::consts::PI,
124        }
125    }
126}
mecha10_controllers/mock/lidar.rs

mecha10_controllers/mock/
lidar.rs
