[rrt_algorithm07]技术文档_rrt

Posted 2021-12-29 AIplusX

写在前面

这篇文章是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);