stopToObstacle.cpp

00001 /*
00002   障害物にて停止 (ログの使用例)
00003   Satofumi KAMIMURA
00004   $Id$
00005 */
00006 
00007 #include "mRunCtrl.h"
00008 #include "mURGCtrl.h"
00009 #include "vutils.h"
00010 #include <stdlib.h>
00011 
00012 enum {
00013   DangerAreaRadius = 600,
00014 };
00015 
00016 // 物体を検出したら停止する領域の円を描画
00017 static void drawDangerZone(const Position& pos) {
00018   static unsigned long pre_box_index = 0;
00019 
00020   vmonitor::clear(pre_box_index);
00021   pre_box_index = vmonitor::drawCircle(pos, DangerAreaRadius, Blue);
00022 }
00023 
00024 
00025 // 測定データ、筐体位置の描画
00026 static void drawInformation(mURGCtrl& urg, mRunCtrl& run, bool draw = true) {
00027   static TicksPosition ticksPos;
00028   static unsigned long pre_urg_index = 0;
00029 
00030   // 現在位置の取得、表示
00031   Position pos = run.getRunPosition();
00032   if (draw) {
00033     vmonitor::printf("position: %d, %d, %d[deg]\n",
00034                      pos.x, pos.y, pos.zt.to_deg());
00035   }
00036   ticksPos.add(run.crd_position, run.crd_ticks);
00037 
00038   // 判定領域の表示
00039   drawDangerZone(pos);
00040 
00041   // URG による周囲の情報を表示
00042   urg.convert(ticksPos);
00043   vmonitor::clear(pre_urg_index);
00044   pre_urg_index = vmonitor::drawPoints(urg.crd_points, White);
00045 
00046   ticksPos.del_olderThan(urg.crd_ticks - 5000);
00047 }
00048 
00049 
00050 // 筐体周辺に物体がなければ true を返す
00051 static bool isSafeArea(mURGCtrl& urg, int radius) {
00052   static unsigned long pre_points_index = 0;
00053 
00054   int first = urg.rad2index(-60 * DEG2RAD);
00055   int last = urg.rad2index(+60 * DEG2RAD);
00056   int step = (last > first) ? +1 : -1;
00057 
00058   std::vector<Grid3D> points;
00059   bool safe = true;
00060   for (int i = first; i != last; i += step) {
00061     int length = urg.length[i];
00062     if ((length > 20) && (length < radius)) {
00063       safe = false;
00064       int x = static_cast<int>(length * cos(urg.index2rad(i) +
00065                                             urg.crd_position.zt.to_rad()));
00066       int y = static_cast<int>(length * sin(urg.index2rad(i) +
00067                                             urg.crd_position.zt.to_rad()));
00068       Grid3D p = Grid3D(urg.crd_position.x + x, urg.crd_position.y + y, 0);
00069       points.push_back(p);
00070     }
00071   }
00072 
00073   vmonitor::clear(pre_points_index);
00074   if (!points.empty()) {
00075     pre_points_index = vmonitor::drawPoints(points, Red);
00076   }
00077   return safe;
00078 }
00079 
00080 
00081 // URG データの更新
00082 static bool captureUpdate(mURGCtrl& urg, mRunCtrl& run, bool draw = true) {
00083   if (urg.capture() <= 0) {
00084     return false;
00085   }
00086 
00087   drawInformation(urg, run, draw);
00088   return true;
00089 }
00090 
00091 
00092 // 到達目標の線を描画
00093 static void drawGoal(int length) {
00094   vmonitor::drawLine(Grid(length, -1000), Grid(length, +1000), Red);
00095 }
00096 
00097 
00098 int main(int argc, char *argv[]) {
00099   try {
00100     mRunCtrl run;
00101     mURGCtrl urg;
00102     if ((initConnection(&run, argc, argv) < 0) ||
00103         (initConnection(&urg, argc, argv, true) < 0)) {
00104       exit(1);
00105     }
00106     urg.setOwnCrdToObject(&run);
00107     run.adjustSubTreeTicks(0);
00108 
00109     vmonitor::show();
00110     //vmonitor::setTimeMagnify(0.5);
00111 
00112     // 一定距離以上、直進して停止
00113     enum {
00114       MoveLength = 2000,
00115       ObstacleTimeout = 5000,
00116     };
00117     vmonitor::printf("go straight over %d [mm]\n", MoveLength);
00118     drawGoal(MoveLength);
00119 
00120     bool quit = false;
00121     run.followLine(Position(0, 0, deg(0)));
00122     while (!quit && run.getLengthToLine(Position(MoveLength, 0, deg(0))) < 0) {
00123 
00124       // 周囲の情報を取得、表示
00125       if (!captureUpdate(urg, run)) {
00126         break;
00127       }
00128 
00129       // 障害物があったら一時停止。ただしタイムアウトあり
00130       if (!isSafeArea(urg, DangerAreaRadius)) {
00131         // 現在の移動コマンドを待避
00132         run.push_runState();
00133         run.stop();
00134 
00135         vmonitor::printf("detect obstacle!!!\n");
00136         unsigned long ticks = vmonitor::getTicks();
00137         do {
00138           VXV::Delay(100);
00139           captureUpdate(urg, run, false);
00140           if (vmonitor::getTicks() - ticks > ObstacleTimeout) {
00141             vmonitor::printf("obstacle timeout\n");
00142             quit = true;
00143             break;
00144           }
00145         } while (!isSafeArea(urg, DangerAreaRadius));
00146         // 待避しておいた移動コマンドを復帰して実行
00147         run.pop_runState();
00148         run.lastMoveCommand();
00149       }
00150       VXV::Delay(100);
00151     }
00152 
00153     // 停止して終了
00154     vmonitor::printf("stop & quit\n");
00155     run.stop();
00156     VXV::Delay(1000);
00157     vmonitor::clear(3);
00158 
00159     VXV::Delay(1000);
00160   } catch (std::exception& e) {
00161     printf("exception: %s\n", e.what());
00162   }
00163   return 0;
00164 }
00165 

Generated on Mon Apr 13 22:52:02 2009 by  doxygen 1.5.7.1