00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include <cmath>
00028
00029 #include <vector>
00030
00031 #include "urbi/uclient.hh"
00032
00033 #ifndef LIBURBI_OPENR
00034 # include "monitor.h"
00035 Monitor* mon = NULL;
00036 #endif
00037
00038
00039 inline double fsgn (double a)
00040 {
00041 return a>0?1:-1;
00042 }
00043
00044 struct PositionData
00045 {
00046 int frame;
00047 double value;
00048 bool operator == (int f) {return f==frame;}
00049 };
00050
00051
00052 class BallTrackingHead
00053 {
00054 public:
00055 BallTrackingHead(const char * robotName);
00056
00057
00058 urbi::UCallbackAction getHead(bool pan, const urbi::UMessage &msg);
00059 urbi::UCallbackAction getImage(const urbi::UMessage &msg);
00060
00061 private:
00063 urbi::UClient robotI;
00065 urbi::UClient robotC;
00068
00069 std::vector<PositionData> current_x, current_y;
00070 int baseframeX, baseframeY;
00071 unsigned char image[500000];
00072 double target_x, target_y;
00073 double expect_x, expect_y;
00074
00075
00076 void doSendCommand(double current_x, double current_y);
00077
00078 static const int VSIZE = 10;
00079 static const int ball_treshold = 50;
00080 static const double factor_x;
00081 static const double factor_y;
00082 static const double maxcommand_x;
00083 static const double maxcommand_y;
00084 };
00085
00086 const double BallTrackingHead::factor_x = 0.9 * 56.9;
00087 const double BallTrackingHead::factor_y = 0.9 * 45.2;
00088 const double BallTrackingHead::maxcommand_x = 150.0;
00089 const double BallTrackingHead::maxcommand_y = 150.0;
00090
00091 int format=1;
00092
00093 void BallTrackingHead::doSendCommand(double current_x, double current_y)
00094 {
00095 static int sframe=0;
00096 static unsigned int stime=0;
00097 double command_x=-1, command_y=-1;
00098 robotC.send("headPan.val = headPan.val + %lf & headTilt.val = headTilt.val + %lf,",target_x,target_y);
00099 if (! (sframe % 1000))
00100 {
00101 if (stime)
00102 robotC.printf("!! csps %f\n",
00103 1000000.0/(float)(robotC.getCurrentTime()-stime));
00104 stime=robotC.getCurrentTime();
00105 }
00106 ++sframe;
00107 return;
00108 if (fabs(current_x-expect_x)< 100)
00109 {
00110 if (fabs(target_x - current_x) > maxcommand_x)
00111 command_x = current_x + maxcommand_x*fsgn(target_x - current_x);
00112 else
00113 command_x = target_x;
00114 if (fabs(command_x-current_x) > 0.0)
00115 {
00116 robotC.send("headPan.val = %lf,",command_x);
00117 expect_x = command_x;
00118 }
00119 else
00120 command_x = -1;
00121 }
00122 if (fabs(current_y-expect_y)< 100)
00123 {
00124 if (fabs(target_y - current_y) > maxcommand_y)
00125 command_y = current_y + maxcommand_y*fsgn(target_y - current_y);
00126 else
00127 command_y = target_y;
00128 if (fabs(command_y-current_y) > 0.0)
00129 {
00130 robotC.send("headTilt.val = %lf,",command_y);
00131 expect_y = command_y;
00132 }
00133 else
00134 command_y = -1;
00135 }
00136
00137 if (command_x!=-1 || command_y!=-1)
00138 {
00139 if (! (sframe % 1000))
00140 {
00141 if (stime)
00142 robotC.printf("!! csps %f\n",
00143 1000000.0/(float)(robotC.getCurrentTime()-stime));
00144 stime=robotC.getCurrentTime();
00145 }
00146 ++sframe;
00147 }
00148 }
00149
00150
00151 urbi::UCallbackAction
00152 BallTrackingHead::getHead(bool pan, const urbi::UMessage &msg)
00153 {
00154 if (msg.type != urbi::MESSAGE_DATA
00155 || msg.value->type != urbi::DATA_DOUBLE)
00156 return urbi::URBI_CONTINUE;
00157
00158 PositionData pd;
00159 pd.frame = msg.timestamp/32;
00160 pd.value = msg.value->val;
00161 if (pan)
00162 {
00163 current_x.insert(current_x.begin(), pd);
00164 current_x.resize(VSIZE);
00165 }
00166 else
00167 {
00168 current_y.insert(current_y.begin(), pd);
00169 current_y.resize(VSIZE);
00170 }
00171
00172 return urbi::URBI_CONTINUE;
00173 }
00174
00175 urbi::UCallbackAction
00176 BallTrackingHead::getImage(const urbi::UMessage &msg)
00177 {
00178 static int framenum=0;
00179 static float interframe=0;
00180 static int frametime=0;
00181
00182 if (msg.type != urbi::MESSAGE_DATA
00183 || msg.value->type != urbi::DATA_BINARY
00184 || msg.value->binary->type != urbi::BINARY_IMAGE)
00185 return urbi::URBI_CONTINUE;
00186
00187 urbi::UImage& img = msg.value->binary->image;
00188 double cx=0, cy=0;
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203 if ((framenum % 50)==0)
00204 {
00205 if (!frametime)
00206 frametime=robotC.getCurrentTime();
00207 else
00208 {
00209 int dt=robotC.getCurrentTime()-frametime;
00210 frametime=robotC.getCurrentTime();
00211 if (interframe == 0)
00212 interframe=((float)dt)/50.0;
00213 else
00214 interframe=interframe*0.5 + 0.5*((float)dt)/50.0;
00215 robotC.printf("## %f fps\n",1000.0/interframe);
00216 }
00217 }
00218 ++framenum;
00219 int imgsize = 500000;
00220 if (img.imageFormat == urbi::IMAGE_JPEG)
00221 urbi::convertJPEGtoYCrCb((const urbi::byte *) img.data,
00222 img.size, image, imgsize);
00223 else
00224 memcpy(image, img.data, img.width * img.height * 3);
00225
00226
00227
00228 int xsum=0, ysum=0;
00229 int nummatch=0;
00230 int w = img.width;
00231 int h = img.height;
00232 for (unsigned i=0;i<img.width; ++i)
00233 for (unsigned j=0;j<img.height; ++j)
00234 {
00235 unsigned char cb = image[(i+j*w)*3+1];
00236 unsigned char cr = image[(i+j*w)*3+2];;
00237 if (150 <= cr && cr<=230
00238 && 120 <= cb && cb<=190)
00239 {
00240 ++nummatch;
00241 xsum+=i;
00242 ysum+=j;
00243 }
00244 }
00245 if (nummatch >= ball_treshold)
00246 {
00247 double bx= (double)xsum / (double)nummatch;
00248 double by= (double)ysum / (double)nummatch;
00249 double dbx = bx - (double)w / 2.0;
00250 double dby = by - (double)h / 2.0;
00251
00252 double dx = (-1.0) * (factor_x / (double)w) * dbx;
00253 double dy = (-1.0) * (factor_y / (double)h) * dby;
00254
00255 #ifndef LIBURBI_OPENR
00256 for (int j=0;j<h; ++j)
00257 image[(((int)bx)+w*j)*3]=255;
00258 for (int j=0;j<w; ++j)
00259 image[(((int)by)*w+j)*3]=255;
00260 #endif
00261
00262 target_x = cx+dx;
00263 target_y = cy+dy;
00264 if (target_x > 90.0)
00265 target_x = 90.0;
00266 if (target_x < -90.0)
00267 target_x = -90.0;
00268 if (target_y > 60.0)
00269 target_y = 60.0;
00270 if (target_y < -30.0)
00271 target_y = -30.0;
00272 doSendCommand(cx, cy);
00273
00274 }
00275
00276 #ifndef LIBURBI_OPENR
00277 urbi::convertYCrCbtoRGB(image, w*h*3, image);
00278 if (!mon)
00279 mon = new Monitor(w, h, "Image");
00280 mon->setImage((bits8*)image, img.width*img.height*3);
00281 #endif
00282
00283 return urbi::URBI_CONTINUE;
00284 }
00285
00286
00287 BallTrackingHead::BallTrackingHead(const char * robotname)
00288 : robotI (robotname),
00289 robotC (robotname)
00290
00291 {
00292 robotI.start();
00293 if (robotI.error())
00294 urbi::exit(1);
00295 robotC.start();
00296 if (robotC.error())
00297 urbi::exit(1);
00298
00299
00300 robotC.send("motoron;");
00301 robotC.send("camera.format = 1;");
00302 #ifdef LIBURBI_OPENR
00303 robotC.send("camera.resolution = 1;");
00304 #else
00305 robotC.send("camera.resolution = 0;");
00306 #endif
00307 robotC.send("camera.jpegfactor = 75;");
00308 robotC.setCallback(*this, &BallTrackingHead::getImage,"cam");
00309
00310 robotC.send("loop cam: camera.val, ");
00311
00312 }
00313
00314
00315 int
00316 main(int argc, char * argv[])
00317 {
00318 if (argc != 2)
00319 {
00320 printf("usage: %s robotname\n", argv[0]);
00321
00322 argv[1]="127.0.0.1";
00323 }
00324
00325 BallTrackingHead bt (argv[1]);
00326
00327 (void) bt;
00328 urbi::execute();
00329 }