在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才能做了。