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