servoFreeDemo.cpp

/*
  URG センサからのデータ処理 (時刻印による位置を利用)
  Satofumi KAMIMURA
  $Id$
*/

#include "mRunCtrl.h"
#include "mURGCtrl.h"
#include "vutils.h"
#include "drawCaptures.h"
#include "dumpPlotData.h"
#include "timeUtils.h"
#include <SDL_thread.h>

using namespace VXV;

typedef struct {
  mURGCtrl* urg;
  SDL_mutex* urg_mutex;
  SDL_mutex* run_mutex;
  TicksPosition* ticks_pos;
  SDL_sem* sem;
  const char* progname;
} thread_args_t;


// URG データの取得スレッド
static int capture_thread(void* args) {
  thread_args_t* data = static_cast<thread_args_t*>(args);

  // 少し待ってみる
  SDL_Delay(1000);

  // URG データ取得とタイムスタンプを用いた位置補正
  while (SDL_SemValue(data->sem) == 0) {

    // URG データの取得
    SDL_LockMutex(data->urg_mutex);
    int n = data->urg->capture();
    SDL_UnlockMutex(data->urg_mutex);
    if (n > 0) {
      SDL_LockMutex(data->run_mutex);
      data->urg->convert(*data->ticks_pos);
      SDL_UnlockMutex(data->run_mutex);

      // データ描画
      drawCaptures(*data->urg);
      outputPlotData(*data->urg, data->progname);
    }

    SDL_Delay(10);
  }
  return 0;
}


int main(int argc, char *argv[]) {
  try {
    mRunCtrl run;
    mURGCtrl urg;

    // URG のデータ取得モードは、ManualCapture にしておく
    if ((initConnection(&run, argc, argv) < 0) ||
        (initConnection(&urg, argc, argv, false) < 0)) {
      exit(1);
    }
    run.set_recvTimeout(1000);

    // タイムスタンプ合わせ
    urg.setOwnCrdToObject(&run);
    run.adjustSubTreeTicks(GetTicks());
    size_t lastAdjust_ticks = GetTicks();

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

    // URG データ取得スレッドの起動
    thread_args_t args;
    args.ticks_pos = &ticksPos;
    args.urg = &urg;
    args.urg_mutex = SDL_CreateMutex();
    args.run_mutex = SDL_CreateMutex();
    args.sem = SDL_CreateSemaphore(0);
    args.progname = argv[0];
    SDL_Thread* thread = SDL_CreateThread(capture_thread, &args);

    vmonitor::show();

    // サーボフリー
    run.servoCtrl(false);

    enum {
      EvaluateSec = 20,         // 動作時間
    };
    size_t first_ticks = GetTicks();
    size_t add_times = 0;
    bool demo_moved = false;
    while ((GetTicks() - first_ticks) < EvaluateSec * 1000) {

      if ((! demo_moved) && (GetTicks() - first_ticks > 2000)) {
        // !!! 補正の動作確認用。２秒後に、動かしてみる
        run.rotateToDirection(deg(-90));
        demo_moved = true;
      }

      // 回転速度がゼロに近ければ、タイムスタンプあわせを行う
      // ただし、頻繁な修正を避けるため、前回から５秒以上経過したときのみ
      if ((GetTicks() - lastAdjust_ticks) > 5000) {
        VXV::Direction rotate_vel = run.getRotateVel();
        if (abs(rotate_vel.to_deg()) <= 0) {
          lastAdjust_ticks = GetTicks();

          SDL_LockMutex(args.urg_mutex);
          run.adjustSubTreeTicks(lastAdjust_ticks);
          SDL_UnlockMutex(args.urg_mutex);

          vmonitor::printf("adjust: %d\n", lastAdjust_ticks);
        }
      }

      // 走行位置の保存
      run.getRunPosition();
      SDL_LockMutex(args.run_mutex);
      ticksPos.add(run.crd_position, run.crd_ticks);
      SDL_UnlockMutex(args.run_mutex);
      ++add_times;

      SDL_Delay(10);
    }

    // 表示用ファイルの出力
    outputPlotFile(args.progname);

    printf("add_times: %d\n", add_times);

    // スレッドの終了
    SDL_SemPost(args.sem);
    SDL_WaitThread(thread, NULL);
    SDL_DestroyMutex(args.urg_mutex);
    SDL_DestroyMutex(args.run_mutex);
    SDL_DestroySemaphore(args.sem);

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