基于C++的RRT代码学习_rrt算法c++代码

本文的代码来源于github作者kushuaiming，文章只是从一个初学者的角度，介绍一下拿到代码之后如何编译、理解，以及添加一些对初学者比较友好的注释。

代码编译

平台：Ubuntu16.04
依赖：cmake/g++、opencv
1
2

如果编译过程中报错可能就是某个依赖没装，网上搜一下报错原因基本就会有解决方案。

方法一：cmake编译
1.下载好代码之后，在代码所在路径下新建一个CMakeLists.txt文件;

2.在CMakeLists.txt文件中写入下面的代码;

cmake_minimum_required(VERSION 2.8.3)
project(rrt)

find_package(OpenCV REQUIRED)

include_directories(include
   ${catkin_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
)

set(SRC_LIST main.cpp rrt.h rrt.cpp)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

add_executable(past ${SRC_LIST})

target_link_libraries(past ${catkin_LIBRARIES} ${OpenCV_LIBS})
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

3.在当前路径下打开终端，输入cmake .(这个地方是一个点，表示内部编译);

cmake .
1

4.输入make;

make
1

5.输入./past运行编译好的可执行文件;

./past
1

方法二：g++编译
1.在代码所在路径打开终端，输入下面的代码后回车;

g++ main.cpp rrt.cpp -std=c++11 `pkg-config --cflags --libs opencv` -o test
1

2.编译好之后运行;

./test
1

rrt.h代码

#ifndef _RRT_H
#define _RRT_H

#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <opencv2/opencv.hpp>
#include <unistd.h>
#include <typeinfo>
#include <time.h>

using namespace std;
using namespace cv;

class node {
private:
    float x, y;                // 节点坐标
    vector<float> pathX, pathY;// 路径
    node* parent;              // 父节点
    float cost;
public:
    node(float _x, float _y);
    float getX();
    float getY();
    void setParent(node*);
    node* getParent();
};

class RRT {
private:
    node* startNode, * goalNode;          // 起始节点和目标节点
    vector< vector<float> > obstacleList; // 障碍物
    vector<node*> nodeList;               // 
    float stepSize;                       // 步长

    int goal_sample_rate;

    // 随机函数产生的是一种伪随机数，它实际是一种序列发生器，有固定的算法，只有当种子不同时，序列才不同，
    // 所以不应该把种子固定在程序中，应该用随机产生的数做种子，如程序运行时的时间等。
    random_device goal_rd;                // random_device可以生成用来作为种子的随机的无符号整数值。
    mt19937 goal_gen;                     // mt19937是一种高效的随机数生成算法
    uniform_int_distribution<int> goal_dis;  //随机数源，随机数源调用随机数算法来生成随机数

    random_device area_rd;
    mt19937 area_gen;
    uniform_real_distribution<float> area_dis;
public:
    RRT(node*, node*, const vector<vector<float>>&, float , int);
    node* getNearestNode(const vector<float>&);
    bool collisionCheck(node*);
    vector<node*> planning();
};

#endif
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

rrt.cpp代码

#include "rrt.h"

// node构造函数，初始化x,y,parent,cost
node::node(float _x, float _y) : x(_x), y(_y), parent(nullptr), cost(0) {}

float node::getX() { return x; }
float node::getY() { return y; }

void node::setParent(node* _parent) { parent = _parent; }
node* node::getParent() { return parent; }

// RRT类构造函数
// 单冒号(:)的作用是表示后面是初始化列表,可以参考https://blog.csdn.net/lusirking/article/details/83988421
RRT::RRT(node* _startNode, node* _goalNode, const vector<vector<float>>& _obstacleList,
         float _stepSize = 1.0, int _goal_sample_rate = 5)
    : startNode(_startNode), goalNode(_goalNode),
      obstacleList(_obstacleList),
      stepSize(_stepSize), goal_sample_rate(_goal_sample_rate),
      goal_gen(goal_rd()), goal_dis(uniform_int_distribution<int>(0, 100)),
      area_gen(area_rd()), area_dis(uniform_real_distribution<float>(0, 15))
      {}

node* RRT::getNearestNode(const vector<float>& randomPosition)
{
    int minID = -1;
    float minDistance = numeric_limits<float>::max(); // 编译器允许的float类型的最大值

    cout << nodeList.size() << endl;

    // 找到和随机位置距离最小的节点,通过遍历所有点
    for (int i = 0; i < nodeList.size(); i++)
    {
        // 在这里距离不需要开根号
        float distance = pow(nodeList[i]->getX() - randomPosition[0], 2) + pow(nodeList[i]->getY() - randomPosition[1], 2);
        // 一开始采用 编译器允许的float类型的最大值 作为最小距离，保证第一次比较时distance一定小于minDistance
        if (distance < minDistance)
        {
            minDistance = distance;    // 更新最小距离，这里的距离应该也可以采用曼哈顿距离或者其他条件判断
            minID = i;                 // 通过minID去记录下distance最小时对应的节点ID
        }
    }

    // 返回找到的距离randomPosition最近的点
    return nodeList[minID];
}


// 检测new节点到父节点的连线是否collision free
bool RRT::collisionCheck(node* newNode) {
    for (auto item : obstacleList)
        // 判断new节点到障碍物节点的欧式距离是否小于障碍物的半径
        if (sqrt(pow((item[0] - newNode->getX()), 2) + std::pow((item[1] - newNode->getY()), 2)) <= item[2])
            return false;
    return true;
}

vector<node*> RRT::planning() {
    // 建立一个窗口,WINDOW_NORMAL表示可以调整窗口大小
    namedWindow("RRT", WINDOW_NORMAL);

    int count = 0;

    // 建立背景
    const int imageSize = 15;
    const int imageResolution = 100;
    // Mat参考：https://blog.csdn.net/u012058778/article/details/90764430
    Mat background(imageSize * imageResolution, imageSize * imageResolution,
        CV_8UC3, cv::Scalar(255, 255, 255)); // CV_8UC3：CV_[位数][是否带符号]C[通道数]

    // 画出起始位置和目标位置
    // circle参考：https://yongqiang.blog.csdn.net/article/details/104562114
    circle(background, Point(startNode->getX() * imageResolution, startNode->getY() * imageResolution), 20, Scalar(0, 0, 255), -1);
    circle(background, Point(goalNode->getX() * imageResolution, goalNode->getY() * imageResolution), 20, Scalar(255, 0, 0), -1);
    // 画出障碍物
    for (auto item : obstacleList)
        circle(background, Point(item[0] * imageResolution, item[1] * imageResolution), item[2] * imageResolution, Scalar(0, 0, 0), -1);

    // RRT
    nodeList.push_back(startNode); // 每次开始都首先在节点列表中添加起点节点
    while(1)
    {
        // 生成一个随机位置(这个随机位置不是直接作为新节点去使用的，只是树的生长方向)
        vector<float> randomPosition;
        // if (goal_dis(goal_gen) > goal_sample_rate)   // 这里可以优化成直接用节点来表示
        if (goal_dis(goal_gen) > goal_sample_rate)   // 这里可以优化成直接用节点来表示
        {
            float randX = area_dis(goal_gen);        // 在(0,15)之间随机产生一个值作为x坐标
            float randY = area_dis(goal_gen);        // 在(0,15)之间随机产生一个值作为y坐标
            randomPosition.push_back(randX);
            randomPosition.push_back(randY);
        }
        else { // 找到了目标,将目标位置保存
            randomPosition.push_back(goalNode->getX());
            randomPosition.push_back(goalNode->getY());
        }

        // 找到和新生成随机节点距离最近的节点
        node* nearestNode = getNearestNode(randomPosition);
        // 利用反正切计算角度,然后利用角度和步长计算新坐标
        float theta = atan2(randomPosition[1] - nearestNode->getY(), randomPosition[0] - nearestNode->getX());
        // 利用之前采样的位置，加上设定的步长，来得到一个new节点
        node* newNode = new node(nearestNode->getX() + stepSize * cos(theta), nearestNode->getY() + stepSize * sin(theta));
        newNode->setParent(nearestNode);

        if (!collisionCheck(newNode)) continue;
        nodeList.push_back(newNode);

        // 画出路径
        line(background,
            Point(static_cast<int>(newNode->getX() * imageResolution), static_cast<int>(newNode->getY() * imageResolution)),
            Point(static_cast<int>(nearestNode->getX() * imageResolution), static_cast<int>(nearestNode->getY() * imageResolution)),
            Scalar(0, 255, 0),10);

        count++;
        imshow("RRT", background);
        waitKey(5);

        if (sqrt(pow(newNode->getX() - goalNode->getX(), 2) + pow(newNode->getY() - goalNode->getY(), 2)) <= stepSize)
        {
            cout << "The path has been found!" << endl;
            break;
        }
    }

    // 画出最终得到的路径
    vector<node*> path;
    path.push_back(goalNode);
    node* tmpNode = nodeList.back(); //返回节点列表的最后一个元素
    while (tmpNode->getParent() != nullptr)
    {
        line(background,
            Point(static_cast<int>(tmpNode->getX() * imageResolution), static_cast<int>(tmpNode->getY() * imageResolution)),
            Point(static_cast<int>(tmpNode->getParent()->getX() * imageResolution), static_cast<int>(tmpNode->getParent()->getY() * imageResolution)),
            Scalar(255, 0, 255), 10);
        path.push_back(tmpNode);
        tmpNode = tmpNode->getParent();
    }

    // 展示背景
    imshow("RRT", background);
    waitKey(0);
    path.push_back(startNode);
    return path;
}
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
132
133
134
135
136
137
138
139
140
141
142
143
144

main.cpp代码

#include <iostream>
#include <vector>
#include "rrt.h"
using namespace std;

int main(int argc, char* argv[]) 
{
    // 障碍物,前两个数表示圆心坐标,最后一个数表示半径
    vector<vector<float>> obstacleList{
        {7, 1, 1},
        {5, 6, 2},
        {5, 15, 2},
        {5, 10, 2},
        {9, 5, 2},
        {11, 5, 2}
    };

    // 起始点和目标点
    node* startNode = new node(2.0, 2.0);
    node* goalNode = new node(14.0, 9.0);

    RRT rrt(startNode, goalNode, obstacleList, 0.5, 5);
    rrt.planning();
    return 0;
}
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

参考引用

代码参考：github作者kushuaiming：https://github.com/kushuaiming/PathPalnning