激光雷达学习笔记(一)数据采集

 

激光雷达学习笔记(一)数据采集

标签: 激光雷达
  6740人阅读  评论(7)  收藏  举报
  分类:
激光雷达(5) 

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

激光雷达数据采集程序:(UTM-30LX)

  

[cpp]  view plain  copy
  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   
  28. #include   
  29. #include   
  30. #include   
  31. #include   
  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. }  

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

激光雷达学习笔记(一)数据采集_第1张图片


你可能感兴趣的:(050,算法)