-
Notifications
You must be signed in to change notification settings - Fork 0
/
RRTStar.h
96 lines (80 loc) · 2.6 KB
/
RRTStar.h
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
// #ifndef RRT_H
// #define RRT_H
// #include <vector>
// #include "Node.h"
// #include <chrono>
// #include "support.h"
// #endif
#ifndef RRTSTAR_PLANNER_H
#define RRTSTAR_PLANNER_H
#include <vector>
#include "Node.h"
#include <chrono>
#include "support.h"
using namespace std;
#define PI 3.141592654
typedef std::chrono::high_resolution_clock myclock;
class RRTStar_Planner
{
public:
vector<Node*> RRT_Graph;
double* map;
int x_size;
int y_size;
double* armstart_anglesV_rad;
double* armgoal_anglesV_rad;
int numdof;
double**** plan;
int** planlength;
int maxSamples;
double epsilon;
double f_interpolate;
int goal_index;
//For testing
// vector<double> d;
// double cost;
RRTStar_Planner(double* map, int x_size, int y_size, double* armstart_anglesV_rad, double* armgoal_anglesV_rad, int numdof, double**** plan, int** planlength, int maxSamples, double epsilon, double f_interpolate)
{
this->map = map;
this->x_size = x_size;
this->y_size = y_size;
this->armstart_anglesV_rad = armstart_anglesV_rad;
this->armgoal_anglesV_rad = armgoal_anglesV_rad;
this->numdof = numdof;
this->plan = plan;
this->planlength = planlength;
this->maxSamples = maxSamples;
this->epsilon = epsilon;
this->f_interpolate = f_interpolate;
goal_index = 0; //CHECK IF ANY ERROR
vector<double> start;
for(int i=0; i<numdof; i++){
start.push_back(armstart_anglesV_rad[i]);
}
Node* root = new Node(start, numdof);
//root->cost = 0;
RRT_Graph.push_back(root);
goalSampleGenerator = std::uniform_real_distribution<double>(0.0, 1.0);
randomAngleGenerator = std::uniform_real_distribution<double>(0.0, 2 * PI);
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
gen.seed(seed);
}
void run();
//void extend(Node* qrand);
//void add_vertex(Node* qnear,Node* qnew);
vector<Node*> neighbours(Node* qadd);
void rewire(Node* qend);
bool isObstacleFree(Node* q1, Node* q2);
vector<double> generateRandom();
Node* nearestNeighbour(Node* qrand);
Node* addVertex(Node* qnear, Node* qrand);
vector<double*> getPath();
double distance(Node* temp, Node* qrand);
bool newConfig(Node* qnear, Node* qrand);
bool isGoal(Node* qrand);
double getCost();
std::mt19937 gen;
std::uniform_real_distribution<double> goalSampleGenerator;
std::uniform_real_distribution<double> randomAngleGenerator;
};
#endif //RRT_H