d803bfe2b1
git-subtree-dir: vendor/ruvector git-subtree-split: b64c21726f2bb37286d9ee36a7869fef60cc6900
539 lines
19 KiB
Rust
539 lines
19 KiB
Rust
//! Integration tests for the ruvector-robotics crate.
|
|
//!
|
|
//! Tests the real crate APIs across bridge types, perception pipeline,
|
|
//! cognitive loop, MCP tool registry + executor, planning, sensor fusion,
|
|
//! Gaussian splatting, and swarm coordination.
|
|
//!
|
|
//! Run with: cargo test --test robotics_integration -p ruvector-robotics
|
|
|
|
use std::sync::{Arc, Mutex};
|
|
use std::time::Instant;
|
|
|
|
use ruvector_robotics::bridge::{
|
|
GaussianConfig, Point3D, PointCloud, SceneObject, SpatialIndex,
|
|
};
|
|
use ruvector_robotics::bridge::gaussian::gaussians_from_cloud;
|
|
use ruvector_robotics::cognitive::behavior_tree::{
|
|
BehaviorNode, BehaviorStatus, BehaviorTree, DecoratorType,
|
|
};
|
|
use ruvector_robotics::cognitive::{
|
|
ActionOption, DecisionConfig, DecisionEngine, Demonstration, EpisodicMemory,
|
|
Episode, SkillLibrary, SwarmConfig, SwarmCoordinator, SwarmTask, RobotCapabilities,
|
|
TrackedObject, WorldModel,
|
|
};
|
|
use ruvector_robotics::mcp::{RoboticsToolRegistry, ToolRequest};
|
|
use ruvector_robotics::mcp::executor::ToolExecutor;
|
|
use ruvector_robotics::perception::PerceptionPipeline;
|
|
use ruvector_robotics::perception::sensor_fusion::{fuse_clouds, FusionConfig};
|
|
use ruvector_robotics::planning;
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Helpers
|
|
// ---------------------------------------------------------------------------
|
|
|
|
fn pseudo_random_f32(seed: u64, index: usize) -> f32 {
|
|
let h = seed
|
|
.wrapping_mul(index as u64 + 1)
|
|
.wrapping_mul(0x5DEECE66D)
|
|
.wrapping_add(0xB);
|
|
((h % 10000) as f32) / 10000.0
|
|
}
|
|
|
|
fn generate_point_cloud_around(center: Point3D, n: usize, spread: f32) -> Vec<Point3D> {
|
|
(0..n)
|
|
.map(|i| {
|
|
Point3D::new(
|
|
center.x + (pseudo_random_f32(42, i * 3) - 0.5) * spread,
|
|
center.y + (pseudo_random_f32(42, i * 3 + 1) - 0.5) * spread,
|
|
center.z + (pseudo_random_f32(42, i * 3 + 2) - 0.5) * spread,
|
|
)
|
|
})
|
|
.collect()
|
|
}
|
|
|
|
// ===========================================================================
|
|
// TESTS
|
|
// ===========================================================================
|
|
|
|
/// Test 1: End-to-end perception pipeline using actual crate API.
|
|
#[test]
|
|
fn test_perception_pipeline_end_to_end() {
|
|
let mut points = generate_point_cloud_around(Point3D::new(2.0, 3.0, 0.0), 50, 0.8);
|
|
points.extend(generate_point_cloud_around(Point3D::new(8.0, 7.0, 0.0), 40, 0.6));
|
|
let cloud = PointCloud::new(points, 1_000_000);
|
|
|
|
let pipe = PerceptionPipeline::with_thresholds(0.5, 2.0);
|
|
|
|
// Detect obstacles using the real API
|
|
let obstacles = pipe.detect_obstacles(&cloud, [0.0, 0.0, 0.0], 20.0).unwrap();
|
|
assert!(!obstacles.is_empty(), "Should detect at least one obstacle");
|
|
|
|
// Build scene graph using the real API
|
|
let scene_objects: Vec<SceneObject> = obstacles
|
|
.iter()
|
|
.map(|obs| SceneObject::new(obs.id as usize, obs.position, [obs.radius; 3]))
|
|
.collect();
|
|
let graph = pipe.build_scene_graph(&scene_objects, 10.0).unwrap();
|
|
assert_eq!(graph.objects.len(), scene_objects.len());
|
|
if scene_objects.len() > 1 {
|
|
assert!(!graph.edges.is_empty(), "Should have edges between nearby objects");
|
|
}
|
|
|
|
// Predict trajectory using the real API
|
|
let traj = pipe
|
|
.predict_trajectory([0.0, 0.0, 0.0], [1.0, 0.5, 0.0], 10, 0.1)
|
|
.unwrap();
|
|
assert_eq!(traj.len(), 10);
|
|
assert!((traj.waypoints[0][0] - 0.1).abs() < 1e-9);
|
|
}
|
|
|
|
/// Test 2: Cognitive pipeline — decision engine selects action from obstacles.
|
|
#[test]
|
|
fn test_cognitive_perceive_think_act() {
|
|
let pipe = PerceptionPipeline::with_thresholds(0.5, 2.0);
|
|
|
|
let mut points = generate_point_cloud_around(Point3D::new(6.0, 5.0, 0.0), 20, 0.3);
|
|
points.extend(generate_point_cloud_around(Point3D::new(10.0, 10.0, 0.0), 15, 0.3));
|
|
let cloud = PointCloud::new(points, 0);
|
|
|
|
let obstacles = pipe.detect_obstacles(&cloud, [5.0, 5.0, 0.0], 20.0).unwrap();
|
|
let min_dist = obstacles.iter().map(|o| o.distance).fold(f64::MAX, f64::min);
|
|
|
|
// Use the real DecisionEngine
|
|
let engine = DecisionEngine::new(DecisionConfig::default());
|
|
let options = vec![
|
|
ActionOption { name: "proceed_fast".into(), reward: 8.0, risk: 0.9, energy_cost: 0.5, novelty: 0.0 },
|
|
ActionOption { name: "slow_down".into(), reward: 5.0, risk: 0.2, energy_cost: 0.3, novelty: 0.0 },
|
|
ActionOption { name: "stop".into(), reward: 2.0, risk: 0.0, energy_cost: 0.0, novelty: 0.0 },
|
|
];
|
|
let (best_idx, _utility) = engine.evaluate(&options).unwrap();
|
|
assert!(!options[best_idx].name.is_empty());
|
|
|
|
// With obstacles nearby, a conservative engine should not pick "proceed_fast"
|
|
if min_dist < 3.0 {
|
|
let conservative = DecisionEngine::new(DecisionConfig {
|
|
risk_aversion: 5.0,
|
|
..Default::default()
|
|
});
|
|
let (best_idx, _) = conservative.evaluate(&options).unwrap();
|
|
assert_ne!(options[best_idx].name, "proceed_fast");
|
|
}
|
|
}
|
|
|
|
/// Test 3: Episodic memory store and recall using actual crate API.
|
|
#[test]
|
|
fn test_episodic_memory_store_recall() {
|
|
let mut memory = EpisodicMemory::new();
|
|
|
|
for i in 0..20 {
|
|
let mut percept = vec![0.0f64; 32];
|
|
percept[i % 32] = 1.0;
|
|
percept[(i + 1) % 32] = 0.5;
|
|
memory.store(Episode {
|
|
percepts: vec![percept],
|
|
actions: vec![format!("action_{}", i)],
|
|
reward: i as f64 * 0.1,
|
|
timestamp: i as i64 * 1000,
|
|
});
|
|
}
|
|
|
|
let mut query = vec![0.0f64; 32];
|
|
query[5] = 0.9;
|
|
query[6] = 0.4;
|
|
let results = memory.recall_similar(&query, 3);
|
|
assert_eq!(results.len(), 3);
|
|
// Most similar episode should be the one with percept[5]=1.0
|
|
assert!(results[0].reward > 0.0);
|
|
}
|
|
|
|
/// Test 4: Behavior tree patrol execution.
|
|
#[test]
|
|
fn test_behavior_tree_patrol() {
|
|
let tree_node = BehaviorNode::Selector(vec![
|
|
BehaviorNode::Sequence(vec![
|
|
BehaviorNode::Condition("battery_ok".into()),
|
|
BehaviorNode::Action("patrol".into()),
|
|
]),
|
|
BehaviorNode::Action("return_to_base".into()),
|
|
]);
|
|
let mut tree = BehaviorTree::new(tree_node);
|
|
tree.set_condition("battery_ok", true);
|
|
tree.set_action_result("patrol", BehaviorStatus::Running);
|
|
tree.set_action_result("return_to_base", BehaviorStatus::Running);
|
|
|
|
for _ in 0..5 {
|
|
assert_eq!(tree.tick(), BehaviorStatus::Running);
|
|
}
|
|
tree.set_action_result("patrol", BehaviorStatus::Success);
|
|
assert_eq!(tree.tick(), BehaviorStatus::Success);
|
|
}
|
|
|
|
/// Test 5: Swarm task assignment using actual SwarmCoordinator.
|
|
#[test]
|
|
fn test_swarm_task_assignment() {
|
|
let mut coordinator = SwarmCoordinator::new(SwarmConfig::default());
|
|
|
|
for i in 0..5 {
|
|
coordinator.register_robot(RobotCapabilities {
|
|
id: i,
|
|
max_speed: 1.0 + i as f64 * 0.2,
|
|
payload: 10.0,
|
|
sensors: vec!["lidar".into()],
|
|
});
|
|
}
|
|
|
|
let tasks: Vec<SwarmTask> = (0..5)
|
|
.map(|i| SwarmTask {
|
|
id: i,
|
|
description: format!("task_{}", i),
|
|
location: [i as f64 * 2.0, 0.0, 0.0],
|
|
required_capabilities: vec!["lidar".into()],
|
|
priority: (i % 3) as u8,
|
|
})
|
|
.collect();
|
|
|
|
let assignments = coordinator.assign_tasks(&tasks);
|
|
assert!(!assignments.is_empty(), "Should assign at least one task");
|
|
}
|
|
|
|
/// Test 6: World model update and predict using actual WorldModel.
|
|
#[test]
|
|
fn test_world_model_update_predict() {
|
|
let mut model = WorldModel::new(50, 0.5);
|
|
|
|
for i in 0..10 {
|
|
model.update_object(TrackedObject {
|
|
id: i,
|
|
position: [i as f64 * 0.5, 0.0, 0.0],
|
|
velocity: [0.5, 0.0, 0.0],
|
|
last_seen: i as i64 * 100_000,
|
|
confidence: 0.9,
|
|
label: format!("obj_{}", i),
|
|
});
|
|
}
|
|
|
|
// Predict state of object 5 forward 2 seconds
|
|
let predicted = model.predict_state(5, 2.0).unwrap();
|
|
let expected_x = 5.0 * 0.5 + 0.5 * 2.0;
|
|
assert!((predicted.position[0] - expected_x).abs() < 1e-6);
|
|
|
|
// Stale removal
|
|
let removed = model.remove_stale_objects(1_000_000, 500_000);
|
|
assert!(removed > 0, "Should remove early objects");
|
|
}
|
|
|
|
/// Test 7: Skill learning using actual SkillLibrary.
|
|
#[test]
|
|
fn test_skill_learning_from_demo() {
|
|
let mut library = SkillLibrary::new();
|
|
|
|
let demos = vec![
|
|
Demonstration {
|
|
trajectory: vec![[0.0, 0.0, 0.0], [0.5, 0.0, 0.2], [1.0, 0.0, 0.5], [1.0, 0.0, 0.0]],
|
|
timestamps: vec![0, 100, 200, 300],
|
|
metadata: "demo1".into(),
|
|
},
|
|
Demonstration {
|
|
trajectory: vec![[0.0, 0.1, 0.0], [0.5, 0.1, 0.25], [1.0, 0.1, 0.55], [1.0, 0.1, 0.0]],
|
|
timestamps: vec![0, 100, 200, 300],
|
|
metadata: "demo2".into(),
|
|
},
|
|
];
|
|
|
|
let skill = library.learn_from_demonstration("pick_up", &demos);
|
|
assert_eq!(skill.trajectory.len(), 4);
|
|
assert!(skill.confidence > 0.0);
|
|
|
|
let avg_z1 = (0.2 + 0.25) / 2.0;
|
|
assert!((skill.trajectory[1][2] - avg_z1).abs() < 1e-6);
|
|
}
|
|
|
|
/// Test 8: Anomaly detection using actual PerceptionPipeline.
|
|
#[test]
|
|
fn test_anomaly_detection_accuracy() {
|
|
let pipe = PerceptionPipeline::with_thresholds(0.5, 2.0);
|
|
let mut pts: Vec<[f32; 3]> = (0..20).map(|i| [i as f32 * 0.1, 0.0, 0.0]).collect();
|
|
pts.push([100.0, 100.0, 100.0]); // outlier
|
|
let cloud = PointCloud::new(
|
|
pts.iter().map(|a| Point3D::new(a[0], a[1], a[2])).collect(),
|
|
0,
|
|
);
|
|
let anomalies = pipe.detect_anomalies(&cloud).unwrap();
|
|
assert!(!anomalies.is_empty());
|
|
assert!(anomalies.iter().any(|a| a.score > 2.0));
|
|
}
|
|
|
|
/// Test 9: Decision engine selects optimal action using actual DecisionEngine.
|
|
#[test]
|
|
fn test_decision_engine_selects_best() {
|
|
let engine = DecisionEngine::new(DecisionConfig {
|
|
risk_aversion: 1.0,
|
|
energy_weight: 0.0,
|
|
curiosity_weight: 0.0,
|
|
});
|
|
let options = vec![
|
|
ActionOption { name: "proceed_fast".into(), reward: 8.0, risk: 0.7, energy_cost: 0.5, novelty: 0.0 },
|
|
ActionOption { name: "detour".into(), reward: 7.0, risk: 0.3, energy_cost: 0.3, novelty: 0.0 },
|
|
ActionOption { name: "stop".into(), reward: 3.0, risk: 0.0, energy_cost: 0.0, novelty: 0.0 },
|
|
];
|
|
let (best_idx, _) = engine.evaluate(&options).unwrap();
|
|
// With risk_aversion=1, proceed_fast (reward=8 - risk*1=7.3) beats detour (7 - 0.3=6.7)
|
|
assert_eq!(options[best_idx].name, "proceed_fast");
|
|
|
|
// Conservative: higher risk_aversion
|
|
let conservative = DecisionEngine::new(DecisionConfig {
|
|
risk_aversion: 10.0,
|
|
energy_weight: 0.0,
|
|
curiosity_weight: 0.0,
|
|
});
|
|
let (best_idx, _) = conservative.evaluate(&options).unwrap();
|
|
// proceed_fast: 8 - 7.0 = 1.0, detour: 7 - 3.0 = 4.0, stop: 3 - 0 = 3.0
|
|
assert_eq!(options[best_idx].name, "detour");
|
|
}
|
|
|
|
/// Test 10: MCP tool listing -- verify all 15 tools are registered.
|
|
#[test]
|
|
fn test_mcp_tool_listing() {
|
|
let registry = RoboticsToolRegistry::new();
|
|
assert_eq!(registry.list_tools().len(), 15);
|
|
|
|
let expected = [
|
|
"detect_obstacles", "build_scene_graph", "predict_trajectory",
|
|
"focus_attention", "detect_anomalies", "spatial_search", "insert_points",
|
|
"store_memory", "recall_memory", "learn_skill", "execute_skill",
|
|
"plan_behavior", "coordinate_swarm", "update_world_model", "get_world_state",
|
|
];
|
|
for name in &expected {
|
|
assert!(registry.get_tool(name).is_some(), "Tool '{}' should be registered", name);
|
|
}
|
|
}
|
|
|
|
/// Test 11: MCP tool execution via ToolExecutor.
|
|
#[test]
|
|
fn test_mcp_tool_execution() {
|
|
let mut executor = ToolExecutor::new();
|
|
|
|
// Predict trajectory
|
|
let req = ToolRequest {
|
|
tool_name: "predict_trajectory".into(),
|
|
arguments: [
|
|
("position".into(), serde_json::json!([0.0, 0.0, 0.0])),
|
|
("velocity".into(), serde_json::json!([1.0, 0.0, 0.0])),
|
|
("steps".into(), serde_json::json!(5)),
|
|
("dt".into(), serde_json::json!(0.5)),
|
|
].into(),
|
|
};
|
|
let resp = executor.execute(&req);
|
|
assert!(resp.success, "predict_trajectory should succeed: {:?}", resp.error);
|
|
|
|
// Unknown tool
|
|
let req = ToolRequest { tool_name: "nonexistent".into(), arguments: Default::default() };
|
|
let resp = executor.execute(&req);
|
|
assert!(!resp.success, "nonexistent tool should fail");
|
|
}
|
|
|
|
/// Test 12: Gaussian splatting from point cloud.
|
|
#[test]
|
|
fn test_gaussian_splatting() {
|
|
let mut points = generate_point_cloud_around(Point3D::new(2.0, 0.0, 0.0), 30, 0.5);
|
|
points.extend(generate_point_cloud_around(Point3D::new(8.0, 0.0, 0.0), 30, 0.5));
|
|
let cloud = PointCloud::new(points, 1000);
|
|
|
|
let gaussians = gaussians_from_cloud(&cloud, &GaussianConfig::default());
|
|
assert!(gaussians.len() >= 2, "Should produce at least 2 Gaussians, got {}", gaussians.len());
|
|
|
|
for g in &gaussians.gaussians {
|
|
assert!(g.point_count >= 2);
|
|
assert!(g.opacity > 0.0);
|
|
assert!(g.scale[0] > 0.0);
|
|
}
|
|
|
|
// Verify JSON export
|
|
let json = ruvector_robotics::bridge::gaussian::to_viewer_json(&gaussians);
|
|
assert_eq!(json["count"].as_u64().unwrap(), gaussians.len() as u64);
|
|
}
|
|
|
|
/// Test 13: A* pathfinding.
|
|
#[test]
|
|
fn test_astar_planning() {
|
|
let mut grid = ruvector_robotics::bridge::OccupancyGrid::new(20, 20, 0.5);
|
|
|
|
// Add a wall
|
|
for y in 0..15 {
|
|
grid.set(10, y, 1.0);
|
|
}
|
|
|
|
let path = planning::astar(&grid, (5, 5), (15, 5)).unwrap();
|
|
assert_eq!(*path.cells.first().unwrap(), (5, 5));
|
|
assert_eq!(*path.cells.last().unwrap(), (15, 5));
|
|
assert!(path.cost > 10.0, "Path around wall should be longer than straight line");
|
|
|
|
// Verify no cell in path is occupied
|
|
for &(x, y) in &path.cells {
|
|
assert!(grid.get(x, y).unwrap() < 0.5, "Path cell ({},{}) is occupied", x, y);
|
|
}
|
|
}
|
|
|
|
/// Test 14: Potential field planner.
|
|
#[test]
|
|
fn test_potential_field_planning() {
|
|
let cmd = planning::potential_field(
|
|
&[0.0, 0.0, 0.0],
|
|
&[10.0, 0.0, 0.0],
|
|
&[[3.0, 0.0, 0.0]],
|
|
&planning::PotentialFieldConfig::default(),
|
|
);
|
|
// Should still move forward but with some deflection from obstacle
|
|
assert!(cmd.vx > 0.0, "Should move toward goal");
|
|
|
|
// No obstacles: straight toward goal
|
|
let cmd_free = planning::potential_field(
|
|
&[0.0, 0.0, 0.0],
|
|
&[10.0, 0.0, 0.0],
|
|
&[],
|
|
&planning::PotentialFieldConfig::default(),
|
|
);
|
|
assert!(cmd_free.vy.abs() < 1e-9);
|
|
}
|
|
|
|
/// Test 15: Sensor fusion.
|
|
#[test]
|
|
fn test_sensor_fusion() {
|
|
let c1 = PointCloud::new(
|
|
vec![Point3D::new(1.0, 0.0, 0.0), Point3D::new(2.0, 0.0, 0.0)],
|
|
1000,
|
|
);
|
|
let c2 = PointCloud::new(
|
|
vec![Point3D::new(3.0, 0.0, 0.0)],
|
|
1010,
|
|
);
|
|
let c3_stale = PointCloud::new(
|
|
vec![Point3D::new(99.0, 0.0, 0.0)],
|
|
200_000, // 199ms later — too stale
|
|
);
|
|
|
|
let config = FusionConfig { max_time_delta_us: 50_000, ..Default::default() };
|
|
let fused = fuse_clouds(&[c1, c2, c3_stale], &config);
|
|
assert_eq!(fused.len(), 3, "Should include c1+c2 but skip c3");
|
|
}
|
|
|
|
/// Test 16: Full pipeline stress test — 100 frames using real APIs.
|
|
#[test]
|
|
fn test_full_pipeline_100_frames() {
|
|
let start = Instant::now();
|
|
let pipe = PerceptionPipeline::with_thresholds(0.5, 2.0);
|
|
let mut total_obstacles = 0usize;
|
|
|
|
for frame in 0..100 {
|
|
let center = Point3D::new(
|
|
3.0 + frame as f32 * 0.1,
|
|
pseudo_random_f32(frame as u64, 0) * 4.0 - 2.0,
|
|
0.0,
|
|
);
|
|
let points = generate_point_cloud_around(center, 100, 1.0);
|
|
let cloud = PointCloud::new(points, frame as i64 * 33_333);
|
|
|
|
let obstacles = pipe
|
|
.detect_obstacles(&cloud, [frame as f64 * 0.1, 0.0, 0.0], 10.0)
|
|
.unwrap();
|
|
total_obstacles += obstacles.len();
|
|
}
|
|
|
|
let elapsed = start.elapsed();
|
|
assert!(total_obstacles > 0, "Should detect obstacles across 100 frames");
|
|
assert!(elapsed.as_secs() < 5, "100 frames should complete in < 5s, took {:?}", elapsed);
|
|
}
|
|
|
|
/// Test 17: Concurrent spatial search from multiple threads.
|
|
#[test]
|
|
fn test_concurrent_spatial_search() {
|
|
let points: Vec<Point3D> = (0..5000)
|
|
.map(|i| {
|
|
Point3D::new(
|
|
pseudo_random_f32(42, i * 3) * 10.0,
|
|
pseudo_random_f32(42, i * 3 + 1) * 10.0,
|
|
pseudo_random_f32(42, i * 3 + 2) * 10.0,
|
|
)
|
|
})
|
|
.collect();
|
|
|
|
let cloud = PointCloud::new(points, 0);
|
|
let mut index = SpatialIndex::new(3);
|
|
index.insert_point_cloud(&cloud);
|
|
let shared_index = Arc::new(index);
|
|
let results = Arc::new(Mutex::new(Vec::new()));
|
|
|
|
let mut handles = Vec::new();
|
|
for thread_id in 0..4 {
|
|
let idx = Arc::clone(&shared_index);
|
|
let res = Arc::clone(&results);
|
|
let handle = std::thread::spawn(move || {
|
|
let query = [(thread_id as f32) * 2.5, (thread_id as f32) * 2.5, 5.0_f32];
|
|
let neighbors = idx.search_nearest(&query, 5).unwrap();
|
|
res.lock().unwrap().push((thread_id, neighbors));
|
|
});
|
|
handles.push(handle);
|
|
}
|
|
|
|
for handle in handles {
|
|
handle.join().expect("Thread should not panic");
|
|
}
|
|
|
|
let final_results = results.lock().unwrap();
|
|
assert_eq!(final_results.len(), 4, "All 4 threads should complete");
|
|
for (tid, neighbors) in final_results.iter() {
|
|
assert_eq!(neighbors.len(), 5, "Thread {} should return 5 neighbors", tid);
|
|
for window in neighbors.windows(2) {
|
|
assert!(window[0].1 <= window[1].1, "Thread {} results should be distance-sorted", tid);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Test 18: Edge cases — empty inputs and boundary conditions.
|
|
#[test]
|
|
fn test_edge_cases() {
|
|
let pipe = PerceptionPipeline::with_thresholds(0.5, 2.0);
|
|
|
|
// Empty point cloud
|
|
let empty = PointCloud::default();
|
|
let obs = pipe.detect_obstacles(&empty, [0.0; 3], 10.0).unwrap();
|
|
assert!(obs.is_empty());
|
|
|
|
// Scene graph with invalid distance
|
|
assert!(pipe.build_scene_graph(&[], -1.0).is_err());
|
|
|
|
// Trajectory with zero steps
|
|
assert!(pipe.predict_trajectory([0.0; 3], [1.0, 0.0, 0.0], 0, 1.0).is_err());
|
|
|
|
// Attention with negative radius
|
|
assert!(pipe.focus_attention(&empty, [0.0; 3], -1.0).is_err());
|
|
|
|
// Anomaly with < 2 points
|
|
let small = PointCloud::new(vec![Point3D::new(1.0, 0.0, 0.0)], 0);
|
|
assert!(pipe.detect_anomalies(&small).unwrap().is_empty());
|
|
|
|
// Empty spatial index
|
|
let index = SpatialIndex::new(3);
|
|
assert!(index.search_nearest(&[0.0_f32, 0.0, 0.0], 5).is_err());
|
|
// Radius search on empty index returns Ok(empty)
|
|
assert!(index.search_radius(&[0.0_f32, 0.0, 0.0], 1.0).unwrap().is_empty());
|
|
|
|
// Behavior tree decorator
|
|
let node = BehaviorNode::Decorator(
|
|
DecoratorType::Inverter,
|
|
Box::new(BehaviorNode::Action("a".into())),
|
|
);
|
|
let mut tree = BehaviorTree::new(node);
|
|
tree.set_action_result("a", BehaviorStatus::Success);
|
|
assert_eq!(tree.tick(), BehaviorStatus::Failure);
|
|
|
|
// Empty Gaussian conversion
|
|
let gs = gaussians_from_cloud(&PointCloud::default(), &GaussianConfig::default());
|
|
assert!(gs.is_empty());
|
|
|
|
// A* on same start/goal
|
|
let grid = ruvector_robotics::bridge::OccupancyGrid::new(5, 5, 1.0);
|
|
let path = planning::astar(&grid, (2, 2), (2, 2)).unwrap();
|
|
assert_eq!(path.cells.len(), 1);
|
|
}
|