激光雷达(一)数据采集C++

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线,主要是和电脑进行通讯的。另一根是电源/信号线,这里我们主要把它用作电源供电线。
激光雷达(一)数据采集C++_第1张图片
电源/信号线包含四根颜色不同的线,其中褐色的是+12V直流电源,蓝色的是0V地线。UTM-30LX要求电源至少提供700mA的供电电流。峰值电流可达1A。这里我们选择一个12V,1.5A的直流电源,将其电源线剪断,然后和褐色12VDC及蓝色的地线焊接在一起。UTM-30LX电源/信号线的另外两根不用,悬空即可。
这里读者一定要小心,千万不要将12V和地线接错了,一旦接错很可能烧毁设备!
激光雷达(一)数据采集C++_第2张图片
3). UTM-30LX驱动
将UTM-30LX电源接上,绿色和红色指示灯都亮了以后,将其USB线与电脑USB口相连。操作系统提示需要安装驱动,这里我们将生产商提供的驱动安装上即可。
http://www.hokuyo-aut.jp/02sensor/07scanner/download/data/URG_USB_DRIVER_Win.zip
驱动安装成功后,我们打开设备管理器,在Ports(COM&LPT)一栏中可以看到设备。将其串口号记下。
激光雷达(一)数据采集C++_第3张图片
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:  }

你可能感兴趣的:(C++)