在VC++中获取Rplidar数据并使用OpneCV显示出来

在VC++中获取Rplidar数据并使用OpneCV显示出来

最近时间空闲下来想研究下SLAM，正好看到大牛的文章http://www.cnblogs.com/gaoxiang12/p/3695962.html，其中讲到如何用kinect深度相机做特征点匹配，进而算出位移。由于手头没有深度相机，想着无非是降个维度，使用2D激光雷达来做位置数据的获取，然后在网上继续查找关于激光雷达数据获取及特征匹配的文章，然后看到http://blog.csdn.net/renshengrumenglibing/article/details/8596401，虽然不是用的思岚科技的Rplidar，但是数据应该差异不大，决定从这个文章入手按照作者的套路用Rplidar一步步做数据获取和展现。

开发环境：Visual Studio 2013

OpenCV版本：OpenCV3.0

文章结构

- 配置OpenCV3.0

- 下载Rplidar sdk

- 创建解决方案并添加sdk项目

- Rplidar数据获取

- 使用OpenCV显示数据

先上显示效果图：



http://blog.csdn.net/renshengrumenglibing/article/details/8596401

配置OpenCV3.0

配置OpenCV花了很长时间，分别尝试下载OpenCV2.4.9和OpenCV2.4.10，环境变量都配好，跑测试程序均死在后提示“OpenCV 无法启动此程序，因为计算机中丢失opencv_core249.dll。请尝试重新安装改程序已解决此问题”尝试各种解决方案无果，抱着试试看的态度下载了OpenCV3.0.0，配置过程参考：http://blog.csdn.net/u010009145/article/details/50756751，最后成功，可以用了

下载Rplidar sdk

在思岚官网下载sdk和应用手册 http://www.slamtec.com/cn/Lidar

解压后可以在tools文件夹中先安装驱动，并记下设备对应的串口号，我的是com30

设备的初始化和数据获取，我是直接看的sdk中带的示例程序ultra_simple，直接看main.cpp很好理解。RPlidar的简单使用流程如下，这些代码是从main函数中摘出来的，方便理解

// create the driver instanc
//创建一个RPlidar的实例，一个实例对应一个设备
RPlidarDriver * drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
// make connection... 连接
drv->connect(opt_com_path, opt_com_baudrate;
//startMotor 启动电机
drv->startMotor();
// start scan...
drv->startScan();
//获取一次旋转一周的数据
op_result = drv->grabScanData(nodes, count);
//停止扫描，停止电机，释放实例
drv->stop();
drv->stopMotor();
RPlidarDriver::DisposeDriver(drv);

如果我们新建一个项目，只需要将路径\rplidar_sdk.1.5.6\sdk\sdk中的sdk文件夹中文件拷贝出来就可以。

创建解决方案并添加sdk项目

在VC2013中按照以下目录结构创建解决方案



其中rplidar_driver项目是静态库，RplidarSLAM项目是win32控制台程序，创建时选择空项目；

1. rplidar_driver中目录结构与sdk文件夹结构一致，然后添加文件即可，在配置-C/C++-附加包含目录中添加路径xxx\sdk\src和xxx\sdk\include；xxx是你自己的工程目录，然后右键该项目-生成，不出意外应该可以生成一个dll

2. RplidarSLAM是一个空项目，包含一个main.cpp、opencv_lidar.cpp 和 opencv_lidar.h，项目的配置-C/C++-附加包含目录中除了第一步中opencv相关的头文件目录外，将rplidar的sdk的include路径xxx\sdk\include也要添加进去；关键的一步是要将上面生成的dll库链接到该项目，在配置-链接器-附加库目录中将上面生成的dll路径添加进去就可以了

3. 开始编程

opencv_lidar.h

#pragma once
#include <iostream>
#include <cmath>

#include <opencv2\opencv.hpp>
#include <highgui.h>

#include "rplidar.h" //RPLIDAR standard sdk, all-in-one header

using namespace std;
using namespace cv;
//定义opencv显示界面大小，rplidar最远距离是6m，所以方便计算，画布取6的倍数
static int RadarImageWdith = 600;
static int RadarImageHeight = 600;
//测量点数据结构，这个可以参考应用手册
struct scanDot {
_u8   quality;
float angle;
float dist;
};

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

vector<scanDot> scan_data; //保存每扫描一周的雷达数据
float scan_speed;
void scanData(rplidar_response_measurement_node_t *buffer, size_t count, float frequency);
void draw(IplImage* RadarImage);
};

opencv_lidar.cpp

#include "opencv_lidar.h"
#define PI 3.141592653

LidarImage::LidarImage(void)
{
}

LidarImage::~LidarImage(void)
{

}
//将扫面的原始数据转化为实际距离和角度，这个可以参考ultra_simple
void LidarImage::scanData(rplidar_response_measurement_node_t *buffer, size_t count, float frequency)
{
scan_data.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;
scan_data.push_back(dot);
}

scan_speed = frequency;
}
//将扫描点映射到画布上
void LidarImage::draw(IplImage* RadarImage)
{
//RadarImage = cvCreateImage(cvSize(RadarImageWdith,RadarImageHeight),IPL_DEPTH_8U,1);
cvZero(RadarImage);
//在中心加上一个圆心
cvCircle(RadarImage, cvPoint(RadarImageWdith / 2, RadarImageHeight / 2), 3, CV_RGB(0, 255, 255), -1, 8, 0);

int x, y;
double theta, rho;
unsigned char * pPixel = 0;
int halfWidth = RadarImageWdith / 2;
int halfHeight = RadarImageHeight / 2;

//此处为在界面上显示文字使用了CvFont
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_COMPLEX, 0.5, 0.5, 0, 1, 8);

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

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

cvCircle(RadarImage, cvPoint(x, y), 1, CV_RGB(0, 255, 0), 1, 8, 3); //每个点用一个小圆表示
}

char s[35];
sprintf_s(s, "Value Count: %d, Scan Speed: %0.2f", scan_data.size(), scan_speed);
cvPutText(RadarImage, s, cvPoint(50, 50), &font, cvScalar(255, 0, 0));//显示扫描的有效点个数和扫描频率
}

main.cpp

#include <stdio.h>
#include <stdlib.h>

#include "rplidar.h" //RPLIDAR standard sdk, all-in-one header
#include "opencv_lidar.h"

#include <iostream>
#include <cmath>
#include "io.h"

#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif

using namespace rp::standalone::rplidar;

bool checkRPLIDARHealth(RPlidarDriver * drv)
{
u_result     op_result;
rplidar_response_device_health_t healthinfo;

op_result = drv->getHealth(healthinfo);
if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed.
printf("RPLidar health status : %d\n", healthinfo.status);
if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
fprintf(stderr, "Error, rplidar internal error detected. Please reboot the device to retry.\n");
// enable the following code if you want rplidar to be reboot by software
// drv->reset();
return false;
}
else {
return true;
}

}
else {
fprintf(stderr, "Error, cannot retrieve the lidar health code: %x\n", op_result);
return false;
}
}

int main(int argc, const char * argv[]) {
const char * opt_com_path = NULL;
_u32         opt_com_baudrate = 115200;
u_result     op_result;

// read serial port from the command line...
if (argc>1) opt_com_path = argv[1]; // or set to a fixed value: e.g. "com3"

// read baud rate from the command line if specified...
if (argc>2) opt_com_baudrate = strtoul(argv[2], NULL, 10);

if (!opt_com_path) {
#ifdef _WIN32
// use default com port
opt_com_path = "\\\\.\\com3";
#else
opt_com_path = "/dev/ttyUSB0";
#endif
}

// create the driver instance
RPlidarDriver * drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);

//creat opencv image
IplImage* RadarImage = cvCreateImage(cvSize(RadarImageWdith, RadarImageHeight), IPL_DEPTH_8U, 3);

LidarImage lidarImage;

if (!drv) {
fprintf(stderr, "insufficent memory, exit\n");
exit(-2);
}

// make connection...
if (IS_FAIL(drv->connect(opt_com_path, opt_com_baudrate))) {
fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
, opt_com_path);
goto on_finished;
}

// check health...
if (!checkRPLIDARHealth(drv)) {
goto on_finished;
}

// start scan...
drv->startScan();

// fetech result and print it out...
char key;
cvNamedWindow("Radar", 1);

while (1) {
rplidar_response_measurement_node_t nodes[360 * 2];
size_t   count = _countof(nodes);

op_result = drv->grabScanData(nodes, count);

if (IS_OK(op_result)) {
float frequency = 0;
drv->getFrequency(nodes, count, frequency);
lidarImage.scanData(nodes, count, frequency);
lidarImage.draw(RadarImage);

cvShowImage("Radar", RadarImage);
key = cvWaitKey(30);
if (key == 27)//esc退出
{
break;
}
}
}

cvReleaseImage(&RadarImage);
cvDestroyWindow("Radar");
// done!
on_finished:
RPlidarDriver::DisposeDriver(drv);
return 0;
}

如果不出意外，生成RplidarSLAM.exe后，直接cmd打开命令行，定位到RplidarSLAM的输出文件夹，输入RplidarSLAM.exe \.\com30后就可以看到雷达数据显示界面啦。

然而这只是学习SLAM微不足道的一步，看了很多文章，2DSLAM很多实用GMAPPING实现的，感觉又要转ROS才能做了。