|
// sensor_hub.rs |
|
use std::sync::Arc; |
|
use std::time::{SystemTime, Duration}; |
|
|
|
/// Unified event type for all SensorHub notifications |
|
#[derive(Debug, Clone)] |
|
pub enum SensorHubEvent { |
|
Connect(ConnectStatus), |
|
EntityUpdate(Arc<dyn Entity>), |
|
PoseReceived(Arc<dyn Pose>), |
|
TrackingFrame(Arc<dyn TrackingFrame>), |
|
PatternUpdate(Vec<Arc<dyn Pattern>>), |
|
CalibrationEvent(CalibrationData), |
|
Error(SensorHubError), |
|
} |
|
|
|
/// Core SensorHub functionality |
|
pub trait SensorHub { |
|
fn connect(&self); |
|
fn disconnect(&self); |
|
fn is_connected(&self) -> bool; |
|
|
|
fn get_entities(&self) -> Vec<Arc<dyn Entity>>; |
|
fn get_patterns(&self) -> Vec<Arc<dyn Pattern>>; |
|
} |
|
|
|
/// Trait for configurable aspects of SensorHub |
|
pub trait ConfigurableSensorHub: SensorHub { |
|
fn set_patterns(&mut self, patterns: &str) -> Result<(), SensorHubError>; |
|
fn register_role(&mut self, role: RoleConfig) -> Result<(), SensorHubError>; |
|
fn set_calibration(&mut self, cal: CalibrationParams) -> Result<(), SensorHubError>; |
|
} |
|
|
|
/// Observable events trait with unified event type |
|
pub trait ObservableSensorHub: SensorHub { |
|
type Receiver: Stream<Item = SensorHubEvent> + Unpin + Send; |
|
|
|
fn subscribe(&self) -> Self::Receiver; |
|
fn subscribe_to( |
|
&self, |
|
filter: EventFilter |
|
) -> Self::Receiver; |
|
} |
|
|
|
// pose.rs |
|
pub trait Pose { |
|
fn entity(&self) -> Option<Arc<dyn Entity>>; |
|
fn timestamp(&self) -> SystemTime; |
|
fn transform(&self) -> Isometry3d; |
|
fn position_uncertainty(&self) -> Option<Vector3d>; |
|
fn warnings(&self) -> PoseWarnings; |
|
} |
|
|
|
// entity.rs |
|
#[derive(Debug, Clone, PartialEq)] |
|
pub enum EntityType { |
|
ReferenceElement, |
|
Tracker, |
|
Headset, |
|
Camera, |
|
} |
|
|
|
pub trait Entity { |
|
fn id(&self) -> &str; |
|
fn entity_type(&self) -> EntityType; |
|
fn latest_pose(&self, max_age: Option<Duration>) -> Option<Arc<dyn Pose>>; |
|
fn tracking_frames(&self) -> Vec<Arc<dyn TrackingFrame>>; |
|
} |
|
|
|
// tracking_frame.rs |
|
pub trait TrackingFrame { |
|
fn frame_id(&self) -> u64; |
|
fn timestamp(&self) -> SystemTime; |
|
fn poses(&self) -> HashMap<String, Arc<dyn Pose>>; |
|
fn strays(&self) -> Vec<Arc<dyn Stray>>; |
|
} |
|
|
|
// patterns.rs |
|
pub trait Pattern { |
|
fn id(&self) -> &str; |
|
fn markers(&self) -> Vec<Arc<dyn Marker>>; |
|
} |
|
|
|
pub trait Marker { |
|
fn centroid(&self) -> Point3d; |
|
fn uncertainty(&self) -> Option<f64>; |
|
} |
|
|
|
// Implementation pattern example |
|
pub struct DefaultSensorHub { |
|
// Channel for broadcasting events |
|
event_tx: broadcast::Sender<SensorHubEvent>, |
|
|
|
// Internal state |
|
entities: DashMap<String, Arc<dyn Entity>>, |
|
patterns: RwLock<Vec<Arc<dyn Pattern>>>, |
|
} |
|
|
|
impl ObservableSensorHub for DefaultSensorHub { |
|
type Receiver = broadcast::Receiver<SensorHubEvent>; |
|
|
|
fn subscribe(&self) -> Self::Receiver { |
|
self.event_tx.subscribe() |
|
} |
|
} |
|
|
|
impl ConfigurableSensorHub for DefaultSensorHub { |
|
fn set_patterns(&mut self, json: &str) -> Result<()> { |
|
let new_patterns = parse_patterns(json)?; |
|
let old = mem::replace(&mut *self.patterns.write(), new_patterns); |
|
self.event_tx.send(SensorHubEvent::PatternUpdate(old)); |
|
Ok(()) |
|
} |
|
} |
