ticksPosUse.cpp

/*
  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);
    run.adjustSubTreeTicks(0);

    vmonitor::show();

    // 位置と時刻を記録して、指定時刻の位置を補間して返すクラス
    TicksPosition ticksPos;

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

    run.getRunPosition();
    ticksPos.add(run.crd_position, run.crd_ticks);
    ++add_times;
    run.rotateToDirection(deg(-90));
    while (!run.isStable()) {
      // 走行位置の保存
      run.getRunPosition();
      ticksPos.add(run.crd_position, run.crd_ticks);
      ++add_times;

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

        ticksPos.add(run.crd_position, run.crd_ticks);

        ++capture_times;
      }
    }

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

    //run.stop();
    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;
}