sample_planning 0.0.4

Sample Based Planner
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
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131

#[macro_use] extern crate log;

extern crate pretty_env_logger;

extern crate kiss3d;

extern crate nalgebra as na;

use kiss3d::window::Window;
use kiss3d::light::Light;

extern crate serde;
use serde::{Serialize,Deserialize};

use std::env;

#[derive(Serialize, Deserialize)]
struct ObsData(pub [f32; 4]);

use na::{Vector3, UnitQuaternion, Translation3, Point3, U3};

extern crate clap;
use clap::{Arg, App, SubCommand};

#[derive(Serialize, Deserialize)]
struct Obs {
    obs: Vec<ObsData>,
}

fn generate_obstacles( num_obs: u32 ) -> Vec<ObsData> {

    use rand::Rng;
    
    //generate boxes for now
    let boxes = {

        let mut rng = rand::thread_rng();
        
        (0..num_obs).map( |_| {

            let size = rng.gen_range(0.05, 0.075);
            let x = rng.gen_range(0.25, 0.75);
            let y = rng.gen_range(0.25, 0.75);

            let z = if cfg!(feature="gen_obs_3d"){
                rng.gen_range(0.25, 0.75)
            } else {
                0.
            };

            ObsData([x,y,z,size])
    
        } ).collect::<Vec<_>>()
    };
    
    boxes
}

fn main(){
    env::set_var("LOG_SETTING", "info" );
    pretty_env_logger::init_custom_env( "LOG_SETTING" );

    //command line ---
    let matches = App::new("gen_obs")
        .version("0.0")
        .author("Yuan Liu")
        .about("obstacle generator")
        .arg(Arg::with_name("num_obs")
             .short("n")
             .help("number of obstacles")
             .default_value("30")
             .takes_value(true))
        .arg(Arg::with_name("file")
             .short("f")
             .help("output file name")
             .required(true)
             .takes_value(true))
        .get_matches();

    let num_obs : u32 = matches
        .value_of( "num_obs" ).unwrap()
        .parse().expect("number of obstacles argument invalid" );

    let output_file : &str = matches
        .value_of( "file" ).unwrap();

    info!("number of obstacles: {}", num_obs );
    
    let obstacles = Obs{ obs: generate_obstacles( num_obs ) };

    let obs_data : Vec<_> = obstacles.obs.iter()
        .map(|x| {
            let size = x.0[3] as f32;
            ( ( size, size, 0.025),
                (x.0[0] as f32, x.0[1] as f32, x.0[2] as f32) ) } ).collect();

    let mut window = Window::new("Sample Planner");
    window.set_light(Light::StickToCamera);

    let mut g1 = window.add_group();
    g1.append_translation(&Translation3::new(0.5, 0.5, 0.0));

    let mut c = g1.add_cube(1.0, 1.0, 0.0001);
    c.set_color(0.3, 0.3, 0.3);
    c.set_lines_width(5.);
    c.set_points_size(5.0);
    c.set_surface_rendering_activation(false);

    while window.render() {
        obs_data.iter()
            .for_each(|x| {
                let a = x.0;
                let b = x.1;
                let mut c = window.add_cube( a.0, a.1, a.2 );
                c.append_translation( &Translation3::new( b.0, b.1, b.2 ) );
                c.set_color(0.8, 0.8, 0.);
            });
    }

    let serialized = serde_json::to_string( &obstacles ).unwrap();

    info!("serializing to file: {}", output_file );

    use std::fs::File;
    use std::io::Write;
    
    let mut f = File::create( output_file ).expect("file creation");
    f.write_all( serialized.as_bytes() );

    println!("serialized: {}", serialized );
}