ruvector_robotics/bridge/
mod.rs

1//! Core robotics types, converters, spatial indexing, and perception pipeline.
2//!
3//! This module provides the foundational types that all other robotics modules
4//! build upon: point clouds, robot state, scene graphs, poses, and trajectories.
5
6pub mod config;
7pub mod converters;
8pub mod gaussian;
9pub mod indexing;
10pub mod pipeline;
11pub mod search;
12
13use serde::{Deserialize, Serialize};
14
15// Re-exports
16pub use config::{BridgeConfig, DistanceMetric};
17pub use converters::ConversionError;
18pub use gaussian::{GaussianConfig, GaussianSplat, GaussianSplatCloud};
19pub use indexing::{IndexError, SpatialIndex};
20pub use pipeline::{PerceptionResult, PipelineConfig, PipelineStats};
21pub use search::{AlertSeverity, Neighbor, ObstacleAlert, SearchResult};
22
23// ---------------------------------------------------------------------------
24// Core types
25// ---------------------------------------------------------------------------
26
27/// 3D point used in point clouds and spatial operations.
28#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
29pub struct Point3D {
30    pub x: f32,
31    pub y: f32,
32    pub z: f32,
33}
34
35impl Point3D {
36    pub fn new(x: f32, y: f32, z: f32) -> Self {
37        Self { x, y, z }
38    }
39
40    pub fn distance_to(&self, other: &Point3D) -> f32 {
41        let dx = self.x - other.x;
42        let dy = self.y - other.y;
43        let dz = self.z - other.z;
44        (dx * dx + dy * dy + dz * dz).sqrt()
45    }
46
47    pub fn as_f64_array(&self) -> [f64; 3] {
48        [self.x as f64, self.y as f64, self.z as f64]
49    }
50
51    pub fn from_f64_array(arr: &[f64; 3]) -> Self {
52        Self {
53            x: arr[0] as f32,
54            y: arr[1] as f32,
55            z: arr[2] as f32,
56        }
57    }
58
59    pub fn to_vec(&self) -> Vec<f32> {
60        vec![self.x, self.y, self.z]
61    }
62}
63
64/// A unit quaternion representing a 3-D rotation.
65#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
66pub struct Quaternion {
67    pub x: f64,
68    pub y: f64,
69    pub z: f64,
70    pub w: f64,
71}
72
73impl Default for Quaternion {
74    fn default() -> Self {
75        Self { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }
76    }
77}
78
79impl Quaternion {
80    pub fn identity() -> Self {
81        Self::default()
82    }
83
84    pub fn new(x: f64, y: f64, z: f64, w: f64) -> Self {
85        Self { x, y, z, w }
86    }
87}
88
89/// A 6-DOF pose: position + orientation.
90#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
91pub struct Pose {
92    pub position: [f64; 3],
93    pub orientation: Quaternion,
94    pub frame_id: String,
95}
96
97impl Default for Pose {
98    fn default() -> Self {
99        Self {
100            position: [0.0; 3],
101            orientation: Quaternion::identity(),
102            frame_id: String::new(),
103        }
104    }
105}
106
107/// A collection of 3D points from a sensor (LiDAR, depth camera, etc.).
108#[derive(Debug, Clone, Default, Serialize, Deserialize)]
109pub struct PointCloud {
110    pub points: Vec<Point3D>,
111    pub intensities: Vec<f32>,
112    pub normals: Option<Vec<Point3D>>,
113    pub timestamp_us: i64,
114    pub frame_id: String,
115}
116
117impl PointCloud {
118    pub fn new(points: Vec<Point3D>, timestamp: i64) -> Self {
119        let len = points.len();
120        Self {
121            points,
122            intensities: vec![1.0; len],
123            normals: None,
124            timestamp_us: timestamp,
125            frame_id: String::new(),
126        }
127    }
128
129    pub fn len(&self) -> usize {
130        self.points.len()
131    }
132
133    pub fn is_empty(&self) -> bool {
134        self.points.is_empty()
135    }
136
137    pub fn timestamp(&self) -> i64 {
138        self.timestamp_us
139    }
140}
141
142/// Robot state: position, velocity, acceleration, timestamp.
143#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
144pub struct RobotState {
145    pub position: [f64; 3],
146    pub velocity: [f64; 3],
147    pub acceleration: [f64; 3],
148    pub timestamp_us: i64,
149}
150
151impl Default for RobotState {
152    fn default() -> Self {
153        Self {
154            position: [0.0; 3],
155            velocity: [0.0; 3],
156            acceleration: [0.0; 3],
157            timestamp_us: 0,
158        }
159    }
160}
161
162/// A synchronised bundle of sensor observations captured at one instant.
163#[derive(Debug, Clone, Default, Serialize, Deserialize)]
164pub struct SensorFrame {
165    pub cloud: Option<PointCloud>,
166    pub state: Option<RobotState>,
167    pub pose: Option<Pose>,
168    pub timestamp_us: i64,
169}
170
171/// A 2-D occupancy grid map.
172#[derive(Debug, Clone, Serialize, Deserialize)]
173pub struct OccupancyGrid {
174    pub width: usize,
175    pub height: usize,
176    pub resolution: f64,
177    pub data: Vec<f32>,
178    pub origin: [f64; 3],
179}
180
181impl OccupancyGrid {
182    pub fn new(width: usize, height: usize, resolution: f64) -> Self {
183        let size = width.checked_mul(height).expect("grid size overflow");
184        Self {
185            width,
186            height,
187            resolution,
188            data: vec![0.0; size],
189            origin: [0.0; 3],
190        }
191    }
192
193    /// Get the occupancy value at `(x, y)`, or `None` if out of bounds.
194    pub fn get(&self, x: usize, y: usize) -> Option<f32> {
195        if x < self.width && y < self.height {
196            Some(self.data[y * self.width + x])
197        } else {
198            None
199        }
200    }
201
202    /// Set the occupancy value at `(x, y)`. Out-of-bounds writes are ignored.
203    pub fn set(&mut self, x: usize, y: usize, value: f32) {
204        if x < self.width && y < self.height {
205            self.data[y * self.width + x] = value;
206        }
207    }
208}
209
210/// An object detected in a scene with bounding information.
211#[derive(Debug, Clone, Serialize, Deserialize)]
212pub struct SceneObject {
213    pub id: usize,
214    pub center: [f64; 3],
215    pub extent: [f64; 3],
216    pub confidence: f32,
217    pub label: String,
218    pub velocity: Option<[f64; 3]>,
219}
220
221impl SceneObject {
222    pub fn new(id: usize, center: [f64; 3], extent: [f64; 3]) -> Self {
223        Self {
224            id,
225            center,
226            extent,
227            confidence: 1.0,
228            label: String::new(),
229            velocity: None,
230        }
231    }
232}
233
234/// An edge in a scene graph connecting two objects.
235#[derive(Debug, Clone, Serialize, Deserialize)]
236pub struct SceneEdge {
237    pub from: usize,
238    pub to: usize,
239    pub distance: f64,
240    pub relation: String,
241}
242
243/// A scene graph representing spatial relationships between objects.
244#[derive(Debug, Clone, Default, Serialize, Deserialize)]
245pub struct SceneGraph {
246    pub objects: Vec<SceneObject>,
247    pub edges: Vec<SceneEdge>,
248    pub timestamp: i64,
249}
250
251impl SceneGraph {
252    pub fn new(objects: Vec<SceneObject>, edges: Vec<SceneEdge>, timestamp: i64) -> Self {
253        Self { objects, edges, timestamp }
254    }
255}
256
257/// A predicted trajectory consisting of waypoints.
258#[derive(Debug, Clone, Serialize, Deserialize)]
259pub struct Trajectory {
260    pub waypoints: Vec<[f64; 3]>,
261    pub timestamps: Vec<i64>,
262    pub confidence: f64,
263}
264
265impl Trajectory {
266    pub fn new(waypoints: Vec<[f64; 3]>, timestamps: Vec<i64>, confidence: f64) -> Self {
267        Self { waypoints, timestamps, confidence }
268    }
269
270    pub fn len(&self) -> usize {
271        self.waypoints.len()
272    }
273
274    pub fn is_empty(&self) -> bool {
275        self.waypoints.is_empty()
276    }
277}
278
279/// An obstacle detected by the perception pipeline.
280#[derive(Debug, Clone, Serialize, Deserialize)]
281pub struct Obstacle {
282    pub id: u64,
283    pub position: [f64; 3],
284    pub distance: f64,
285    pub radius: f64,
286    pub label: String,
287    pub confidence: f32,
288}
289
290/// Bridge error type.
291#[derive(Debug, thiserror::Error)]
292pub enum BridgeError {
293    #[error("Invalid data: {0}")]
294    InvalidData(String),
295    #[error("Conversion error: {0}")]
296    ConversionError(String),
297    #[error("Serialization error: {0}")]
298    SerializationError(#[from] serde_json::Error),
299}
300
301pub type Result<T> = std::result::Result<T, BridgeError>;
302
303#[cfg(test)]
304mod tests {
305    use super::*;
306
307    #[test]
308    fn test_point3d_distance() {
309        let a = Point3D::new(0.0, 0.0, 0.0);
310        let b = Point3D::new(3.0, 4.0, 0.0);
311        assert!((a.distance_to(&b) - 5.0).abs() < 1e-6);
312    }
313
314    #[test]
315    fn test_point_cloud() {
316        let cloud = PointCloud::new(vec![Point3D::new(1.0, 2.0, 3.0)], 100);
317        assert_eq!(cloud.len(), 1);
318        assert_eq!(cloud.timestamp(), 100);
319    }
320
321    #[test]
322    fn test_robot_state_default() {
323        let state = RobotState::default();
324        assert_eq!(state.position, [0.0; 3]);
325        assert_eq!(state.velocity, [0.0; 3]);
326    }
327
328    #[test]
329    fn test_scene_graph() {
330        let obj = SceneObject::new(0, [1.0, 2.0, 3.0], [0.5, 0.5, 0.5]);
331        let graph = SceneGraph::new(vec![obj], vec![], 0);
332        assert_eq!(graph.objects.len(), 1);
333    }
334
335    #[test]
336    fn test_quaternion_identity() {
337        let q = Quaternion::identity();
338        assert_eq!(q.w, 1.0);
339        assert_eq!(q.x, 0.0);
340    }
341
342    #[test]
343    fn test_pose_default() {
344        let p = Pose::default();
345        assert_eq!(p.position, [0.0; 3]);
346        assert_eq!(p.orientation.w, 1.0);
347    }
348
349    #[test]
350    fn test_sensor_frame_default() {
351        let f = SensorFrame::default();
352        assert!(f.cloud.is_none());
353        assert!(f.state.is_none());
354        assert!(f.pose.is_none());
355    }
356
357    #[test]
358    fn test_occupancy_grid() {
359        let mut grid = OccupancyGrid::new(10, 10, 0.05);
360        grid.set(3, 4, 0.8);
361        assert!((grid.get(3, 4).unwrap() - 0.8).abs() < f32::EPSILON);
362        assert!(grid.get(10, 10).is_none()); // out of bounds
363    }
364
365    #[test]
366    fn test_trajectory() {
367        let t = Trajectory::new(
368            vec![[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]],
369            vec![100, 200],
370            0.95,
371        );
372        assert_eq!(t.len(), 2);
373        assert!(!t.is_empty());
374    }
375
376    #[test]
377    fn test_serde_roundtrip() {
378        let obj = SceneObject::new(0, [1.0, 2.0, 3.0], [0.5, 0.5, 0.5]);
379        let json = serde_json::to_string(&obj).unwrap();
380        let obj2: SceneObject = serde_json::from_str(&json).unwrap();
381        assert_eq!(obj.id, obj2.id);
382    }
383}
ruvector_robotics/bridge/mod.rs

ruvector_robotics/bridge/
mod.rs
