激光雷达学习笔记（二）数据读取和显示

关注

转载最新推荐文章于 2024-02-14 08:00:50 发布 1.8w 阅读 AI 写同款· GEO 优化 ›

实验二读取和理解激光雷达数据 1. 体验内容（1）为rplidar添加USB权限将激光雷达连接到电脑上，检查识别到的USB设备 ls -l /dev|grep ttyUSB 若没有出现如下图所示的“rplidar”字样，说明需要添加USB的默认识别权限，遵循接下来的教程进行操作。 sudo gedit /etc/udev/rules.d/rapliar.rules 输入密码打开文件进行编辑，添加以下内容 KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct} 阅读详情

我使用的激光雷达都是日本产北阳电机公司Hokuyo的产品，URG04LX和UTM-30LX。UTM-30LX的价格贵一些，10买的时候大概3W，现在taobao价貌似已经涨到5W了，当然URG04LX也涨价了，09年买的时候是1W，只是不知道现在涨到了多少。

URG-04LX基本参数：

激光类型：λ=785nm，Class1，0.8mw激光

扫描距离：20-5600 mm

扫描范围：240度

角度分辨率：0.36度

距离分辨率：1mm

精度：距离 0.06-1m: +30 距离 1-4m:30%

扫描时间：100ms （10Hz刷新率）

接口类型：USB 2.0

输入电压：5V

消耗电流：500mA (最高可达800mA)

重量：160g

尺寸：50x50x70mm

典型寿命:5年

UTM-30LX基本参数：

Voltage 12.0 V ±10%
Current 0.7 A (Rush current 1.0 A)
Detection range 0.1 m to approximately 60 m (<30 m guaranteed)
Laser wavelength 870 nm, Class 1
Scan angle 270°
Scan time 25 ms/scan (40.0 Hz)
Angular resolution 0.25°
Interface USB 2.0
Weight 8.2 oz (233 g)

北阳电机提供激光雷达的上位机软件，叫做UrgBenri，可以很方便的进行数据的采集、显示、存储。如果想用C++程序自己采集可以参照上一篇笔记。

UrgBenri采集存储的数据是ASCII格式，可以直接用记事本之类的软件打开，数据格式基本是这样的：

   01.
   [model]
  
   02.
   UTM-30LX(Hokuyo Automatic Co.,Ltd.)
  
   03.
   [frontStep]
  
   04.
   540
  
   05.
   [minDistance]
  
   06.
   23
  
   07.
   [maxDistance]
  
   08.
   60000
  
   09.
   [motorSpeed]
  
   10.
   2400
  
   11.
   [totalSteps]
  
   12.
   1440
  
   13.
   [captureMode]
  
   14.
   GD_Capture_mode
  
   15.
   [scanMsec]
  
   16.
   25
  
   17.
   [startStep]
  
   18.
   0
  
   19.
   [endStep]
  
   20.
   1080
  
   21.
   [serialNumber]
  
   22.
   00904066
  
   23.
   [timestamp]
  
   24.
   793464
  
   25.
   [logtime]
  
   26.
   2013:1:12:15:6:32:312
  
   27.
   [scan]
  
   28.
   392;397;398;400;403;403;407;409;409;409;411;411;422;424;424;422;420;420;420;427;428;435;436;439;440;444;449;449;449;449;454;454;456;462;463;463;463;459;456;451;449;445;444;439;439;433;433;433;433;431;431;430;430;425;425;422;425;425;433;437;437;433;433;425;421;409;395;391;391;389;384;383;378;378;378;378;377;377;377;376;377;376;376;367;367;367;367;368;371;371;371;371;371;370;370;372;369;369;372;372;371;371;371;369;369;365;365;364;363;357;357;357;357;353;359;359;353;353;352;344;341;339;339;339;339;339;341;334;332;331;331;331;331;331;331;331;330;330;330;330;330;329;329;329;326;323;323;323;329;329;329;327;323;322;319;319;319;319;319;320;322;321;322;322;328;323;325;325;325;324;324;317;315;315;316;315;316;321;321;314;318;314;308;303;303;303;317;317;317;317;314;314;314;314;311;308;308;310;310;316;316;316;315;310;310;312;312;317;317;317;317;313;313;313;313;316;316;315;315;315;315;314;314;317;317;317;319;321;320;320;320;319;317;317;319;317;317;315;315;315;316;324;324;324;318;318;318;325;332;332;332;332;332;330;331;333;331;331;331;330;328;327;327;328;328;328;328;328;328;325;325;326;326;326;330;330;331;330;337;337;337;338;340;338;338;338;337;336;336;336;335;336;350;350;350;350;350;345;343;343;341;343;343;347;346;350;350;352;352;357;357;357;357;357;357;364;357;367;367;366;364;366;366;370;370;370;370;370;370;378;378;379;380;380;379;379;380;379;379;382;384;382;382;384;380;380;384;386;388;388;388;383;383;383;389;394;395;395;396;394;396;396;406;410;410;413;413;413;413;421;421;426;427;427;428;429;429;432;432;439;442;451;451;454;457;458;460;468;472;489;492;495;495;493;495;507;512;522;524;533;546;585;614;614;614;611;602;602;598;590;590;591;591;594;596;596;596;598;596;589;589;589;589;590;590;594;594;594;592;592;587;577;570;559;556;552;552;552;551;551;541;541;539;539;538;534;534;534;534;536;536;533;526;526;526;524;524;524;512;510;510;510;510;510;508;477;467;462;453;449;445;434;434;420;410;403;388;387;386;377;346;342;337;325;325;321;315;294;294;292;281;277;262;257;242;218;180;173;172;170;170;170;170;170;171;171;171;172;174;172;171;172;171;170;170;177;170;170;174;170;170;170;174;175;176;187;187;176;176;180;174;180;184;184;179;179;178;178;176;176;179;185;180;181;181;180;180;177;177;177;180;182;182;182;182;180;178;180;183;183;186;186;186;187;188;188;188;188;186;186;186;187;188;188;190;188;187;187;189;189;189;189;189;188;188;188;188;195;196;195;194;195;194;194;201;201;199;199;199;199;198;198;198;195;192;190;190;190;190;196;197;197;197;194;193;193;192;193;193;194;194;196;198;198;198;198;198;198;194;194;194;194;194;197;197;197;192;192;192;194;205;205;205;193;189;189;189;210;210;210;213;213;332;506;627;860;907;958;980;1031;1043;1111;1117;1117;1117;1000;986;985;985;985;981;981;978;971;970;970;970;970;976;976;977;977;981;981;981;945;945;945;946;956;970;975;979;979;975;948;933;920;898;859;836;823;814;799;799;794;794;793;793;789;773;748;745;736;732;727;727;727;727;729;743;744;744;744;742;729;725;711;706;706;706;708;699;698;693;690;689;688;684;675;668;665;660;660;660;660;659;659;649;644;644;646;649;647;649;647;647;634;631;631;634;628;628;625;623;622;615;601;601;601;601;601;601;589;589;589;589;590;590;590;583;583;583;580;576;576;575;575;577;577;572;569;568;567;567;568;566;568;568;566;566;561;561;561;559;557;557;557;561;562;562;561;557;557;557;559;559;560;561;561;561;561;563;563;561;561;561;559;556;556;556;562;562;562;556;556;552;552;550;547;543;547;548;548;555;555;555;553;553;547;547;547;547;547;542;541;541;541;548;551;558;558;552;552;558;559;561;564;564;564;567;567;567;568;568;570;568;568;570;573;573;573;573;568;567;568;565;565;567;573;567;567;566;566;566;574;574;574;568;568;568;570;572;572;572;573;569;569;569;573;575;579;579;579;582;581;581;585;587;587;587;585;581;581;581;586;586;586;586;586;586;597;605;605;607;610;612;612;613;620;623;625;629;645;648;650;650;650;645;638;638;638;645;647;647;647;647;647;647;674;695;696;696;696;695;681;681;696;701;737;814;880;880;880;860;828;799;791;764;737;735;723;723;723;722;722;722;722;723;728;733;733;733;711;705;683;677;672;671;669;668;662;641;641;639;634;630;630;612;606;606;609;606;606;590;585;578;578;575;575;560;560;556;551;549;548;547;538;536;535;520;520;517;515;513;509;503;489;489;490;490;490;490;485;479;465;465;465;468;468;468;458;457;448;448;446;440;440;439;431;405;405;405;413;416;419;419;418;416;418;418;418;416;408;408;408;410;410;410;403;396
  
   29.
   [timestamp]
  
   30.
   793514
  
   31.
   [logtime]
  
   32.
   2013:1:12:15:6:32:351
  
   33.
   [scan]

[]内记录属性名，下一行会记录属性的内容，这个在读取的时候是非常的方便的。到[scan]不是文件结束了，而是后面又是一大串的数据，然后继续往下记录一帧一帧的数据。

首先用MATLAB编写了一份数据解析的代码，能够读取ubh文件并显示：

   01.
   %根据ubh文件的特点，进行读取
  
   02.
    
   03.
   %1 读取各个属性
  
   04.
   % Search for number of string matches per line.
  
   05.
   % fid = fopen('ring.ubh', 'rt');
  
   06.
   fid = fopen('utm30LX.ubh', 'rt');
  
   07.
   lineNumber = 0;
  
   08.
   while feof(fid) ==0
  
   09.
   tline = fgetl(fid);
  
   10.
   switch(tline)
  
   11.
   case '[model]'
  
   12.
   model = fgetl(fid); %sscanf(fgetl(fid),'%d')
  
   13.
   case '[frontStep]'
  
   14.
   frontStep = sscanf(fgetl(fid),'%d');
  
   15.
   case '[minDistance]'
  
   16.
   minDistance = sscanf(fgetl(fid),'%d');
  
   17.
   case '[maxDistance]'
  
   18.
   maxDistance = sscanf(fgetl(fid),'%d');
  
   19.
   case '[motorSpeed]'
  
   20.
   motorSpeed = sscanf(fgetl(fid),'%d');
  
   21.
   case '[totalSteps]'
  
   22.
   totalSteps = sscanf(fgetl(fid),'%d');
  
   23.
   case '[captureMode]'
  
   24.
   captureMode = fgetl(fid);
  
   25.
   case '[scanMsec]'
  
   26.
   scanMsec = fgetl(fid);
  
   27.
   case '[startStep]'
  
   28.
   startStep = sscanf(fgetl(fid),'%d');
  
   29.
   case '[endStep]'
  
   30.
   endStep = sscanf(fgetl(fid),'%d');
  
   31.
   case '[serialNumber]'
  
   32.
   serialNumber = fgetl(fid);
  
   33.
   case '[timestamp]'
  
   34.
   frontStep = sscanf(fgetl(fid),'%d');
  
   35.
   case '[logtime]'
  
   36.
   frontStep = sscanf(fgetl(fid),'%d');
  
   37.
   case '[scan]'
  
   38.
   [rho,count] = sscanf(fgetl(fid),'%d;');
  
   39.
   %数据显示
  
   40.
   rho = rho';
  
   41.
   length = size(rho,2);
  
   42.
   theta = 1:length;
  
   43.
   polar(theta/4*pi/180,rho);
  
   44.
   pause(0.25);
  
   45.
   %对数据进行处理
  
   46.
    
   47.
   otherwise
  
   48.
   %disp('parameter init...');       
  
   49.
   end  
  
   50.
   lineNumber = lineNumber+1;
  
   51.
   disp(lineNumber);
  
   52.
   %     if lineNumber >100
  
   53.
   %        break; 
  
   54.
   %     end
  
   55.
   end
  
   56.
   fclose(fid);

显示效果如下：

上一篇文章中提到，激光雷达的算法最终是需要移植到嵌入式设备上的，因此还是希望用C/C++来开发，这样便于移植。只是C++读取ubh格式的文件稍微有些麻烦，而在实时应用中也是直接从激光雷达获取一帧的数据进行处理，不会去读取ubh格式的文件，这里偷个懒，直接用Matlab将ubh文件拆分成一帧一帧的数据文件，然后再由C++一帧一帧的读取。

Matlab已经能够将数据一帧一帧的读取出来，那么稍微改动几句据可以将每帧拆分成单个的文件存储。

   01.
   %将ubh文件拆分成一个一个的CSV文件
  
   02.
   %根据ubh文件的特点，进行读取
  
   03.
   %1 读取各个属性
  
   04.
   % Search for number of string matches per line.
  
   05.
   fid = fopen('utm30LX.ubh', 'rt');
  
   06.
   lineNumber = 0;
  
   07.
   fileName = 'data_0.csv';
  
   08.
   frameCnt = 0;
  
   09.
   while feof(fid) ==0
  
   10.
   tline = fgetl(fid);
  
   11.
   switch(tline)
  
   12.
   case '[model]'
  
   13.
   model = fgetl(fid); %sscanf(fgetl(fid),'%d')
  
   14.
   case '[frontStep]'
  
   15.
   frontStep = sscanf(fgetl(fid),'%d');
  
   16.
   case '[minDistance]'
  
   17.
   minDistance = sscanf(fgetl(fid),'%d');
  
   18.
   case '[maxDistance]'
  
   19.
   maxDistance = sscanf(fgetl(fid),'%d');
  
   20.
   case '[motorSpeed]'
  
   21.
   motorSpeed = sscanf(fgetl(fid),'%d');
  
   22.
   case '[totalSteps]'
  
   23.
   totalSteps = sscanf(fgetl(fid),'%d');
  
   24.
   case '[captureMode]'
  
   25.
   captureMode = fgetl(fid);
  
   26.
   case '[scanMsec]'
  
   27.
   scanMsec = fgetl(fid);
  
   28.
   case '[startStep]'
  
   29.
   startStep = sscanf(fgetl(fid),'%d');
  
   30.
   case '[endStep]'
  
   31.
   endStep = sscanf(fgetl(fid),'%d');
  
   32.
   case '[serialNumber]'
  
   33.
   serialNumber = fgetl(fid);
  
   34.
   case '[timestamp]'
  
   35.
   frontStep = sscanf(fgetl(fid),'%d');
  
   36.
   case '[logtime]'
  
   37.
   frontStep = sscanf(fgetl(fid),'%d');
  
   38.
   case '[scan]'
  
   39.
   [rho,count] = sscanf(fgetl(fid),'%d;');
  
   40.
   %将数据写入文件
  
   41.
   fileName =  sprintf('data_%d.csv',frameCnt);
  
   42.
   fidw = fopen(fileName,'w');
  
   43.
   fprintf(fidw,'%d,',rho);
  
   44.
   fclose(fidw);
  
   45.
   frameCnt = frameCnt + 1;
  
   46.
   %数据显示
  
   47.
   %             rho = rho';
  
   48.
   %             length = size(rho,2);
  
   49.
   %             theta = ((1:length)/4 - 45)*pi/180;
  
   50.
    
   51.
   %             polar(theta,rho);
  
   52.
   %             pause(0.1);
  
   53.
   %对数据进行处理    
  
   54.
   otherwise
  
   55.
   %disp('parameter init...');       
  
   56.
   end  
  
   57.
   lineNumber = lineNumber+1;
  
   58.
   disp(lineNumber);
  
   59.
   %     if lineNumber >100
  
   60.
   %        break; 
  
   61.
   %     end
  
   62.
   end
  
   63.
   fclose(fid);

此时已经有了一帧一帧的雷达数据，可以进行计算了。C++读取文件比较简单，不再讲，但是在进行算法实现直线一个关键的问题实现雷达数据的显示，OpenCV是进行图像处理常用开源库，里面有很多库函数激光雷达也是可以用的，因此决定使用C++ + OpenCV来开发激光雷达的算法。刚好OpenCV可以进行数据的显示。 www.it165.net

雷达数据显示的基本思路是将数据绘制到一张图片上进行显示。（高兴，这下到了OpenCV的老本行啦）

OpenRadar.h

   01.
   #pragma once
  
   02.
   #include <iostream>
  
   03.
   #include <cmath>
  
   04.
   #include<cv.h>
  
   05.
   #include<highgui.h>
  
   06.
   using namespace std;
  
   07.
   #include <vector>
  
   08.
    
   09.
   static int RadarImageWdith  = 600;
  
   10.
   static int RadarImageHeight = 600;
  
   11.
    
   12.
   class OpenRadar
  
   13.
   {
  
   14.
   public:
  
   15.
   OpenRadar(void);
  
   16.
   ~OpenRadar(void);
  
   17.
    
   18.
   vector<int>RadarRho;
  
   19.
   //数据读取
  
   20.
   bool RadarRead(char  *fileName);
  
   21.
   void CreateRadarImage(IplImage* RadarImage);
  
   22.
    
   23.
   };</int></vector></highgui.h></cv.h></cmath></iostream>

OpenRadar.cpp

   01.
   #include "OpenRadar.h"
  
   02.
   #define pi 3.141592653
  
   03.
    
   04.
   OpenRadar::OpenRadar(void)
  
   05.
   {
  
   06.
   }
  
   07.
    
   08.
    
   09.
   OpenRadar::~OpenRadar(void)
  
   10.
   {
  
   11.
   }
  
   12.
   bool OpenRadar::RadarRead(char *fileName){
  
   13.
   FILE* fp = NULL;
  
   14.
   int dis = 0;
  
   15.
   int totalCnt = 0;
  
   16.
   fp = fopen(fileName,"r");
  
   17.
   if (fp == NULL)
  
   18.
   {
  
   19.
   //cout<<"failed to read"<<endl; -="" char="" count:="" double="" for="" halfheight="RadarImageHeight/2;" halfwidth="RadarImageWdith/2;" i="0;" if="" include="" int="" iostream="" pi="" ppixel="0;" radarimage="cvCreateImage(cvSize(RadarImageWdith,RadarImageHeight),IPL_DEPTH_8U,1);" radarimageheight="" radarimagewdith="" return="" rho="RadarRho.at(i);" theta="(i/4.0" unsigned="" void="" x="(int)(rho*cos(theta)/5)" y="(int)(-rho*sin(theta)/5)+">
  
   20.
   #include <cmath>
  
   21.
   #include "io.h"
  
   22.
   using namespace std;
  
   23.
   const int MAX_POINT_COUNT = 1200;
  
   24.
   int Rho[MAX_POINT_COUNT] = {0}; 
  
   25.
    
   26.
   int main(){
  
   27.
   OpenRadar openRadar;
  
   28.
   char fileName[32] = "csv\\data_0.csv";
  
   29.
   int frameCnt = 0;
  
   30.
   char key;
  
   31.
   IplImage* RadarImage = cvCreateImage(cvSize(RadarImageWdith,RadarImageHeight),IPL_DEPTH_8U,3);
  
   32.
   while (access(fileName,0) == 0)
  
   33.
   {
  
   34.
   sprintf(fileName,"csv\\data_%d.csv",frameCnt);
  
   35.
   openRadar.RadarRead(fileName);
  
   36.
    
   37.
   openRadar.CreateRadarImage(RadarImage);
  
   38.
   cvNamedWindow("Radar",1);
  
   39.
   cvShowImage("Radar",RadarImage);
  
   40.
   key = cvWaitKey(30);
  
   41.
   if (key == 27)//esc退出
  
   42.
   {
  
   43.
   break;
  
   44.
   }
  
   45.
   frameCnt++;
  
   46.
   }
  
   47.
   cvReleaseImage(&RadarImage);
  
   48.
   cvDestroyWindow("Radar");
  
   49.
   return 0;
  
   50.
   }</cmath></endl;>