[rrt_algorithm03]基于easyX的交互界面实现

Posted 2021-12-20 AIplusX

写在前面

这篇文章分享的主要是交互界面的实现，因为我是基于C++实现的，所以可视化界面需要第三方库，我选择的是easyX，然后这篇文章主要介绍atan2()的坐标转换测试的可视化以及rrt探索枝干的可视化。

程序效果

黑色：障碍物（可由鼠标绘制）；
绿色线段：探索路径；
绿色方块：终点
红色方块：起点
棕色线段：最终路径
点划线：路径探索下边界
绿色圆点：代表算法开始运行

程序介绍

atan2()测试

首先是对于atan2()函数输出的测试，函数使用是比较简单的，输入就是dx和dy，输出就是atan的弧度值，但是算法应用的是相机坐标系，不知道atan2()输出的弧度值是什么坐标系的，我们需要进行测试，于是就有了这个测试的可视化程序。

角度转换完毕之后就得到下面的测试图结果：

测试方法就是在空白部分坐标点击一下，测试程序会生成一条从特定点到鼠标所在点的线段，并且在图中显示出此时的弧度值。

可以看到转换之后弧度数值已经正常了，y轴正方向是正的，y轴负方向是负的，此时就可以带入算法中进行运算了。

程序实现如下所示：

void RRT::changeAngle(float& angle)

	if (angle < -PI / 2 && angle > -PI)  angle = -3 * PI / 2 - angle; 
	else  angle = PI / 2 - angle; 


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);
		
	

探索节点线段

rrt在搜索的时候会不断延伸节点探索环境，因此就涉及到了绘制rrt枝干的可视化问题，于是我就写了这个测试例程。

测试的方法就是，基于纠正出来的atan()2函数返回的弧度值进行线段绘制，线段起点是鼠标坐标，然后往终点方向延伸特定长度。

生成了线段之后，测试程序将会在分割线下面显示线段2个端点的坐标。

示意图如下：

程序实现如下所示：

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);