file: rrt.h

brief:这个rrt.h文件是rrt算法实现类的声明

author:AIplusX

version:beta_v0.0

date:2021_12_02

update:2021_12_02

warning:记得在类的析构函数里面delete掉堆内申请的内存

remarks:因为在做这个类的时候没有考虑虚函数的问题,所以析构函数没有做成virtual的,所以在做子类的时候要将其析构函数设计成virtual的

todo:无

#pragma once
#include <cmath>
#include<queue>
#include "rrt_graph.h"

#define PI acos(-1)

class RRT
{
public:
    RRT() { srand((unsigned)time(NULL)); };
    ~RRT() {};
    void rrt_algorithm();
    void getPath(const Point*);
    void testAtan2Radian();
    void getAtan2Node(Point*, const Point* node, float&);
    void delayMs(const int&);
    void changeAngle(float&);
    void showParaBelow(const Point*);
    void addRRTNode(Point*, Point*);
    bool isLegalPoint(const Point*);
    bool isObstacle(const Point*);
    int calcuDist(const Point*, const Point*);
    float getRandomFloat(const float&, const float&);
    Point* getRRTNode(const Point*);
    Point* getRandomRRTNode(int &);
    Point* findMinDistPoint(const Point*);


private:

    std::vector<Point*> rrt_path;
    rrtPara rrt_user_para;
    Point* start = new Point(0, 0);
    Point* end = new Point(rrt_user_para.rrt_graph_width, rrt_user_para.rrt_graph_height);
};

file:rrt.cpp

brief:障碍物的碰撞检测以及生成坐标点的合法性

author:AIplusX

param:const的点坐标

return:布尔型变量

exception:无

note:以探索节点为中心的边长为5的正方形去验证是否有障碍物碰撞,具体参数参考上一篇文章

remarks:判断探索路径是否碰到了障碍物,同时也做了探索路径的地图边界溢出判断

bool RRT::isObstacle(const Point* p)
{
    for (int i = -2; i < rrt_user_para.avoid_obstacles_cnt; i++) {
        for (int j = -2; j < rrt_user_para.avoid_obstacles_cnt; j++) {
            Point tmp_p(p->x + i, p->y + j);
            if (isLegalPoint(&tmp_p)) {
                if (getpixel(tmp_p.x, tmp_p.y) == BLACK) {
                    return true;
                }
            }
        }
    }
    return false;
}

file:rrt.cpp

brief:找到目前的探索路径中离终点最近的探索节点

author:AIplusX

param:const的终点坐标

return:堆中内存的地址

exception:无

note:遍历rrt_path这个vector,这个rrt_path里面存的是探索节点,也就是指针,指向堆中的内存,内存里放的是坐标。线性遍历探索节点找到离终点最近的节点之后返回指针

remarks:现在用的是vector实现的,作者计划在之后的rrt*中利用priority_queue进行实现,在生成探索节点的时候就生成到终点路径的距离,之后要用的时候直接取就可以

Point* RRT::findMinDistPoint(const Point* end)
{
    int min_dist = INT_MAX;
    int temp_dist = 0;
    int min_dist_idx = 0;

    for (int i = 0; i < rrt_path.size(); i++) {
        temp_dist = calcuDist(rrt_path[i], end);
        if (temp_dist < min_dist) {
            min_dist = temp_dist;
            min_dist_idx = i;
        }
    }

    return rrt_path[min_dist_idx];
}

file:rrt.cpp

brief:从已有节点往特定方向(弧度角)延长特定距离

author:AIplusX

param:待修改节点的坐标指针,现有节点坐标指针,角度

return:无

exception:无

note:因为输入的theta角是atan2()生成的,所以首先是将theta角度转换成当前地图坐标系的弧度值,然后分别对探索枝干长度乘以sin和cos,并在已有节点基础上进行延伸从而得到待修改节点的坐标

remarks:无

void RRT::getAtan2Node(Point* p, const Point* node, float& theta)
{
    changeAngle(theta);
    p->x = (cos(theta) * rrt_user_para.rrt_edge_len) + node->x;
    p->y = (sin(theta) * rrt_user_para.rrt_edge_len) + node->y;
}

file:rrt.cpp

brief:在rrt_path中获得离生成的随机点最近的探索节点,并且进行延伸得到新的探索节点,并且通过引用的方式返回探索节点的索引

author:AIplusX

param:索引的引用

return:堆中新的探索节点内存的地址指针

exception:无

note:在函数内部生成随机点,并且实现类似于findMinDistPoint中找到离随机点最近的探索节点,并且用getAtan2Node生成新的探索节点坐标,并返回新的探索节点坐标的地址指针,而且通过引用返回当前探索坐标的索引

remarks:

1:因为我打算在之后的rrt*算法中利用priority_queue进行排列,这样会影响已有探索节点的顺序,因此返回索引的办法就没用了,要重新想一个办法
2:现在的代码不够优美,没有实现代码简洁,找离特定点最近的探索节点这个功能多处用到,应该进行抽象,然后在需要用的地方进行调用

Point* RRT::getRandomRRTNode(int &idx)
{

    Point* p_new = new Point();
    Point p(RANDOM_RANGE(0,rrt_user_para.rrt_graph_width-1),
        RANDOM_RANGE(0,rrt_user_para.rrt_graph_height-1));

    int temp_dist = 0;
    int min_dist = INT_MAX;
    for (int i = 0; i < rrt_path.size(); i++) {
        temp_dist = calcuDist(rrt_path[i], &p);
        if (temp_dist < min_dist) {
            min_dist = temp_dist;
            idx = i;
        }
    }

    Point* p_node = rrt_path[idx];

    if (p.x == p_node->x) {
        p_new->x = p.x;
        int symbol = (p.y - p_node->y) > 0 ? 1 : -1;
        int p_y = p_node->y;
        p_new->y = p_y + symbol * rrt_user_para.rrt_edge_len;
    }
    else {
        float theta = atan2((p.x - p_node->x), (p.y - p_node->y));
        getAtan2Node(p_new, p_node, theta);
    }

    return p_new;
}