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
#include <CImg.h>

struct Node {
  // X,Y coordinates of node in occupancy grid.
  int x;
  int y;
  int parent_id;
  int id;

  Node() = delete;  //prohibit default constructor
  // Initialization constructor.
  // start node has id 0 and parent -1
  // end node has id 1 and parent initially is -2 
  Node(const int& x,
       const int& y):
       x(x),
       y(y) {};

  Node(const int& x,
       const int& y,
       const int& parent_id,
       const int& id):
       x(x),
       y(y),
       parent_id(parent_id),
       id(id) {};

  void SetParentId(const int& pid);
  void SetID(const int& sid);

};

class RRT {
public:
  RRT() = delete;
  // Initialization constructor.
  RRT(const cimg_library::CImg<unsigned char>& image);
  Node NearestNeighbor(std::vector<Node> node_vector,
                        Node input_node);
  bool CheckCollision(Node start, Node end);
  std::vector<Node> FindPath(Node start, Node end,
                        int goal_radius, int extension_distance);
  void DrawNodePath();
  void Sample();
  float Distance(Node n1, Node n2);
  void Project(int sample_x, int sample_y, Node nearest_node, int distance);
  void Link_Path(Node& new_node, Node& nearest_node);
  int sign(int& number);
  void PrintNodeVector(const std::vector<Node> node_vector);
  int GenerateNodeID();
  bool CheckNodeExist(Node new_node);
private:
  cimg_library::CImg<unsigned char> image_;
  std::vector<Node> node_vector_;
  int sample_x_;
  int sample_y_;
  bool path_found_;

};