写在前面

这篇文章是rrt算法系列的最后一篇文章啦，那么下面一个系列呢，我计划做c++相关语法的学习博客。

为什么要做这么一个c++系列的文章呢？因为做自动驾驶的路径规划的话，实现是必不可少的，那么为了保证算法实现的时间不会太长，高效率的c++肯定是一个不错的选项。

rrt算法效果

file：rrt.cpp

brief：rrt算法实现的主体

author：AIplusX

param：成员变量

return：无

exception：无

note：本系列的前面2篇文章已经进行了详细的介绍，在此不再赘述，请移步浏览

remarks：无

void RRT::rrt_algorithm()
{
    srand((unsigned)time(NULL));
    setlinecolor(GREEN);
    setlinestyle(PS_SOLID, rrt_user_para.rrt_line_width);

    int min_dist_random_idx = 0;
    Point* search_p = new Point(start->x,start->y);
    Point* search_next_p = nullptr;
    rrt_path.push_back(search_p);

    //testAtan2Radian();


    while (1) {
        //showParaBelow(end);
        delayMs(1);
        if (getRandomFloat(0, 1) > rrt_user_para.beta) {
            search_p = findMinDistPoint(end);
            search_next_p = getRRTNode(search_p);
        }
        else {
            search_next_p = getRandomRRTNode(min_dist_random_idx);
            search_p = rrt_path[min_dist_random_idx];
        }


        if (!isObstacle(search_next_p)) {
            search_next_p->pre_point = search_p;

            rrt_path.push_back(search_next_p);

            line(search_p->x, search_p->y, search_next_p->x, search_next_p->y);
            solidcircle(search_next_p->x, search_next_p->y, rrt_user_para.rrt_line_width);

        }
        else{
            delete search_next_p;
        }

        search_p = findMinDistPoint(end);
        if (calcuDist(search_p, end) < rrt_user_para.rrt_edge_len) { break; }
    }

    getPath(search_p);

}

file：rrt.cpp

brief：ms级别的延时函数

author：AIplusX

param：延时ms数

return：无

exception：无

note：无

remarks：无

void RRT::delayMs(const int& cnt)
{
    clock_t start = clock(); while ((clock() - start)<cnt);
}

file：rrt.cpp

brief：判断坐标是否超出了地图的范围

author：AIplusX

param：存储坐标的内存地址

return：布尔型变量

exception：无

note：合法点返回true，不合法点返回false

remarks：无

bool RRT::isLegalPoint(const Point* p)
{
    if (p->x >= 0 && p->x < rrt_user_para.rrt_graph_width && 
        p->y >= 0 && p->y < rrt_user_para.rrt_graph_height) {
        return true;
    }
    else {
        return false;
    }

    return false;
}

file：rrt.cpp

brief：生成[x,y]范围的浮点数

author：AIplusX

param：范围上界和范围下界

return：随机生成的浮点数

exception：无

note：无

remarks：无

float RRT::getRandomFloat(const float& x, const float& y)
{
    return ((x) + (1.0 * rand()) / RAND_MAX * ((y)-(x)));//[x, y]
}

file：rrt.cpp

brief：给定当前节点，往特定方向延伸特定长度

author：AIplusX

param：存储当前节点内存的地址

return：下一坐标节点内存地址

exception：无

note：要考虑新节点和当前节点x值相同的情况，如果x值相同的话直接变换y坐标即可

remarks：无

Point* RRT::getRRTNode(const Point* node)
{
    Point* p = new Point();

    if (node->x != end->x) {
        float theta = atan2((end->x - node->x), (end->y - node->y));
        getAtan2Node(p, node, theta);
    }
    else {
        p->x = node->x;
        p->y = node->y + rrt_user_para.rrt_edge_len;
    }

    return p;
}

file：rrt.cpp

brief：atan()函数输出的弧度值转换成图像坐标系下的弧度值

author：AIplusX

param：弧度值的引用

return：无

exception：无

note：直接进行弧度值的引用修改即可，转换方式就是进行画图，然后观察规律

remarks：无

void RRT::changeAngle(float& angle)
{
    if (angle < -PI / 2 && angle > -PI) { angle = -3 * PI / 2 - angle; }
    else { angle = PI / 2 - angle; }
}

file：rrt.cpp

brief：计算地图中两坐标点之间的欧几里得距离

author：AIplusX

param：两坐标点内存的地址

return：int类型的距离结果

exception：无

note：无

remarks：无

int RRT::calcuDist(const Point* p1, const Point* p2)
{
    int dx = abs(p1->x - p2->x);
    int dy = abs(p1->y - p2->y);
    return int(sqrt(dx * dx + dy * dy));
}

file：rrt.cpp

brief：从终点向前回溯获取可视化路径

author：AIplusX

param：探索路径节点中到终点距离小于阈值的坐标的内存的地址

return：无

exception：无

note：无

remarks：无

void RRT::getPath(const Point* p)
{
    setlinecolor(BROWN);
    setlinestyle(PS_SOLID, rrt_user_para.rrt_line_width*2);
    while (p->pre_point != nullptr) {
        line(p->x, p->y, p->pre_point->x, p->pre_point->y);
        p = p->pre_point;
    }
}

file：rrt.cpp

brief：分别进行相应参数的调试可视化界面

author：AIplusX

param：无

return：无

exception：无

note：这两个函数的实现原理以及效果已在之前文章中进行了讲解，在此不再赘述，请看官移步浏览

remarks：无

void RRT::testAtan2Radian()
{
    ExMessage mse;

    while (1) {
        mse = getmessage(EM_MOUSE);
        if (mse.message == WM_LBUTTONDOWN) {
            line(480, 240, mse.x, mse.y);
            float theta = atan2((mse.x - 480), (mse.y - 240));
            changeAngle(theta);
            TCHAR s1[5];
            _stprintf_s(s1, sizeof(s1), _T("%.4f"), theta);
            outtextxy(mse.x, mse.y, s1);
        }
    }
}

void RRT::showParaBelow(const Point* search_end)
{
    ExMessage mse;

    while (1) {
        mse = getmessage(EM_MOUSE);

        wchar_t s1[7];
        _stprintf_s(s1, sizeof(s1), _T("(%d,"), mse.x);
        outtextxy(100, 500, s1);
        wchar_t s2[7];
        _stprintf_s(s2, sizeof(s2), _T("%d)"), mse.y);
        outtextxy(150, 500, s2);

        Point p(mse.x, mse.y);
        search_end = getRRTNode(&p);

        _stprintf_s(s1, sizeof(s1), _T("(%d,"), search_end->x);
        outtextxy(300, 500, s1);
        _stprintf_s(s2, sizeof(s2), _T("%d)"), search_end->y);
        outtextxy(350, 500, s2);

        if (mse.message == WM_LBUTTONDOWN)
            line(p.x, p.y, search_end->x, search_end->y);
    }
}