Rplidar A1利用Qt图形库进行周围环境的轮廓抓取

转载请注明出处：https://blog.csdn.net/huaweijian0324/article/details/80551615

之前利用Rplidar A1做了一个简单的周围环境的轮廓抓取软件，

现在整理一下，防止遗忘，也方便给大家做个参考。

我是用Qt图形库来把环境的轮廓给画出来的，

在这里主要介绍一下如何将雷达反馈的数据转化为点在图片上画出来，

可以将雷达反馈的数据按照协议转化为每个点的实际的距离信息和角度信息，

然后主要根据三角函数来算出每个点应该在图片上所在的坐标位置。

这里附上代码：

vacDraw.h

#pragma once

#include <QApplication>
#include <QWidget>
#include <QPainter>
#include <QSettings>

#include <iostream>
#include <cmath>
//#include <opencv2\opencv.hpp>
#include <highgui.h> 
#include <vacDataDeal.h>

using namespace std;
using namespace cv;

#define PI 3.141592653

//定义显示界面大小，rplidar最远距离是6m，所以方便计算，画布取6的倍数
static int LidarImageWdith = 600;
static int LidardarImageHeight = 600;


//测量点数据结构，这个可以参考应用手册
struct scanDot {
	_u8   quality;
	float angle;
	float dist;
};



/**
* @class LidarImage
* @brief 对雷达的数据进行处理，以点的形式画出
*/
class LidarImage
{
friend class vacDealThread;

public:
	LidarImage(void);
	~LidarImage(void);

private:
	//保存每扫描一周的雷达数据
	vector<scanDot> m_scanData; 
	QPixmap lidarImage;

	//读取ini文件
	QSettings *m_configIniWrite;
	QByteArray m_picturePath;

	QFont m_font;

public:
	//将扫面的原始数据转化为实际距离和角度
	void __scanData(rplidar_response_measurement_node_t *buffer, size_t count);
	//将扫描点映射到画布上
	void __draw(QImage &lidarImage);

};
           

vacDraw.cpp

#include "vacDraw.h"


LidarImage::LidarImage(void)
{
	m_configIniWrite = new QSettings("LidarPicturePath.ini", QSettings::IniFormat);
	m_picturePath = m_configIniWrite->value("/Path/LidarPicturePath").toString().toLatin1();
}

LidarImage::~LidarImage(void)
{

}

/**
* @brief  将扫描的原始数据转化为实际距离和角度，并将带有实际数据的点放到m_scanData里
* @param  buffer  雷达数据
* @param  count  雷达数据的个数
* @param  frequency  扫描的频率
*/
void LidarImage::__scanData(rplidar_response_measurement_node_t *buffer, size_t count)
{
	m_scanData.clear();
	for (int pos = 0; pos < (int)count; ++pos) {
		scanDot dot;
		if (!buffer[pos].distance_q2) continue;
		dot.quality = (buffer[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
		dot.angle = (buffer[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
		dot.dist = buffer[pos].distance_q2 / 4.0f;
		m_scanData.push_back(dot);
	}
}

/**
* @brief 在图片上画出相应的点
*/
void LidarImage::__draw(QImage &lidarImage)
{	
	lidarImage = QImage(m_picturePath);

	QPainter painter(&lidarImage);

	int x, y;
	double theta, rho;
	int halfWidth = LidarImageWdith / 2;
	int halfHeight = LidardarImageHeight / 2;

	for (int i = 0; i < m_scanData.size(); i++)
	{
		scanDot dot;
		dot = m_scanData[i];
		theta = dot.angle*PI / 180;
		rho = dot.dist;

		x = (int)(rho*sin(theta) / 20) + halfWidth;
		y = (int)(-rho*cos(theta) / 20) + halfHeight;

		painter.setPen(QColor(255, 255, 255));

		painter.drawEllipse(x, y, 1, 1);

	}


	painter.setPen(QColor(255, 155, 0));

	m_font.setFamily("Microsoft YaHei");
	m_font.setPointSize(10);
	m_font.setItalic(true);
	painter.setFont(m_font);

	painter.drawText(240, 23, "Value Count: ");

	painter.drawText(335, 23, QString::number(m_scanData.size()));

	painter.end();
}
           

另外的话考虑到常见的雷达扫描界面都有一个余晖的效果，所以就想着自己能不能也画出这样的效果，

参考了一些资料，发现用OpenGL可以画出想要的效果。

这里附上代码：

vacAfterglow.h

#ifndef VACAFTERGLOW_H
#define VACAFTERGLOW_H

#include <QOpenGLWidget>
#include <GL/glu.h>
#include <QWidget>
#include <math.h>

static int i = 3600;

static GLfloat the_translatef = (GLfloat)- 1.3;

/**
* @class Afterglow 
* @brief 雷达余辉界面
*/

class Afterglow : public QOpenGLWidget 
{

Q_OBJECT

public:
	Afterglow(QWidget *parent = 0);
	~Afterglow();
	void initializeGL();
	void paintGL();
	void resizeGL(int w, int h);
	//画雷达余辉
	void __DrawLidar();
	
private:
	bool m_isScanLidar;

};

#endif // GLWIDGET_H
           

vacAfterglow.cpp

#include "vacAfterglow.h"

/**
* @brief 此为雷达余辉界面
*/

Afterglow::Afterglow(QWidget *parent) :
QOpenGLWidget(parent)
{
	setGeometry(20, 50, 600, 600);
	m_isScanLidar = false;
}

Afterglow::~Afterglow()
{

}

void Afterglow::initializeGL()
{
	glShadeModel(GL_SMOOTH);               
	glClearDepth(1.0);                     
	glEnable(GL_DEPTH_TEST);               
	glEnable(GL_LINE_SMOOTH);
	glDepthFunc(GL_LEQUAL);               
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
}


void Afterglow::paintGL()
{

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);     
	glLoadIdentity();         
	glTranslatef(0.0, 0.0, the_translatef);        

	//画雷达余辉
	if (m_isScanLidar)
	{
		__DrawLidar();
	}
}


void Afterglow::resizeGL(int w, int h)
{
	if (h == 0)          
	{
		h = 1;
	}
	glViewport(0, 0, (GLint)w, (GLint)h);    
	glMatrixMode(GL_PROJECTION);              
	glLoadIdentity();                          
	gluPerspective(45.0, (GLfloat)w / (GLfloat)h, 0.1, 100.0);      
	glMatrixMode(GL_MODELVIEW);         
	glLoadIdentity();                   
}



/**
* @brief  画雷达的余辉
*/
void Afterglow::__DrawLidar()
{

	glEnable(GL_BLEND);

	GLfloat pi = (GLfloat)3.1415926;

	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

	glBegin(GL_LINES);
	GLfloat agent = 1.0;

	//雷达余晖算法
	for (float m = 0; m < 60; m = m + 0.1)            
	{
		agent = 1.0 - (m / 60);

		glColor4f(0.0, agent, 0.0, 1.0 - (m / 60));

		glVertex3f(0.0f, 0.0f, 0.0f);

		glVertex3f(0.415*cos(pi*((0.0 + 0.2*i) + m) / 180.0), 0.415*sin(pi*((0.0 + 0.2*i) + m) / 180.0), 0.0f);

	}

	glEnd();
	glDisable(GL_BLEND);

	if (i > 0)
	{
		i--;
	}
	else
	{
		i = 3600;
	}
	update();               
}


           

当然界面是有个底图的，我是直接从网上下载了一张雷达界面的图片，这样看起来更好看一点。

下面附上一张效果图：

有不对的地方还请大家多多指教~^o^~！