[rrt_algorithm06]技术文档_rrt

Posted 2021-12-28 AIplusX

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;