stopToObstacle.cpp

/*
  障害物にて停止 (ログの使用例)
  Satofumi KAMIMURA
  $Id$
*/

#include "mRunCtrl.h"
#include "mURGCtrl.h"
#include "vutils.h"
#include <stdlib.h>

enum {
  DangerAreaRadius = 600,
};

// 物体を検出したら停止する領域の円を描画
static void drawDangerZone(const Position& pos) {
  static unsigned long pre_box_index = 0;

  vmonitor::clear(pre_box_index);
  pre_box_index = vmonitor::drawCircle(pos, DangerAreaRadius, Blue);
}


// 測定データ、筐体位置の描画
static void drawInformation(mURGCtrl& urg, mRunCtrl& run, bool draw = true) {
  static TicksPosition ticksPos;
  static unsigned long pre_urg_index = 0;

  // 現在位置の取得、表示
  Position pos = run.getRunPosition();
  if (draw) {
    vmonitor::printf("position: %d, %d, %d[deg]\n",
                     pos.x, pos.y, pos.zt.to_deg());
  }
  ticksPos.add(run.crd_position, run.crd_ticks);

  // 判定領域の表示
  drawDangerZone(pos);

  // URG による周囲の情報を表示
  urg.convert(ticksPos);
  vmonitor::clear(pre_urg_index);
  pre_urg_index = vmonitor::drawPoints(urg.crd_points, White);

  ticksPos.del_olderThan(urg.crd_ticks - 5000);
}


// 筐体周辺に物体がなければ true を返す
static bool isSafeArea(mURGCtrl& urg, int radius) {
  static unsigned long pre_points_index = 0;

  int first = urg.rad2index(-60 * DEG2RAD);
  int last = urg.rad2index(+60 * DEG2RAD);
  int step = (last > first) ? +1 : -1;

  std::vector<Grid3D> points;
  bool safe = true;
  for (int i = first; i != last; i += step) {
    int length = urg.length[i];
    if ((length > 20) && (length < radius)) {
      safe = false;
      int x = static_cast<int>(length * cos(urg.index2rad(i) +
                                            urg.crd_position.zt.to_rad()));
      int y = static_cast<int>(length * sin(urg.index2rad(i) +
                                            urg.crd_position.zt.to_rad()));
      Grid3D p = Grid3D(urg.crd_position.x + x, urg.crd_position.y + y, 0);
      points.push_back(p);
    }
  }

  vmonitor::clear(pre_points_index);
  if (!points.empty()) {
    pre_points_index = vmonitor::drawPoints(points, Red);
  }
  return safe;
}


// URG データの更新
static bool captureUpdate(mURGCtrl& urg, mRunCtrl& run, bool draw = true) {
  if (urg.capture() <= 0) {
    return false;
  }

  drawInformation(urg, run, draw);
  return true;
}


// 到達目標の線を描画
static void drawGoal(int length) {
  vmonitor::drawLine(Grid(length, -1000), Grid(length, +1000), Red);
}


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();
    //vmonitor::setTimeMagnify(0.5);

    // 一定距離以上、直進して停止
    enum {
      MoveLength = 2000,
      ObstacleTimeout = 5000,
    };
    vmonitor::printf("go straight over %d [mm]\n", MoveLength);
    drawGoal(MoveLength);

    bool quit = false;
    run.followLine(Position(0, 0, deg(0)));
    while (!quit && run.getLengthToLine(Position(MoveLength, 0, deg(0))) < 0) {

      // 周囲の情報を取得、表示
      if (!captureUpdate(urg, run)) {
        break;
      }

      // 障害物があったら一時停止。ただしタイムアウトあり
      if (!isSafeArea(urg, DangerAreaRadius)) {
        // 現在の移動コマンドを待避
        run.push_runState();
        run.stop();

        vmonitor::printf("detect obstacle!!!\n");
        unsigned long ticks = vmonitor::getTicks();
        do {
          VXV::Delay(100);
          captureUpdate(urg, run, false);
          if (vmonitor::getTicks() - ticks > ObstacleTimeout) {
            vmonitor::printf("obstacle timeout\n");
            quit = true;
            break;
          }
        } while (!isSafeArea(urg, DangerAreaRadius));
        // 待避しておいた移動コマンドを復帰して実行
        run.pop_runState();
        run.lastMoveCommand();
      }
      VXV::Delay(100);
    }

    // 停止して終了
    vmonitor::printf("stop & quit\n");
    run.stop();
    VXV::Delay(1000);
    vmonitor::clear(3);

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