Merge commit 'd803bfe2b1fe7f5e219e50ac20d6801a0a58ac75' as 'vendor/ruvector'

vendor/ruvector/crates/ruvector-robotics/tests/integration.rs

@@ -0,0 +1,435 @@
+//! Integration tests for the unified ruvector-robotics crate.
+//!
+//! Each test exercises a cross-module workflow to verify that the public API
+//! composes correctly.
+
+use ruvector_robotics::bridge::{
+    OccupancyGrid, Point3D, PointCloud, SceneEdge, SceneGraph, SceneObject, SpatialIndex,
+};
+use ruvector_robotics::cognitive::{
+    BehaviorNode, BehaviorStatus, BehaviorTree, CognitiveConfig, CognitiveCore, CognitiveMode,
+    Demonstration, EpisodicMemory, Episode, Formation, FormationType, MemoryItem, Outcome,
+    Percept, RobotCapabilities, SkillLibrary, SwarmConfig, SwarmCoordinator, SwarmTask,
+    TrackedObject, WorkingMemory, WorldModel,
+};
+use ruvector_robotics::mcp::{RoboticsToolRegistry, ToolCategory};
+use ruvector_robotics::perception::{PerceptionConfig, PerceptionPipeline};
+
+// ---------------------------------------------------------------------------
+// Helpers
+// ---------------------------------------------------------------------------
+
+fn make_cloud(pts: &[[f32; 3]], timestamp: i64) -> PointCloud {
+    let points = pts.iter().map(|p| Point3D::new(p[0], p[1], p[2])).collect();
+    PointCloud::new(points, timestamp)
+}
+
+fn cluster_pts(center: [f32; 3], n: usize, spread: f32) -> Vec<[f32; 3]> {
+    let mut pts = Vec::new();
+    for i in 0..n {
+        let f = i as f32 / n as f32;
+        pts.push([
+            center[0] + spread * (f * 6.28).cos(),
+            center[1] + spread * (f * 6.28).sin(),
+            center[2],
+        ]);
+    }
+    pts
+}
+
+// ---------------------------------------------------------------------------
+// 1. Bridge types roundtrip
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_bridge_types_roundtrip() {
+    // Point3D
+    let p = Point3D::new(1.0, 2.0, 3.0);
+    let json = serde_json::to_string(&p).unwrap();
+    let p2: Point3D = serde_json::from_str(&json).unwrap();
+    assert!((p.x - p2.x).abs() < f32::EPSILON);
+
+    // PointCloud
+    let cloud = PointCloud::new(vec![p, Point3D::new(4.0, 5.0, 6.0)], 999);
+    let json = serde_json::to_string(&cloud).unwrap();
+    let cloud2: PointCloud = serde_json::from_str(&json).unwrap();
+    assert_eq!(cloud2.len(), 2);
+    assert_eq!(cloud2.timestamp_us, 999);
+
+    // SceneObject
+    let obj = SceneObject::new(0, [1.0, 2.0, 3.0], [0.5, 0.5, 0.5]);
+    let json = serde_json::to_string(&obj).unwrap();
+    let obj2: SceneObject = serde_json::from_str(&json).unwrap();
+    assert_eq!(obj2.id, 0);
+
+    // SceneGraph
+    let graph = SceneGraph::new(
+        vec![obj.clone()],
+        vec![SceneEdge {
+            from: 0,
+            to: 0,
+            distance: 0.0,
+            relation: "self".into(),
+        }],
+        1000,
+    );
+    let json = serde_json::to_string(&graph).unwrap();
+    let graph2: SceneGraph = serde_json::from_str(&json).unwrap();
+    assert_eq!(graph2.objects.len(), 1);
+    assert_eq!(graph2.edges.len(), 1);
+
+    // OccupancyGrid
+    let mut grid = OccupancyGrid::new(5, 5, 0.5);
+    grid.set(2, 2, 0.9);
+    let json = serde_json::to_string(&grid).unwrap();
+    let grid2: OccupancyGrid = serde_json::from_str(&json).unwrap();
+    assert!((grid2.get(2, 2).unwrap() - 0.9).abs() < f32::EPSILON);
+}
+
+// ---------------------------------------------------------------------------
+// 2. Spatial index insert & search
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_spatial_index_insert_search() {
+    let mut index = SpatialIndex::new(3);
+    let n = 1000;
+    let vecs: Vec<Vec<f32>> = (0..n)
+        .map(|i| {
+            let f = i as f32;
+            vec![f * 0.01, f * 0.02, f * 0.03]
+        })
+        .collect();
+    index.insert_vectors(&vecs);
+    assert_eq!(index.len(), n);
+
+    // kNN: nearest to origin should be index 0
+    let results = index.search_nearest(&[0.0, 0.0, 0.0], 5).unwrap();
+    assert_eq!(results.len(), 5);
+    assert_eq!(results[0].0, 0);
+    assert!(results[0].1 < 0.001);
+
+    // Results sorted by distance
+    for w in results.windows(2) {
+        assert!(w[0].1 <= w[1].1);
+    }
+
+    // Radius search
+    let within = index.search_radius(&[0.0, 0.0, 0.0], 1.0).unwrap();
+    assert!(!within.is_empty());
+    for (_, d) in &within {
+        assert!(*d <= 1.0);
+    }
+}
+
+// ---------------------------------------------------------------------------
+// 3. Perception pipeline end-to-end
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_perception_pipeline_end_to_end() {
+    let mut pipeline = PerceptionPipeline::new(PerceptionConfig::default());
+
+    let mut pts = Vec::new();
+    pts.extend(cluster_pts([2.0, 0.0, 0.0], 10, 0.2));
+    pts.extend(cluster_pts([8.0, 5.0, 0.0], 10, 0.2));
+    let cloud = make_cloud(&pts, 500);
+
+    let (obstacles, graph) = pipeline.process(&cloud, &[0.0, 0.0, 0.0]);
+    assert!(!obstacles.is_empty());
+    assert!(!graph.objects.is_empty());
+    assert_eq!(pipeline.frames_processed(), 1);
+
+    // Classify
+    let classified = pipeline.classify(&obstacles);
+    assert_eq!(classified.len(), obstacles.len());
+
+    // Second frame increments counter
+    let _ = pipeline.process(&cloud, &[0.0, 0.0, 0.0]);
+    assert_eq!(pipeline.frames_processed(), 2);
+}
+
+// ---------------------------------------------------------------------------
+// 4. Cognitive loop
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_cognitive_loop() {
+    let mut core = CognitiveCore::new(CognitiveConfig {
+        mode: CognitiveMode::Reactive,
+        attention_threshold: 0.3,
+        learning_rate: 0.05,
+        max_percepts: 10,
+    });
+
+    // Perceive
+    core.perceive(Percept {
+        source: "lidar".into(),
+        data: vec![2.0, 1.0, 0.0],
+        confidence: 0.9,
+        timestamp: 100,
+    });
+    assert_eq!(core.percept_count(), 1);
+
+    // Think
+    let decision = core.think().expect("should produce a decision");
+    assert!(decision.utility > 0.0);
+
+    // Act
+    let cmd = core.act(decision);
+    assert!(cmd.confidence > 0.0);
+
+    // Learn
+    core.learn(Outcome {
+        success: true,
+        reward: 1.0,
+        description: "test".into(),
+    });
+    assert!(core.cumulative_reward() > 0.0);
+    assert_eq!(core.decision_count(), 1);
+}
+
+// ---------------------------------------------------------------------------
+// 5. Behavior tree sequence
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_behavior_tree_sequence() {
+    let seq = BehaviorNode::Sequence(vec![
+        BehaviorNode::Condition("battery_ok".into()),
+        BehaviorNode::Action("move".into()),
+        BehaviorNode::Action("report".into()),
+    ]);
+    let mut tree = BehaviorTree::new(seq);
+
+    // All success
+    tree.set_condition("battery_ok", true);
+    tree.set_action_result("move", BehaviorStatus::Success);
+    tree.set_action_result("report", BehaviorStatus::Success);
+    assert_eq!(tree.tick(), BehaviorStatus::Success);
+
+    // Condition fails -> Failure
+    tree.set_condition("battery_ok", false);
+    assert_eq!(tree.tick(), BehaviorStatus::Failure);
+
+    // Running propagates
+    tree.set_condition("battery_ok", true);
+    tree.set_action_result("move", BehaviorStatus::Running);
+    assert_eq!(tree.tick(), BehaviorStatus::Running);
+
+    assert_eq!(tree.context().tick_count, 3);
+}
+
+// ---------------------------------------------------------------------------
+// 6. Memory store & recall
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_memory_store_recall() {
+    // Working memory
+    let mut wm = WorkingMemory::new(3);
+    for i in 0..5 {
+        wm.add(MemoryItem {
+            key: format!("item_{}", i),
+            data: vec![i as f64],
+            importance: i as f64 * 0.2,
+            timestamp: i as i64 * 100,
+            access_count: 0,
+        });
+    }
+    assert_eq!(wm.len(), 3); // bounded
+
+    // Access increments count
+    let item = wm.get("item_4").expect("most important should survive");
+    assert_eq!(item.access_count, 1);
+
+    // Episodic memory
+    let mut em = EpisodicMemory::new();
+    em.store(Episode {
+        percepts: vec![vec![1.0, 0.0, 0.0]],
+        actions: vec!["move".into()],
+        reward: 1.0,
+        timestamp: 100,
+    });
+    em.store(Episode {
+        percepts: vec![vec![0.0, 1.0, 0.0]],
+        actions: vec!["turn".into()],
+        reward: 0.5,
+        timestamp: 200,
+    });
+    let recalled = em.recall_similar(&[1.0, 0.0, 0.0], 1);
+    assert_eq!(recalled.len(), 1);
+    assert_eq!(recalled[0].actions[0], "move");
+}
+
+// ---------------------------------------------------------------------------
+// 7. Skill learning cycle
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_skill_learning_cycle() {
+    let mut lib = SkillLibrary::new();
+
+    let demos = vec![
+        Demonstration {
+            trajectory: vec![[0.0, 0.0, 0.0], [1.0, 1.0, 0.0], [2.0, 2.0, 0.0]],
+            timestamps: vec![0, 100, 200],
+            metadata: "demo_1".into(),
+        },
+        Demonstration {
+            trajectory: vec![[0.0, 0.0, 0.0], [1.2, 0.8, 0.0], [2.1, 1.9, 0.0]],
+            timestamps: vec![0, 110, 210],
+            metadata: "demo_2".into(),
+        },
+    ];
+
+    // Learn
+    let skill = lib.learn_from_demonstration("reach", &demos);
+    assert_eq!(skill.trajectory.len(), 3);
+    assert!(skill.confidence > 0.0);
+
+    // Execute
+    let traj = lib.execute_skill("reach").unwrap();
+    assert_eq!(traj.len(), 3);
+    assert_eq!(lib.get("reach").unwrap().execution_count, 1);
+
+    // Improve
+    let before = lib.get("reach").unwrap().confidence;
+    lib.improve_skill("reach", 0.1);
+    let after = lib.get("reach").unwrap().confidence;
+    assert!(after > before);
+
+    // Missing skill
+    assert!(lib.execute_skill("nonexistent").is_none());
+}
+
+// ---------------------------------------------------------------------------
+// 8. Swarm task assignment
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_swarm_task_assignment() {
+    let mut coord = SwarmCoordinator::new(SwarmConfig::default());
+
+    for i in 0..4 {
+        coord.register_robot(RobotCapabilities {
+            id: i,
+            max_speed: 1.0 + i as f64 * 0.5,
+            payload: 5.0,
+            sensors: vec!["lidar".into(), "camera".into()],
+        });
+    }
+    assert_eq!(coord.robot_count(), 4);
+
+    let tasks = vec![
+        SwarmTask {
+            id: 10,
+            description: "scan".into(),
+            location: [3.0, 4.0, 0.0],
+            required_capabilities: vec!["lidar".into()],
+            priority: 8,
+        },
+        SwarmTask {
+            id: 11,
+            description: "photo".into(),
+            location: [5.0, 0.0, 0.0],
+            required_capabilities: vec!["camera".into()],
+            priority: 5,
+        },
+    ];
+
+    let assignments = coord.assign_tasks(&tasks);
+    assert_eq!(assignments.len(), 2);
+
+    // Formation
+    let formation = Formation {
+        formation_type: FormationType::Circle,
+        spacing: 2.0,
+        center: [0.0, 0.0, 0.0],
+    };
+    let positions = coord.compute_formation(&formation);
+    assert_eq!(positions.len(), 4);
+}
+
+// ---------------------------------------------------------------------------
+// 9. World model tracking
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_world_model_tracking() {
+    let mut world = WorldModel::new(20, 0.5);
+
+    // Update objects
+    world.update_object(TrackedObject {
+        id: 1,
+        position: [2.0, 3.0, 0.0],
+        velocity: [1.0, 0.0, 0.0],
+        last_seen: 1000,
+        confidence: 0.9,
+        label: "rover".into(),
+    });
+    world.update_object(TrackedObject {
+        id: 2,
+        position: [8.0, 1.0, 0.0],
+        velocity: [0.0, 0.5, 0.0],
+        last_seen: 500,
+        confidence: 0.7,
+        label: "box".into(),
+    });
+    assert_eq!(world.object_count(), 2);
+
+    // Predict
+    let pred = world.predict_state(1, 2.0).unwrap();
+    assert!((pred.position[0] - 4.0).abs() < 1e-6);
+    assert!(pred.confidence < 0.9); // decayed
+
+    // Missing object
+    assert!(world.predict_state(99, 1.0).is_none());
+
+    // Occupancy
+    world.update_occupancy(5, 5, 1.0);
+    assert!((world.get_occupancy(5, 5).unwrap() - 1.0).abs() < f32::EPSILON);
+
+    // Path clearance
+    assert!(world.is_path_clear([0, 0], [4, 4])); // no obstacle in path
+    assert!(!world.is_path_clear([0, 5], [19, 5])); // (5,5) is blocked
+
+    // Remove stale
+    let removed = world.remove_stale_objects(1200, 300);
+    assert_eq!(removed, 1); // id=2 is stale
+    assert!(world.get_object(2).is_none());
+    assert!(world.get_object(1).is_some());
+}
+
+// ---------------------------------------------------------------------------
+// 10. MCP registry
+// ---------------------------------------------------------------------------
+
+#[test]
+fn test_mcp_registry() {
+    let registry = RoboticsToolRegistry::new();
+
+    // Has built-in tools
+    assert!(registry.list_tools().len() >= 10);
+
+    // Look up by name
+    let tool = registry.get_tool("detect_obstacles").unwrap();
+    assert_eq!(tool.category, ToolCategory::Perception);
+    assert!(!tool.parameters.is_empty());
+
+    // Category filtering
+    let perception = registry.list_by_category(ToolCategory::Perception);
+    assert!(!perception.is_empty());
+    for t in &perception {
+        assert_eq!(t.category, ToolCategory::Perception);
+    }
+
+    // MCP schema
+    let schema = registry.to_mcp_schema();
+    let tools = schema["tools"].as_array().unwrap();
+    assert!(!tools.is_empty());
+    for tool_schema in tools {
+        assert!(tool_schema["name"].is_string());
+        assert!(tool_schema["inputSchema"].is_object());
+    }
+}