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

extern crate pretty_env_logger;

use std::collections::{HashSet,HashMap};
use std::marker::PhantomData;
use std::cmp::Ordering;

use rand::Rng;

use crate::states::States;
use crate::control::Control;
use crate::planner_param::{Param,ParamObstacles};

use crate::rrt::sst::Node;

#[derive(Default)]
pub struct NN_Naive<TS,TC,TObs> where TS: States, TC: Control, TObs: States {
    pub phantom_ts: PhantomData< TS >,
    pub phantom_tc: PhantomData< TC >,
    pub phantom_tobs: PhantomData< TObs >,
}

impl<TS,TC,TObs> NN_Naive<TS,TC,TObs> where TS: States, TC: Control, TObs: States {
    
    ///return the node index of the best node with respect to cost function within delta_v vicinity or else return the node index of the nearest node
    pub fn query_nearest_state_active( & self,
                                       sample_state: TS,
                                       ns: & Vec< Node<TS> >,
                                       nodes_active: & HashSet< usize >,
                                       param: & Param<TS,TC,TObs>,
                                       delta_v: f32 ) -> usize {

        //consider vicinity
        let node_best = nodes_active.iter()
            .map(|x| &ns[*x] )
            .filter(|x| (param.ss_metric)( x.state.clone(), sample_state.clone() ) < delta_v )
            .min_by(|a,b| a.cost.partial_cmp( &b.cost ).unwrap_or(Ordering::Equal) );

        match node_best {
            Some(x) => { //non-empty
                x.id
            },
            _ => { //else consider nearest one
                let node_nearest = nodes_active.iter()
                    .map(|x| &ns[*x] )
                    .min_by(|a,b| a.cost.partial_cmp( &b.cost ).unwrap_or(Ordering::Equal) ).expect("no nodes active");
                node_nearest.id
            },
        }
    }

    ///return witness index within delta_s of query_state, or else return None
    pub fn query_nearest_witness( & self,
                                  query_state: TS,
                                  witnesses: & Vec< TS >,
                                  param: & Param<TS,TC,TObs>,
                                  delta_s: f32 ) -> Option<usize> {

        use std::cmp::Ordering::Equal;
        
        let witness_match = witnesses.iter().enumerate()
            .filter_map(|(idx,x)|
                        {
                            let dist = (param.ss_metric)( x.clone(), query_state.clone() );
                            if dist< delta_s {
                                Some( (idx,dist) )
                            } else {
                                None
                            }
                        } )
            .min_by(|a,b| a.1.partial_cmp(&b.1).unwrap_or(Equal) );
        match witness_match {
            Some(x) => {
                Some(x.0)
            },
            _ => {
                None
            },
        }
    }
}