simuPosUse.cpp

/*
  URG センサからのデータ処理 (モニタ用位置を利用)
  Satofumi KAMIMURA
  $Id$
*/

#include "mRunCtrl.h"
#include "mURGCtrl.h"
#include "vutils.h"
#include "drawCaptures.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);
    run.adjustSubTreeTicks(0);

    vmonitor::show();

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

    run.rotateToDirection(deg(-90));
    while (!run.isStable()) {
      // 周囲の情報を取得
      urg.capture();
      int times = urg.getCaptureTimes();
      if (times != pre_times) {
        pre_times = times;
        urg.convert(run.ticksPos); // シミュレータ位置を用いる
        drawCaptures(urg);

        ++capture_times;
      }
    }

    // 表示用ファイルの出力
    printf("captures: %d\n", capture_times);

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

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