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

激光雷达或者叫激光测距仪数据采集比较简单，有位好心的网友提供了一篇博客专门讲这个，这里就不再赘述，贴出链接，需要的直接去看原文，激光雷达的型号：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
028. #include
029. #include
030. #include
031. #include
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. // QT 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. // QT 僐儅儞僪傪梡偄偰丄柧帵揑偵僨乕僞掆巭傪峴偆
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. }

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