mdScan.cpp

Sample to get data using MD command

Author:

Satofumi KAMIMURA

Id:

mdScan.cpp 1737 2010-03-06 07:15:19Z satofumi

#include "mUrgDevice.h"
#include "delay.h"
#include "ticks.h"
#include <SDL.h>

using namespace qrk;
using namespace std;


int main(int argc, char *argv[])
{
  //const char device[] = "COM3";
  const char device[] = "/dev/ttyACM0";

  mUrgDevice urg(argc, argv);

  if (! urg.connect(device)) {
    printf("UrgDevice::connect: %s\n", urg.what());
    exit(1);
  }

#if 1
  // 取得モードを MD に設定
  urg.setCaptureMode(AutoCapture);

#else
  // 取得モードを ME に設定
  urg.setCaptureMode(IntensityCapture);
  urg.setCaptureSkipLines(2);
#endif
  int scan_msec = urg.scanMsec();

#if 0
  // 取得範囲を 中央から左右方向に 90度(計180度)に設定
  const double rad90 = 90.0 * M_PI / 180.0;
  urg.setCaptureRange(urg.rad2index(-rad90), urg.rad2index(rad90));
#endif

  // タイムスタンプを初期化するための呼び出し
  int pre_timestamp = ticks();

  // MD コマンドで連続データ取得を行い、指定回数だけ出力する
  enum { CaptureTimes = 10 };
  urg.setCaptureTimes(CaptureTimes);
  for (int i = 0; i < CaptureTimes;) {
    long timestamp = 0;
    vector<long> data;
    int n = urg.capture(data, &timestamp);
    if (n <= 0) {
      delay(scan_msec);
      continue;
    }

    printf("%03d: timestamp: %ld, (%ld), %ld\n",
           i, timestamp, ticks(), timestamp - pre_timestamp);
    pre_timestamp = timestamp;
#if 0
    for (int j = 0; j < n; ++j) {
      printf("%d:%ld, ", j, data[j]);
    }
    printf("\n");
#endif
    ++i;
  }
  return 0;
}