鐳射雷達學習筆記（一）資料採集
阿新 • • 發佈：2019-01-08
鐳射雷達或者叫鐳射測距儀資料採集比較簡單，有位好心的網友提供了一篇部落格專門講這個，這裡就不再贅述，貼出連結，需要的直接去看原文，鐳射雷達的型號：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.