1). UTM-30LX激光测距仪相关资料
网页
http://www.hokuyo-aut.jp/02sensor/07scanner/utm_30lx.html
下载资源
http://www.hokuyo-aut.jp/02sensor/07scanner/download/index.html#common_section
windows驱动
http://www.hokuyo-aut.jp/02sensor/07scanner/download/data/URG_USB_DRIVER_Win.zip
程序示例
http://www.hokuyo-aut.jp/02sensor/07scanner/download/urg_programs_en/samples/capture_sample.cpp
2). UTM-30LX供电和USB链接
UTM-30LX有两根比较粗的黑色连线。一根是USB线,主要是和电脑进行通讯的。另一根是电源/信号线,这里我们主要把它用作电源供电线。
电源/信号线包含四根颜色不同的线,其中褐色的是+12V直流电源,蓝色的是0V地线。UTM-30LX要求电源至少提供700mA的供电电流。峰值电流可达1A。这里我们选择一个12V,1.5A的直流电源,将其电源线剪断,然后和褐色12VDC及蓝色的地线焊接在一起。UTM-30LX电源/信号线的另外两根不用,悬空即可。
这里读者一定要小心,千万不要将12V和地线接错了,一旦接错很可能烧毁设备!
3). UTM-30LX驱动
将UTM-30LX电源接上,绿色和红色指示灯都亮了以后,将其USB线与电脑USB口相连。操作系统提示需要安装驱动,这里我们将生产商提供的驱动安装上即可。
http://www.hokuyo-aut.jp/02sensor/07scanner/download/data/URG_USB_DRIVER_Win.zip
驱动安装成功后,我们打开设备管理器,在Ports(COM&LPT)一栏中可以看到设备。将其串口号记下。
4). UTM-30LX编程
打开Visual Studio,新建一个win32控制台工程,将capture_sample.cpp代码复制过去。记住将const char* com_port = "COM17";改为读者先前记下的串口号。这里可能有涉及到UNICODE的编译错误,读者可参考如下文章 http://blog.sina.com.cn/s/blog_6e0693f70100t1yg.html 修改。编译,连接后,调试程序。本程序将测量UTM-30LX正前方处物体的距离。
5). capture_sample.cpp代码
1: /*!
2: \file
3: \brief Sample to get URG data using Win32
4:
5: \author Satofumi KAMIMURA
6:
7: $Id: capture_sample.cpp 1724 2010-02-25 10:43:11Z satofumi $
8:
9: Compling and execute process
10: - In case of Visual Studio
11: - Select capture_sample.sln from capture_sample.zip
12: - When Visual Studio is started, press F5 to build and execute.
13: - If COM port is not found, then change the com_port in main function.
14:
15: - In case of MinGW, Cygwin
16: - % g++ capture_sample.cpp -o capture_sample
17: - % ./capture_sample
18: - If COM port is not found, then change the com_port in main function.
19:
20: \attention Change com_port, com_baudrate values in main() with relevant values.
21: \attention We are not responsible for any loss or damage occur by using this program
22: \attention We appreciate the suggestions and bug reports
23: */
24:
25: #define _CRT_SECURE_NO_WARNINGS
26:
27: #include <windows.h>
28: #include <cstdio>
29: #include <cstdlib>
30: #include <cstring>
31: #include <string>
32:
33: using namespace std;
34:
35:
36: // To record the output of SCIP,define RAW_OUTPUT
37: //#define RAW_OUTPUT
38:
39: #if defined(RAW_OUTPUT)
40: static FILE* Raw_fd_ = NULL;
41: #endif
42:
43:
44: enum {
45: Timeout = 1000, // [msec]
46: EachTimeout = 2, // [msec]
47: LineLength = 64 + 3 + 1 + 1 + 1 + 16,
48: };
49:
50: static HANDLE HCom = INVALID_HANDLE_VALUE;
51: static int ReadableSize = 0;
52: static char* ErrorMessage = "no error.";
53:
54:
55: /*!
56: \brief Manage sensor information
57: */
58: typedef struct
59: {
60: enum {
61: MODL = 0, //!< Sensor model information
62: DMIN, //!< Minimum measurable distance [mm]
63: DMAX, //!< Maximum measurable distance [mm]
64: ARES, //!< Angle of resolution
65: AMIN, //!< Minimum measurable area
66: AMAX, //!< Maximum measurable area
67: AFRT, //!< Front direction value
68: SCAN, //!< Standard angular velocity
69: };
70: string model; //!< Obtained MODL information
71: long distance_min; //!< Obtained DMIN information
72: long distance_max; //!< Obtained DMAX information
73: int area_total; //!< Obtained ARES information
74: int area_min; //!< Obtained AMIN information
75: int area_max; //!< Obtained AMAX information
76: int area_front; //!< Obtained AFRT information
77: int scan_rpm; //!< Obtained SCAN information
78:
79: int first; //!< Starting position of measurement
80: int last; //!< End position of measurement
81: int max_size; //!< Maximum size of data
82: long last_timestamp; //!< Time stamp when latest data is obtained
83: } urg_state_t;
84:
85:
86: // Delay
87: static void delay(int msec)
88: {
89: Sleep(msec);
90: }
91:
92:
93: static int com_changeBaudrate(long baudrate)
94: {
95: DCB dcb;
96:
97: GetCommState(HCom, &dcb);
98: dcb.BaudRate = baudrate;
99: 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: }