436 lines
14 KiB
Rust
436 lines
14 KiB
Rust
//! 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());
|
|
}
|
|
}
|