Merge commit 'd803bfe2b1fe7f5e219e50ac20d6801a0a58ac75' as 'vendor/ruvector'

vendor/ruvector/crates/agentic-robotics-rt/src/executor.rs

@@ -0,0 +1,157 @@
+//! Unified async real-time executor
+//!
+//! Combines Tokio for soft real-time I/O and priority scheduling for hard real-time tasks
+
+use crate::scheduler::PriorityScheduler;
+use crate::RTPriority;
+use anyhow::Result;
+use parking_lot::Mutex;
+use std::future::Future;
+use std::sync::Arc;
+use std::time::Duration;
+use tokio::runtime::{Builder, Runtime};
+use tracing::{debug, info};
+
+/// Task priority wrapper
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct Priority(pub u8);
+
+/// Task deadline
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct Deadline(pub Duration);
+
+impl From<Duration> for Deadline {
+    fn from(duration: Duration) -> Self {
+        Deadline(duration)
+    }
+}
+
+/// ROS3 unified executor
+pub struct ROS3Executor {
+    tokio_rt_high: Runtime,
+    tokio_rt_low: Runtime,
+    scheduler: Arc<Mutex<PriorityScheduler>>,
+}
+
+impl ROS3Executor {
+    /// Create a new executor
+    pub fn new() -> Result<Self> {
+        info!("Initializing ROS3 unified executor");
+
+        // High-priority runtime for control loops (2 threads)
+        let tokio_rt_high = Builder::new_multi_thread()
+            .worker_threads(2)
+            .thread_name("ros3-rt-high")
+            .enable_all()
+            .build()?;
+
+        // Low-priority runtime for planning (4 threads)
+        let tokio_rt_low = Builder::new_multi_thread()
+            .worker_threads(4)
+            .thread_name("ros3-rt-low")
+            .enable_all()
+            .build()?;
+
+        let scheduler = Arc::new(Mutex::new(PriorityScheduler::new()));
+
+        Ok(Self {
+            tokio_rt_high,
+            tokio_rt_low,
+            scheduler,
+        })
+    }
+
+    /// Spawn a real-time task with priority and deadline
+    pub fn spawn_rt<F>(&self, priority: Priority, deadline: Deadline, task: F)
+    where
+        F: Future<Output = ()> + Send + 'static,
+    {
+        let rt_priority: RTPriority = priority.0.into();
+
+        debug!(
+            "Spawning RT task with priority {:?} and deadline {:?}",
+            rt_priority, deadline.0
+        );
+
+        // Route to appropriate runtime based on deadline
+        if deadline.0 < Duration::from_millis(1) {
+            // Hard RT: Use high-priority runtime
+            self.tokio_rt_high.spawn(async move {
+                // In a real implementation with RTIC, this would use hardware interrupts
+                task.await;
+            });
+        } else {
+            // Soft RT: Use low-priority runtime
+            self.tokio_rt_low.spawn(async move {
+                task.await;
+            });
+        }
+    }
+
+    /// Spawn a high-priority task
+    pub fn spawn_high<F>(&self, task: F)
+    where
+        F: Future<Output = ()> + Send + 'static,
+    {
+        self.spawn_rt(Priority(3), Deadline(Duration::from_micros(500)), task);
+    }
+
+    /// Spawn a low-priority task
+    pub fn spawn_low<F>(&self, task: F)
+    where
+        F: Future<Output = ()> + Send + 'static,
+    {
+        self.spawn_rt(Priority(1), Deadline(Duration::from_millis(100)), task);
+    }
+
+    /// Spawn CPU-bound blocking work
+    pub fn spawn_blocking<F, R>(&self, f: F) -> tokio::task::JoinHandle<R>
+    where
+        F: FnOnce() -> R + Send + 'static,
+        R: Send + 'static,
+    {
+        self.tokio_rt_low.spawn_blocking(f)
+    }
+
+    /// Get a handle to the high-priority runtime
+    pub fn high_priority_runtime(&self) -> &Runtime {
+        &self.tokio_rt_high
+    }
+
+    /// Get a handle to the low-priority runtime
+    pub fn low_priority_runtime(&self) -> &Runtime {
+        &self.tokio_rt_low
+    }
+}
+
+impl Default for ROS3Executor {
+    fn default() -> Self {
+        Self::new().expect("Failed to create ROS3Executor")
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use std::sync::atomic::{AtomicBool, Ordering};
+
+    #[test]
+    fn test_executor_creation() {
+        let executor = ROS3Executor::new();
+        assert!(executor.is_ok());
+    }
+
+    #[test]
+    fn test_spawn_high_priority() {
+        let executor = ROS3Executor::new().unwrap();
+        let completed = Arc::new(AtomicBool::new(false));
+        let completed_clone = completed.clone();
+
+        executor.spawn_high(async move {
+            completed_clone.store(true, Ordering::SeqCst);
+        });
+
+        std::thread::sleep(Duration::from_millis(100));
+        // Note: In a real test, we'd use proper synchronization
+    }
+}

vendor/ruvector/crates/agentic-robotics-rt/src/latency.rs

@@ -0,0 +1,145 @@
+//! High-precision latency tracking using HDR histogram
+
+use hdrhistogram::Histogram;
+use parking_lot::Mutex;
+use std::sync::Arc;
+use std::time::{Duration, Instant};
+
+/// Latency tracker with HDR histogram
+pub struct LatencyTracker {
+    histogram: Arc<Mutex<Histogram<u64>>>,
+    name: String,
+}
+
+impl LatencyTracker {
+    /// Create a new latency tracker
+    pub fn new(name: impl Into<String>) -> Self {
+        // 3 significant digits, max value 1 hour in microseconds
+        let histogram = Histogram::<u64>::new(3)
+            .expect("Failed to create histogram");
+
+        Self {
+            histogram: Arc::new(Mutex::new(histogram)),
+            name: name.into(),
+        }
+    }
+
+    /// Record a latency measurement
+    pub fn record(&self, duration: Duration) {
+        let micros = duration.as_micros() as u64;
+        if let Some(mut hist) = self.histogram.try_lock() {
+            let _ = hist.record(micros);
+        }
+    }
+
+    /// Get latency statistics
+    pub fn stats(&self) -> LatencyStats {
+        let hist = self.histogram.lock();
+
+        LatencyStats {
+            name: self.name.clone(),
+            count: hist.len(),
+            min: hist.min(),
+            max: hist.max(),
+            mean: hist.mean(),
+            p50: hist.value_at_quantile(0.50),
+            p90: hist.value_at_quantile(0.90),
+            p99: hist.value_at_quantile(0.99),
+            p999: hist.value_at_quantile(0.999),
+        }
+    }
+
+    /// Reset the histogram
+    pub fn reset(&self) {
+        self.histogram.lock().reset();
+    }
+
+    /// Create a measurement guard
+    pub fn measure(&self) -> LatencyMeasurement {
+        LatencyMeasurement {
+            tracker: self.clone(),
+            start: Instant::now(),
+        }
+    }
+}
+
+impl Clone for LatencyTracker {
+    fn clone(&self) -> Self {
+        Self {
+            histogram: self.histogram.clone(),
+            name: self.name.clone(),
+        }
+    }
+}
+
+/// Latency statistics
+#[derive(Debug, Clone)]
+pub struct LatencyStats {
+    pub name: String,
+    pub count: u64,
+    pub min: u64,
+    pub max: u64,
+    pub mean: f64,
+    pub p50: u64,
+    pub p90: u64,
+    pub p99: u64,
+    pub p999: u64,
+}
+
+impl std::fmt::Display for LatencyStats {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        write!(
+            f,
+            "{}: count={}, min={}µs, max={}µs, mean={:.2}µs, p50={}µs, p90={}µs, p99={}µs, p99.9={}µs",
+            self.name, self.count, self.min, self.max, self.mean, self.p50, self.p90, self.p99, self.p999
+        )
+    }
+}
+
+/// RAII guard for automatic latency measurement
+pub struct LatencyMeasurement {
+    tracker: LatencyTracker,
+    start: Instant,
+}
+
+impl Drop for LatencyMeasurement {
+    fn drop(&mut self) {
+        let duration = self.start.elapsed();
+        self.tracker.record(duration);
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_latency_tracker() {
+        let tracker = LatencyTracker::new("test");
+
+        // Record some measurements
+        tracker.record(Duration::from_micros(100));
+        tracker.record(Duration::from_micros(200));
+        tracker.record(Duration::from_micros(300));
+
+        let stats = tracker.stats();
+        assert_eq!(stats.count, 3);
+        assert!(stats.min >= 100);
+        assert!(stats.max <= 300);
+        assert!(stats.mean > 0.0);
+    }
+
+    #[test]
+    fn test_latency_measurement() {
+        let tracker = LatencyTracker::new("measurement");
+
+        {
+            let _measurement = tracker.measure();
+            std::thread::sleep(Duration::from_micros(100));
+        }
+
+        let stats = tracker.stats();
+        assert_eq!(stats.count, 1);
+        assert!(stats.min >= 100);
+    }
+}

vendor/ruvector/crates/agentic-robotics-rt/src/lib.rs

@@ -0,0 +1,60 @@
+//! ROS3 Real-Time Execution
+//!
+//! Dual runtime architecture combining Tokio (soft RT) and RTIC (hard RT)
+
+pub mod executor;
+pub mod scheduler;
+pub mod latency;
+
+pub use executor::{ROS3Executor, Priority, Deadline};
+pub use scheduler::PriorityScheduler;
+pub use latency::LatencyTracker;
+
+
+/// Real-time task priority levels
+#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
+pub enum RTPriority {
+    /// Lowest priority (background tasks)
+    Background = 0,
+    /// Low priority
+    Low = 1,
+    /// Normal priority
+    Normal = 2,
+    /// High priority
+    High = 3,
+    /// Critical priority (hard real-time)
+    Critical = 4,
+}
+
+impl From<u8> for RTPriority {
+    fn from(value: u8) -> Self {
+        match value {
+            0 => RTPriority::Background,
+            1 => RTPriority::Low,
+            2 => RTPriority::Normal,
+            3 => RTPriority::High,
+            _ => RTPriority::Critical,
+        }
+    }
+}
+
+impl From<RTPriority> for u8 {
+    fn from(priority: RTPriority) -> Self {
+        priority as u8
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_priority_conversion() {
+        let priority = RTPriority::High;
+        let value: u8 = priority.into();
+        assert_eq!(value, 3);
+
+        let converted: RTPriority = value.into();
+        assert_eq!(converted, RTPriority::High);
+    }
+}

vendor/ruvector/crates/agentic-robotics-rt/src/scheduler.rs

@@ -0,0 +1,121 @@
+//! Priority-based task scheduler
+
+use crate::RTPriority;
+use std::collections::BinaryHeap;
+use std::cmp::Ordering;
+use std::time::{Duration, Instant};
+
+/// Scheduled task
+#[derive(Debug)]
+pub struct ScheduledTask {
+    pub priority: RTPriority,
+    pub deadline: Instant,
+    pub task_id: u64,
+}
+
+impl PartialEq for ScheduledTask {
+    fn eq(&self, other: &Self) -> bool {
+        self.priority == other.priority && self.deadline == other.deadline
+    }
+}
+
+impl Eq for ScheduledTask {}
+
+impl PartialOrd for ScheduledTask {
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+impl Ord for ScheduledTask {
+    fn cmp(&self, other: &Self) -> Ordering {
+        // Higher priority first, then earlier deadline
+        match self.priority.cmp(&other.priority) {
+            Ordering::Equal => other.deadline.cmp(&self.deadline),
+            ordering => ordering,
+        }
+    }
+}
+
+/// Priority scheduler
+pub struct PriorityScheduler {
+    queue: BinaryHeap<ScheduledTask>,
+    next_task_id: u64,
+}
+
+impl PriorityScheduler {
+    /// Create a new scheduler
+    pub fn new() -> Self {
+        Self {
+            queue: BinaryHeap::new(),
+            next_task_id: 0,
+        }
+    }
+
+    /// Schedule a task
+    pub fn schedule(&mut self, priority: RTPriority, deadline: Duration) -> u64 {
+        let task_id = self.next_task_id;
+        self.next_task_id += 1;
+
+        let task = ScheduledTask {
+            priority,
+            deadline: Instant::now() + deadline,
+            task_id,
+        };
+
+        self.queue.push(task);
+        task_id
+    }
+
+    /// Get the next task to execute
+    pub fn next_task(&mut self) -> Option<ScheduledTask> {
+        self.queue.pop()
+    }
+
+    /// Get the number of pending tasks
+    pub fn pending_tasks(&self) -> usize {
+        self.queue.len()
+    }
+
+    /// Clear all tasks
+    pub fn clear(&mut self) {
+        self.queue.clear();
+    }
+}
+
+impl Default for PriorityScheduler {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_scheduler() {
+        let mut scheduler = PriorityScheduler::new();
+
+        // Schedule tasks with different priorities
+        scheduler.schedule(RTPriority::Low, Duration::from_millis(100));
+        scheduler.schedule(RTPriority::High, Duration::from_millis(100));
+        scheduler.schedule(RTPriority::Critical, Duration::from_millis(100));
+
+        assert_eq!(scheduler.pending_tasks(), 3);
+
+        // Should get critical first
+        let task1 = scheduler.next_task().unwrap();
+        assert_eq!(task1.priority, RTPriority::Critical);
+
+        // Then high
+        let task2 = scheduler.next_task().unwrap();
+        assert_eq!(task2.priority, RTPriority::High);
+
+        // Then low
+        let task3 = scheduler.next_task().unwrap();
+        assert_eq!(task3.priority, RTPriority::Low);
+
+        assert_eq!(scheduler.pending_tasks(), 0);
+    }
+}