rust_robotics 0.1.0

Umbrella crate for the RustRobotics workspace - feature-gated re-exports of all domain crates
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

//! A* Path Planning Example
//!
//! Demonstrates the A* path planning algorithm on a simple grid with obstacles.

use rust_robotics::planning::a_star::{AStarConfig, AStarPlanner};
use rust_robotics::prelude::*;
use rust_robotics::viz::{PathStyle, Visualizer};

fn main() {
    println!("A* path planning start!!");

    // Parameters
    let start = Point2D::new(2.0, 2.0);
    let goal = Point2D::new(8.0, 8.0);
    let grid_size = 1.0;
    let robot_radius = 0.5;

    println!("Setting up environment...");

    // Create boundary obstacles
    let mut ox = Vec::new();
    let mut oy = Vec::new();

    for i in 0..11 {
        ox.push(i as f64);
        oy.push(0.0);
        ox.push(i as f64);
        oy.push(10.0);
        ox.push(0.0);
        oy.push(i as f64);
        ox.push(10.0);
        oy.push(i as f64);
    }

    // Add internal obstacle
    for i in 4..7 {
        ox.push(5.0);
        oy.push(i as f64);
    }

    println!("Created {} obstacles", ox.len());

    // Create planner
    let config = AStarConfig {
        resolution: grid_size,
        robot_radius,
        heuristic_weight: 1.0,
    };
    let planner = AStarPlanner::new(&ox, &oy, config);

    // Plan path
    match planner.plan(start, goal) {
        Ok(path) => {
            println!("Path found with {} points", path.len());

            // Visualize
            let mut vis = Visualizer::new();
            vis.set_title("A* Path Planning");
            vis.plot_obstacles_xy(&ox, &oy);
            vis.plot_start(start);
            vis.plot_goal(goal);
            vis.plot_path(&path, &PathStyle::default());

            // Save and show
            let _ = vis.save_png("img/path_planning/a_star_result.png", 800, 600);
            println!("Plot saved to: img/path_planning/a_star_result.png");
            let _ = vis.show();
        }
        Err(e) => {
            println!("Planning failed: {}", e);
        }
    }

    println!("A* path planning finish!!");
}