激光雷达或者叫激光测距仪数据采集比较简单,有位好心的网友提供了一篇博客专门讲这个,这里就不再赘述,贴出链接,需要的直接去看原文,激光雷达的型号:UTM-30LX。
当前网上关于激光雷达的资料比较少,毕竟用的人不是很多。开发环境主流的还是C/C++,官方提供的例程也都是C/C++的。
官网资料:http://www.hokuyo-aut.jp/02sensor/07scanner/download/urg_programs_en/ 上面包括激光雷达的驱动和采集软件都有提供,需要的话只需要按照上面的步骤去做就可以。
虽然激光雷达的型号不同,采集部分的代码不同,但是数据处理部分的方法确实相同的,在接下来的日子里我会逐渐共享我使用激光雷达数据所用到的算法和代码,共同学习,共同进步。
现在的开发语言是C/C++,控制台程序,为了便于快速实现以及进行图形的显示,使用了OpenCV2.4的代码。当然也有人用Labview或者Matlab开发激光雷达,不同的方法之间各有利弊,同时也看个人的喜好。Labview界面部分很简单快捷,编程效率也很高,但是感觉算法的实现稍微麻烦些;Matlab编程效率高,常用函数齐全。Matlab和Labview开发激光雷达一个共同的问题是程序的可移植性。我更加希望我的程序可以移植到嵌入式平台上,不管是Linux环境或者裸机环境。www.it165.net
激光雷达数据采集程序:(UTM-30LX)
001.
/*!
002.
\file
003.
\brief Sample to get URG data using Win32
004.
005.
\author Satofumi KAMIMURA
006.
007.
$Id: capture_sample.cpp 1724 2010-02-25 10:43:11Z satofumi $
008.
009.
Compling and execute process
010.
- In case of Visual Studio
011.
- Select capture_sample.sln from capture_sample.zip
012.
- When Visual Studio is started, press F5 to build and execute.
013.
- If COM port is not found, then change the com_port in main function.
014.
015.
- In case of MinGW, Cygwin
016.
- % g++ capture_sample.cpp -o capture_sample
017.
- % ./capture_sample
018.
- If COM port is not found, then change the com_port in main function.
019.
020.
\attention Change com_port, com_baudrate values in main() with relevant values.
021.
\attention We are not responsible for any loss or damage occur by using this program
022.
\attention We appreciate the suggestions and bug reports
023.
*/
024.
025.
#define _CRT_SECURE_NO_WARNINGS
026.
027.
#include <windows.h>
028.
#include <cstdio>
029.
#include <cstdlib>
030.
#include <cstring>
031.
#include <string>
032.
033.
using
namespace
std;
034.
035.
036.
// To record the output of SCIP,define RAW_OUTPUT
037.
//#define RAW_OUTPUT
038.
039.
#if defined(RAW_OUTPUT)
040.
static
FILE
* Raw_fd_ = NULL;
041.
#endif
042.
043.
044.
enum
{
045.
Timeout = 1000,
// [msec]
046.
EachTimeout = 2,
// [msec]
047.
LineLength = 64 + 3 + 1 + 1 + 1 + 16,
048.
};
049.
050.
static
HANDLE
HCom = INVALID_HANDLE_VALUE;
051.
static
int
ReadableSize = 0;
052.
static
char
* ErrorMessage =
"no error."
;
053.
054.
055.
/*!
056.
\brief Manage sensor information
057.
*/
058.
typedef
struct
059.
{
060.
enum
{
061.
MODL = 0,
//!< Sensor model information
062.
DMIN,
//!< Minimum measurable distance [mm]
063.
DMAX,
//!< Maximum measurable distance [mm]
064.
ARES,
//!< Angle of resolution
065.
AMIN,
//!< Minimum measurable area
066.
AMAX,
//!< Maximum measurable area
067.
AFRT,
//!< Front direction value
068.
SCAN,
//!< Standard angular velocity
069.
};
070.
string model;
//!< Obtained MODL information
071.
long
distance_min;
//!< Obtained DMIN information
072.
long
distance_max;
//!< Obtained DMAX information
073.
int
area_total;
//!< Obtained ARES information
074.
int
area_min;
//!< Obtained AMIN information
075.
int
area_max;
//!< Obtained AMAX information
076.
int
area_front;
//!< Obtained AFRT information
077.
int
scan_rpm;
//!< Obtained SCAN information
078.
079.
int
first;
//!< Starting position of measurement
080.
int
last;
//!< End position of measurement
081.
int
max_size;
//!< Maximum size of data
082.
long
last_timestamp;
//!< Time stamp when latest data is obtained
083.
} urg_state_t;
084.
085.
086.
// Delay
087.
static
void
delay(
int
msec)
088.
{
089.
Sleep(msec);
090.
}
091.
092.
093.
static
int
com_changeBaudrate(
long
baudrate)
094.
{
095.
DCB dcb;
096.
097.
GetCommState(HCom, &dcb);
098.
dcb.BaudRate = baudrate;
099.
dcb.ByteSize = 8;
100.
dcb.Parity = NOPARITY;
101.
dcb.fParity = FALSE;
102.
dcb.StopBits = ONESTOPBIT;
103.
SetCommState(HCom, &dcb);
104.
105.
return
0;
106.
}
107.
108.
109.
// Serial transceiver
110.
static
int
com_connect(
const
char
* device,
long
baudrate)
111.
{
112.
#if defined(RAW_OUTPUT)
113.
Raw_fd_ =
fopen
(
"raw_output.txt"
,
"w"
);
114.
#endif
115.
116.
char
adjust_device[16];
117.
_snprintf(adjust_device, 16,
"\\\\.\\%s"
, device);
118.
HCom = CreateFileA(adjust_device, GENERIC_READ | GENERIC_WRITE, 0,
119.
NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
120.
121.
if
(HCom == INVALID_HANDLE_VALUE) {
122.
return
-1;
123.
}
124.
125.
// Baud rate setting
126.
return
com_changeBaudrate(baudrate);
127.
}
128.
129.
130.
static
void
com_disconnect(
void
)
131.
{
132.
if
(HCom != INVALID_HANDLE_VALUE) {
133.
CloseHandle(HCom);
134.
HCom = INVALID_HANDLE_VALUE;
135.
}
136.
}
137.
138.
139.
static
int
com_send(
const
char
* data,
int
size)
140.
{
141.
DWORD
n;
142.
WriteFile(HCom, data, size, &n, NULL);
143.
return
n;
144.
}
145.
146.
147.
static
int
com_recv(
char
* data,
int
max_size,
int
timeout)
148.
{
149.
if
(max_size <= 0) {
150.
return
0;
151.
}
152.
153.
if
(ReadableSize < max_size) {
154.
DWORD
dwErrors;
155.
COMSTAT ComStat;
156.
ClearCommError(HCom, &dwErrors, &ComStat);
157.
ReadableSize = ComStat.cbInQue;
158.
}
159.
160.
if
(max_size > ReadableSize) {
161.
COMMTIMEOUTS pcto;
162.
int
each_timeout = 2;
163.
164.
if
(timeout == 0) {
165.
max_size = ReadableSize;
166.
167.
}
else
{
168.
if
(timeout < 0) {
169.
/* If timeout is 0, this function wait data infinity */
170.
timeout = 0;
171.
each_timeout = 0;
172.
}
173.
174.
/* set timeout */
175.
GetCommTimeouts(HCom, &pcto);
176.
pcto.ReadIntervalTimeout = timeout;
177.
pcto.ReadTotalTimeoutMultiplier = each_timeout;
178.
pcto.ReadTotalTimeoutConstant = timeout;
179.
SetCommTimeouts(HCom, &pcto);
180.
}
181.
}
182.
183.
DWORD
n;
184.
ReadFile(HCom, data, (
DWORD
)max_size, &n, NULL);
185.
#if defined(RAW_OUTPUT)
186.
if
(Raw_fd_) {
187.
for
(
int
i = 0; i < n; ++i) {
188.
fprintf
(Raw_fd_,
"%c"
, data[i]);
189.
}
190.
fflush
(Raw_fd_);
191.
}
192.
#endif
193.
if
(n > 0) {
194.
ReadableSize -= n;
195.
}
196.
197.
return
n;
198.
}
199.
200.
201.
// The command is transmitted to URG
202.
static
int
urg_sendTag(
const
char
* tag)
203.
{
204.
char
send_message[LineLength];
205.
_snprintf(send_message, LineLength,
"%s\n"
, tag);
206.
int
send_size = (
int
)
strlen
(send_message);
207.
com_send(send_message, send_size);
208.
209.
return
send_size;
210.
}
211.
212.
213.
// Read one line data from URG
214.
static
int
urg_readLine(
char
*buffer)
215.
{
216.
int
i;
217.
for
(i = 0; i < LineLength -1; ++i) {
218.
char
recv_ch;
219.
int
n = com_recv(&recv_ch, 1, Timeout);
220.
if
(n <= 0) {
221.
if
(i == 0) {
222.
return
-1;
// timeout
223.
}
224.
break
;
225.
}
226.
if
((recv_ch ==
'\r'
) || (recv_ch ==
'\n'
)) {
227.
break
;
228.
}
229.
buffer[i] = recv_ch;
230.
}
231.
buffer[i] =
'\0'
;
232.
233.
return
i;
234.
}
235.
236.
237.
// Trasmit command to URG and wait for response
238.
static
int
urg_sendMessage(
const
char
* command,
int
timeout,
int
* recv_n)
239.
{
240.
int
send_size = urg_sendTag(command);
241.
int
recv_size = send_size + 2 + 1 + 2;
242.
char
buffer[LineLength];
243.
244.
int
n = com_recv(buffer, recv_size, timeout);
245.
*recv_n = n;
246.
247.
if
(n < recv_size) {
248.
// if received data size is incorrect
249.
return
-1;
250.
}
251.
252.
if
(
strncmp
(buffer, command, send_size -1)) {
253.
// If there is mismatch in command
254.
return
-1;
255.
}
256.
257.
// !!! check checksum here
258.
259.
// Convert the response string into hexadecimal number and return that value
260.
char
reply_str[3] =
"00"
;
261.
reply_str[0] = buffer[send_size];
262.
reply_str[1] = buffer[send_size + 1];
263.
return
strtol
(reply_str, NULL, 16);
264.
}
265.
266.
267.
// Change baudrate
268.
static
int
urg_changeBaudrate(
long
baudrate)
269.
{
270.
char
buffer[] =
"SSxxxxxx\r"
;
271.
_snprintf(buffer, 10,
"SS%06d\r"
, baudrate);
272.
int
dummy = 0;
273.
int
ret = urg_sendMessage(buffer, Timeout, &dummy);
274.
275.
if
((ret == 0) || (ret == 3) || (ret == 4)) {
276.
return
0;
277.
}
else
{
278.
return
-1;
279.
}
280.
}
281.
282.
283.
// Read out URG parameter
284.
static
int
urg_getParameters(urg_state_t* state)
285.
{
286.
// Read parameter
287.
urg_sendTag(
"PP"
);
288.
char
buffer[LineLength];
289.
int
line_index = 0;
290.
enum
{
291.
TagReply = 0,
292.
DataReply,
293.
Other,
294.
};
295.
int
line_length;
296.
for
(; (line_length = urg_readLine(buffer)) > 0; ++line_index) {
297.
298.
if
(line_index == Other + urg_state_t::MODL) {
299.
buffer[line_length - 2] =
'\0'
;
300.
state->model = &buffer[5];
301.
302.
}
else
if
(line_index == Other + urg_state_t::DMIN) {
303.
state->distance_min =
atoi
(&buffer[5]);
304.
305.
}
else
if
(line_index == Other + urg_state_t::DMAX) {
306.
state->distance_max =
atoi
(&buffer[5]);
307.
308.
}
else
if
(line_index == Other + urg_state_t::ARES) {
309.
state->area_total =
atoi
(&buffer[5]);
310.
311.
}
else
if
(line_index == Other + urg_state_t::AMIN) {
312.
state->area_min =
atoi
(&buffer[5]);
313.
state->first = state->area_min;
314.
315.
}
else
if
(line_index == Other + urg_state_t::AMAX) {
316.
state->area_max =
atoi
(&buffer[5]);
317.
state->last = state->area_max;
318.
319.
}
else
if
(line_index == Other + urg_state_t::AFRT) {
320.
state->area_front =
atoi
(&buffer[5]);
321.
322.
}
else
if
(line_index == Other + urg_state_t::SCAN) {
323.
state->scan_rpm =
atoi
(&buffer[5]);
324.
}
325.
}
326.
327.
if
(line_index <= Other + urg_state_t::SCAN) {
328.
return
-1;
329.
}
330.
// Calculate the data size
331.
state->max_size = state->area_max +1;
332.
333.
return
0;
334.
}
335.
336.
337.
/*!
338.
\brief Connection to URG
339.
340.
\param state [o] Sensor information
341.
\param port [i] Device
342.
\param baudrate [i] Baudrate [bps]
343.
344.
\retval 0 Success
345.
\retval < 0 Error
346.
*/
347.
static
int
urg_connect(urg_state_t* state,
348.
const
char
* port,
const
long
baudrate)
349.
{
350.
static
char
message_buffer[LineLength];
351.
352.
if
(com_connect(port, baudrate) < 0) {
353.
_snprintf(message_buffer, LineLength,
354.
"Cannot connect COM device: %s"
, port);
355.
ErrorMessage = message_buffer;
356.
return
-1;
357.
}
358.
359.
const
long
try_baudrate[] = { 19200, 115200, 38400 };
360.
size_t
n =
sizeof
(try_baudrate) /
sizeof
(try_baudrate[0]);
361.
for
(
size_t
i = 0; i < n; ++i) {
362.
363.
// Search for the communicate able baud rate by trying different baud rate
364.
if
(com_changeBaudrate(try_baudrate[i])) {
365.
ErrorMessage =
"change baudrate fail."
;
366.
return
-1;
367.
}
368.
369.
// Change to SCIP2.0 mode
370.
int
recv_n = 0;
371.
urg_sendMessage(
"SCIP2.0"
, Timeout, &recv_n);
372.
if
(recv_n <= 0) {
373.
// If there is difference in baud rate value,then there will be no
374.
// response. So if there is no response, try the next baud rate.
375.
continue
;
376.
}
377.
378.
// If specified baudrate is different, then change the baudrate
379.
if
(try_baudrate[i] != baudrate) {
380.
urg_changeBaudrate(baudrate);
381.
382.
// Wait for SS command applied.
383.
delay(100);
384.
385.
com_changeBaudrate(baudrate);
386.
}
387.
388.
// Get parameter
389.
if
(urg_getParameters(state) < 0) {
390.
ErrorMessage =
391.
"PP command fail.\n"
392.
"This COM device may be not URG, or URG firmware is too old.\n"
393.
"SCIP 1.1 protocol is not supported. Please update URG firmware."
;
394.
return
-1;
395.
}
396.
state->last_timestamp = 0;
397.
398.
// success
399.
return
0;
400.
}
401.
402.
// fail
403.
ErrorMessage =
"no urg ports."
;
404.
return
-1;
405.
}
406.
407.
408.
/*!
409.
\brief Disconnection
410.
*/
411.
static
void
urg_disconnect(
void
)
412.
{
413.
com_disconnect();
414.
}
415.
416.
417.
/*!
418.
\brief Receive range data by using GD command
419.
420.
\param state[i] Sensor information
421.
422.
\retval 0 Success
423.
\retval < 0 Error
424.
*/
425.
static
int
urg_captureByGD(
const
urg_state_t* state)
426.
{
427.
char
send_message[LineLength];
428.
_snprintf(send_message, LineLength,
429.
"GD%04d%04d%02d"
, state->first, state->last, 1);
430.
431.
return
urg_sendTag(send_message);
432.
}
433.
434.
435.
/*!
436.
\brief Get range data by using MD command
437.
438.
\param state [i] Sensor information
439.
\param capture_times [i] capture times
440.
441.
\retval 0 Success
442.
\retval < 0 Error
443.
*/
444.
static
int
urg_captureByMD(
const
urg_state_t* state,
int
capture_times)
445.
{
446.
// 100 夞傪挻偊傞僨乕僞庢摼偵懳偟偰偼丄夞悢偵 00 (柍尷夞庢摼)傪巜掕偟丄
447.
// <a href="http://www.it165.net/pro/pkqt/" target="_blank" class="keylink">QT</a> or RS 僐儅儞僪偱僨乕僞庢摼傪掆巭偡傞偙偲
448.
if
(capture_times >= 100) {
449.
capture_times = 0;
450.
}
451.
452.
char
send_message[LineLength];
453.
_snprintf(send_message, LineLength,
"MD%04d%04d%02d%01d%02d"
,
454.
state->first, state->last, 1, 0, capture_times);
455.
456.
return
urg_sendTag(send_message);
457.
}
458.
459.
460.
// Decode 6bit data
461.
static
long
urg_decode(
const
char
data[],
int
data_byte)
462.
{
463.
long
value = 0;
464.
for
(
int
i = 0; i < data_byte; ++i) {
465.
value <<= 6;
466.
value &= ~0x3f;
467.
value |= data[i] - 0x30;
468.
}
469.
return
value;
470.
}
471.
472.
473.
// Receive range data
474.
static
int
urg_addRecvData(
const
char
buffer[],
long
data[],
int
* filled)
475.
{
476.
static
int
remain_byte = 0;
477.
static
char
remain_data[3];
478.
const
int
data_byte = 3;
479.
480.
const
char
* pre_p = buffer;
481.
const
char
* p = pre_p;
482.
483.
if
(*filled <= 0) {
484.
remain_byte = 0;
485.
}
486.
487.
if
(remain_byte > 0) {
488.
memmove
(&remain_data[remain_byte], buffer, data_byte - remain_byte);
489.
data[*filled] = urg_decode(remain_data, data_byte);
490.
++(*filled);
491.
pre_p = &buffer[data_byte - remain_byte];
492.
p = pre_p;
493.
remain_byte = 0;
494.
}
495.
496.
do
{
497.
++p;
498.
if
((p - pre_p) >=
static_cast
<
int
>(data_byte)) {
499.
data[*filled] = urg_decode(pre_p, data_byte);
500.
++(*filled);
501.
pre_p = p;
502.
}
503.
}
while
(*p !=
'\0'
);
504.
remain_byte = (
int
)(p - pre_p);
505.
memmove
(remain_data, pre_p, remain_byte);
506.
507.
return
0;
508.
}
509.
510.
511.
static
int
checkSum(
char
buffer[],
int
size,
char
actual_sum)
512.
{
513.
char
expected_sum = 0x00;
514.
int
i;
515.
516.
for
(i = 0; i < size; ++i) {
517.
expected_sum += buffer[i];
518.
}
519.
expected_sum = (expected_sum & 0x3f) + 0x30;
520.
521.
return
(expected_sum == actual_sum) ? 0 : -1;
522.
}
523.
524.
525.
/*!
526.
\brief Receive URG data
527.
528.
應掕僨乕僞傪攝楍偵奿擺偟丄奿擺僨乕僞悢傪栠傝抣偱曉偡丅
529.
530.
\param state [i] Sensor information
531.
\param data [o] range data
532.
\param max_size [i] range data buffer size
533.
534.
\retval >= 0 number of range data
535.
\retval < 0 Error
536.
*/
537.
static
int
urg_receiveData(urg_state_t* state,
long
data[],
size_t
max_size)
538.
{
539.
int
filled = 0;
540.
541.
// fill -1 from 0 to first
542.
for
(
int
i = state->first -1; i >= 0; --i) {
543.
data[filled++] = -1;
544.
}
545.
546.
char
message_type =
'M'
;
547.
char
buffer[LineLength];
548.
int
line_length;
549.
for
(
int
line_count = 0; (line_length = urg_readLine(buffer)) >= 0;
550.
++line_count) {
551.
552.
// check sum
553.
if
((line_count > 3) && (line_length >= 3)) {
554.
if
(checkSum(buffer, line_length - 1, buffer[line_length - 1]) < 0) {
555.
fprintf
(stderr,
"line_count: %d: %s\n"
, line_count, buffer);
556.
return
-1;
557.
}
558.
}
559.
560.
if
((line_count >= 6) && (line_length == 0)) {
561.
562.
// 僨乕僞庴怣偺姰椆
563.
for
(
size_t
i = filled; i < max_size; ++i) {
564.
// fill -1 to last of data buffer
565.
data[filled++] = -1;
566.
}
567.
return
filled;
568.
569.
}
else
if
(line_count == 0) {
570.
// 憲怣儊僢僙乕僕偺嵟弶偺暥帤偱儊僢僙乕僕偺敾掕傪峴偆
571.
if
((buffer[0] !=
'M'
) && (buffer[0] !=
'G'
)) {
572.
return
-1;
573.
}
574.
message_type = buffer[0];
575.
576.
}
else
if
(!
strncmp
(buffer,
"99b"
, 3)) {
577.
// "99b" 傪専弌偟丄埲崀傪乽僞僀儉僗僞儞僾乿乽僨乕僞乿偲傒側偡
578.
line_count = 4;
579.
580.
}
else
if
((line_count == 1) && (message_type ==
'G'
)) {
581.
line_count = 4;
582.
583.
}
else
if
(line_count == 4) {
584.
// "99b" 屌掕
585.
if
(
strncmp
(buffer,
"99b"
, 3)) {
586.
return
-1;
587.
}
588.
589.
}
else
if
(line_count == 5) {
590.
state->last_timestamp = urg_decode(buffer, 4);
591.
592.
}
else
if
(line_count >= 6) {
593.
// 庢摼僨乕僞
594.
if
(line_length > (64 + 1)) {
595.
line_length = (64 + 1);
596.
}
597.
buffer[line_length -1] =
'\0'
;
598.
int
ret = urg_addRecvData(buffer, data, &filled);
599.
if
(ret < 0) {
600.
return
ret;
601.
}
602.
}
603.
}
604.
return
-1;
605.
}
606.
607.
608.
void
outputData(
long
data[],
int
n,
size_t
total_index)
609.
{
610.
char
output_file[] =
"data_xxxxxxxxxx.csv"
;
611.
_snprintf(output_file,
sizeof
(output_file),
"data_%03d.csv"
, total_index);
612.
FILE
* fd =
fopen
(output_file,
"w"
);
613.
if
(! fd) {
614.
perror
(
"fopen"
);
615.
return
;
616.
}
617.
618.
for
(
int
i = 0; i < n; ++i) {
619.
fprintf
(fd,
"%ld, "
, data[i]);
620.
}
621.
fprintf
(fd,
"\n"
);
622.
623.
fclose
(fd);
624.
}
625.
626.
627.
int
main(
int
argc,
char
*argv[])
628.
{
629.
// COM 億乕僩愝掕
630.
// !!! 奺帺偺娐嫬偵崌傢偣偰 COM 愝掕傪曄峏偡傞偙偲
631.
const
char
com_port[] =
"COM10"
;
632.
const
long
com_baudrate = 115200;
633.
634.
// URG 偵愙懕
635.
urg_state_t urg_state;
636.
int
ret = urg_connect(&urg_state, com_port, com_baudrate);
637.
if
(ret < 0) {
638.
// 僄儔乕儊僢僙乕僕傪弌椡偟偰廔椆
639.
printf
(
"urg_connect: %s\n"
, ErrorMessage);
640.
641.
// 懄嵗偵廔椆偟側偄偨傔偺張棟丅晄梫側傜偽嶍彍偡傞偙偲
642.
getchar
();
643.
exit
(1);
644.
}
645.
646.
int
max_size = urg_state.max_size;
647.
long
* data =
new
long
[max_size];
648.
649.
enum
{ CaptureTimes = 5 };
650.
size_t
total_index = 0;
651.
652.
//
653.
// GD 僐儅儞僪傪梡偄偨僨乕僞庢摼
654.
printf
(
"using GD command\n"
);
655.
656.
// GD 僐儅儞僪偱偺僨乕僞庢摼偺応崌偵偼丄BM 僐儅儞僪偱偺儗乕僓揰摂偑昁梫
657.
int
recv_n = 0;
658.
urg_sendMessage(
"BM"
, Timeout, &recv_n);
659.
660.
for
(
int
i = 0; i < CaptureTimes; ++i) {
661.
urg_captureByGD(&urg_state);
662.
int
n = urg_receiveData(&urg_state, data, max_size);
663.
if
(n > 0) {
664.
printf
(
"% 3d: front: %ld, urg_timestamp: %ld\n"
,
665.
i, data[urg_state.area_front], urg_state.last_timestamp);
666.
667.
outputData(data, n, ++total_index);
668.
}
669.
}
670.
printf
(
"\n"
);
671.
672.
/
673.
// MD 僐儅儞僪傪梡偄偨僨乕僞庢摼
674.
printf
(
"using MD command\n"
);
675.
676.
urg_captureByMD(&urg_state, CaptureTimes);
677.
for
(
int
i = 0; i < CaptureTimes; ++i) {
678.
int
n = urg_receiveData(&urg_state, data, max_size);
679.
if
(n > 0) {
680.
printf
(
"% 3d: front: %ld, urg_timestamp: %ld\n"
,
681.
i, data[urg_state.area_front], urg_state.last_timestamp);
682.
683.
outputData(data, n, ++total_index);
684.
}
685.
}
686.
// MD 僐儅儞僪偱偺庢摼偑姰椆偡傞偲丄儗乕僓偼帺摦徚摂偡傞
687.
688.
// 偨偩偟丄100 夞埲忋偺僨乕僞庢摼傪巜掕偟偨応崌偵偼丄
689.
// urg_captureByMD() 撪晹偱柍尷夞偺僨乕僞庢摼偵愝掕偝傟偰偄傞偺偱丄
690.
// <a href="http://www.it165.net/pro/pkqt/" target="_blank" class="keylink">QT</a> 僐儅儞僪傪梡偄偰丄柧帵揑偵僨乕僞掆巭傪峴偆
691.
if
(CaptureTimes >= 100) {
692.
int
dummy;
693.
urg_sendMessage(
"QT"
, Timeout, &dummy);
694.
}
695.
696.
urg_disconnect();
697.
delete
[] data;
698.
699.
printf
(
"end.\n"
);
700.
701.
// 懄嵗偵廔椆偟側偄偨傔偺張棟丅晄梫側傜偽嶍彍偡傞偙偲
702.
getchar
();
703.
return
0;
704.
}
//激光雷达采集到数据的显示:
![\](https://i-blog.csdnimg.cn/blog_migrate/ed6da4c306c05c48ff9f8a65fcf1d481.jpeg)