noAdjust.cpp

タイムスタンプを用いない場合の URG センサからのデータ処理

実環境(廊下)における実行例

/*
  URG センサからのデータ処理
  Satofumi KAMIMURA
  $Id$
*/

#include "mRunCtrl.h"
#include "mURGCtrl.h"
#include "vutils.h"
#include "drawCaptures.h"
#include "dumpPlotData.h"

using namespace VXV;


int main(int argc, char *argv[]) {
  try {
    mRunCtrl run;
    mURGCtrl urg;
    if ((initConnection(&run, argc, argv) < 0) ||
        (initConnection(&urg, argc, argv, true) < 0)) {
      exit(1);
    }
    urg.setOwnCrdToObject(&run);

    vmonitor::show();

    int capture_times = 0;
    int pre_times = urg.getCaptureTimes() - 1;

    run.rotateToDirection(deg(-90));
    while (!run.isStable()) {
      // 周囲の情報を取得
      int n = urg.capture();
      int times = urg.getCaptureTimes();
      if ((n > 0) && (times != pre_times)) {
        pre_times = times;
        urg.convert();
        drawCaptures(urg);
        outputPlotData(urg, argv[0]);

        ++capture_times;
      }
    }
    printf("captures: %d\n", capture_times);

    run.rotateToDirection(deg(0));
    VXV::waitStable(run, 100);
    VXV::Delay(1000);

    outputPlotFile(argv[0]);

  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}