Mecha10 Planning

Path planning and navigation algorithms as BehaviorNode implementations for autonomous robots.

Overview

This package provides production-ready path planning algorithms that implement the BehaviorNode trait, making them fully composable with the Mecha10 behavior system.

Available Algorithms:

• A (A-Star)* - Optimal grid-based path planning
• RRT (Rapidly-exploring Random Tree) - Sampling-based planning for complex spaces

Installation

[dependencies]
mecha10-planning = "0.1.0"

Quick Start

A* Path Planning

A* is optimal for grid-based environments and guarantees the shortest path.

use mecha10_planning::prelude::*;

#[tokio::main]
async fn main() -> anyhow::Result<()> {
    let ctx = Context::new("robot").await?;

    // Create A* planner
    let mut planner = AStarPlanner::new(
        (0.0, 0.0),      // start
        (10.0, 10.0),    // goal
        0.5,             // resolution (grid cell size)
    );

    // Add obstacles
    planner.add_obstacle(Obstacle::new(Point2D::new(5.0, 5.0), 2.0));

    // Wrap in BehaviorNode
    let mut node = PathPlannerNode::new(Box::new(planner));

    // Execute planning
    let status = node.tick(&ctx).await?;

    if status == NodeStatus::Success {
        if let Some(path) = node.path() {
            println!("Found path with {} waypoints", path.len());
            println!("Path length: {:.2} meters", path.length);
        }
    }

    Ok(())
}

###RRT Path Planning

RRT is better for high-dimensional or complex spaces where grid-based methods struggle.

use mecha10_planning::prelude::*;

// Create RRT planner
let mut planner = RRTPlanner::new(
    (0.0, 0.0),      // start
    (50.0, 50.0),    // goal
    2.0,             // step size
)
.with_max_iterations(5000)
.with_goal_sample_rate(0.1)  // 10% chance to sample goal
.with_bounds(
    Point2D::new(-10.0, -10.0),
    Point2D::new(60.0, 60.0),
);

// Add obstacles
planner.add_obstacle(Obstacle::new(Point2D::new(25.0, 25.0), 5.0));

// Execute planning
let path = planner.plan();

Algorithms

A* Algorithm

Best for:

• Grid-based environments
• 2D navigation
• When optimality is required

Features:

• Guaranteed shortest path
• Configurable diagonal movement
• Efficient with good heuristics

Parameters:

• resolution: Grid cell size (smaller = more precise, slower)
• diagonal_movement: Allow diagonal moves (default: true)

Example:

let planner = AStarPlanner::new((0.0, 0.0), (10.0, 10.0), 0.5)
    .with_diagonal_movement(false);  // Manhattan distance only

RRT Algorithm

Best for:

• Complex/cluttered environments
• High-dimensional spaces
• When speed > optimality

Features:

• Sampling-based exploration
• Handles complex obstacles
• Probabilistically complete

Parameters:

• step_size: Maximum extension distance per iteration
• max_iterations: Maximum planning iterations (default: 1000)
• goal_sample_rate: Probability of sampling goal (default: 0.1)
• bounds: Search space boundaries

Example:

let planner = RRTPlanner::new((0.0, 0.0), (50.0, 50.0), 2.0)
    .with_max_iterations(10000)
    .with_goal_sample_rate(0.2);  // Sample goal 20% of the time

Path Representation

Paths are represented as sequences of waypoints:

pub struct Path {
    pub waypoints: Vec<Point2D>,
    pub length: f32,
}

impl Path {
    // Check if path is empty
    pub fn is_empty(&self) -> bool;

    // Get number of waypoints
    pub fn len(&self) -> usize;

    // Simplify path (Douglas-Peucker algorithm)
    pub fn simplify(&mut self, tolerance: f32);
}

Path Simplification

Remove redundant waypoints to reduce path complexity:

let mut path = planner.plan().unwrap();
println!("Original: {} waypoints", path.len());

path.simplify(0.5);  // Remove points within 0.5m tolerance
println!("Simplified: {} waypoints", path.len());

Obstacles

Circular obstacles are supported:

// Create obstacle at (x, y) with radius
let obstacle = Obstacle::new(Point2D::new(5.0, 5.0), 2.0);

// Check if point is inside
if obstacle.contains(&Point2D::new(5.5, 5.0)) {
    println!("Point is in obstacle!");
}

// Check if line segment intersects
if obstacle.intersects_segment(&start, &end) {
    println!("Path blocked!");
}

Integration with Behavior System

Use planning as part of larger behavior compositions:

use mecha10_behavior_patterns::prelude::*;

// Combine planning with navigation
let sequence = SequenceNode::new(vec![
    Box::new(PathPlannerNode::new(Box::new(planner))),
    Box::new(PathFollowerNode::new(/* ... */)),
    Box::new(GoalReachedCheckNode::new(/* ... */)),
]);

Or use with subsumption for reactive planning:

let subsumption = SubsumptionNode::new()
    .add_layer(10, Box::new(EmergencyStopNode))
    .add_layer(5, Box::new(ReactiveAvoidanceNode))
    .add_layer(1, Box::new(PathPlannerNode::new(Box::new(planner))));

JSON Configuration

Path planning can be configured via JSON:

{
  "type": "path_planner",
  "algorithm": "astar",
  "start": [0.0, 0.0],
  "goal": [10.0, 10.0],
  "params": {
    "resolution": 0.5,
    "diagonal_movement": true
  }
}

Performance

A Performance:*

• Time Complexity: O(b^d) where b=branching factor, d=depth
• Space Complexity: O(b^d)
• Typical: ~100ms for 100x100 grid
• Optimizations: Good heuristics, pruning

RRT Performance:

• Time Complexity: O(n) where n=iterations
• Space Complexity: O(n)
• Typical: ~50-500ms depending on environment
• Probabilistically complete (not optimal)

Testing

# Run all tests
cargo test -p mecha10-planning

# Run specific algorithm tests
cargo test -p mecha10-planning astar
cargo test -p mecha10-planning rrt

# Run clippy
cargo clippy -p mecha10-planning -- -D warnings

Future Enhancements

Planned additions (see TODOS.md 7.3):

• TEB Local Planner: For dynamic obstacle avoidance
• D Lite*: For replanning in changing environments
• Hybrid A*: For non-holonomic robots (cars, etc.)
• 3D Planning: Extend to 3D spaces (drones)

See Also

• mecha10-behavior-runtime - Core behavior system
• mecha10-behavior-patterns - Subsumption, Ensemble
• TODOS.md - Priority 7.3: Non-AI Behavior Nodes

License

MIT