senseOnly.cpp

/*
  環境を測定し、走行経路を表示
  Satofumi KAMIMURA
  $Id$
*/

#include "mURGCtrl.h"
#include "vutils.h"


// URG データの描画
static void drawURGSensed(URGCtrl& urg) {
  //vmonitor::drawPoints(urg.crd_points, White);
}


// コンフィグレーション空間の描画
static void drawConfigurationSpace(std::vector<VXV::Grid3D>& points) {
  for (std::vector<VXV::Grid3D>::iterator it = points.begin();
       it != points.end(); ++it) {
    //vmonitor::drawCircle(*it, 450, Cyan);
  }
}


int main(int argc, char *argv[]) {
  try {
    mURGCtrl urg;
    if (initConnection(&urg, argc, argv) < 0) {
      exit(1);
    }
    vmonitor::show();

    int pre_times = -1;
    while (true) {
      urg.capture();
      int times = urg.getCaptureTimes();
      if (times != pre_times) {
        pre_times = times;
        urg.convert();

        vmonitor::clear();
        drawConfigurationSpace(urg.crd_points);
        drawURGSensed(urg);

        // !!! 道路地図の生成
        // !!! 走行候補経路の強調表示
      }
      VXV::Delay(10);
    }
  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}