您的位置：首页 > 其它

激光雷达学习笔记（一）数据采集

2015-04-08 15:40 302 查看
激光雷达或者叫激光测距仪数据采集比较简单，有位好心的网友提供了一篇博客专门讲这个，这里就不再赘述，贴出链接，需要的直接去看原文，激光雷达的型号：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）

view sourceprint?

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.
}


//激光雷达采集到数据的显示：

内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理 点击这里给我发消息
标签： 
相关文章推荐
新的分享
章节导航