urbiballtrackinghead.cc

00001 /****************************************************************************
00002  * $Id: urbiballtrackinghead.cpp,v 1.7 2005/10/03 12:46:38 nottale Exp $
00003  *
00004  * Sample demonstration of URBI capabilities.
00005  *
00006  * Copyright (C) 2004, 2006, 2007 Jean-Christophe Baillie.  All rights reserved.
00007  *
00008  * This program is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * as published by the Free Software Foundation; either version 2
00011  * of the License, or (at your option) any later version.
00012  *
00013  * This program is distributed in the hope that it will be useful,
00014  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  * GNU General Public License for more details.
00017  *
00018  * You should have received a copy of the GNU General Public License
00019  * along with this program; if not, write to the Free Software
00020  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
00021 **********************************************************************/
00022 
00023 /*
00024  * This is a port to URBI of the Sony OPEN-R balltrackinghead example.
00025  * The algorithms used are as close as possible to the original version.
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 //inline double fabs(double a) {return  a>0?a:(a*-1);}
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   //callback functions
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; //joint value for the last few frames
00070   int baseframeX, baseframeY; //base of current_x, current_y (currentx[i] = frame_base - i)
00071   unsigned char  image[500000];  //uncompressed image
00072   double target_x, target_y;  //command to center the ball
00073   double expect_x, expect_y;  //values in last command, that should be reached
00074 
00075 
00076   void doSendCommand(double current_x, double current_y);
00077 
00078   static const int VSIZE = 10; //size of the joint position list.
00079   static const int ball_treshold = 50;
00080   static const double factor_x; //x field of view
00081   static const double factor_y;  //y field of view
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     std::vector<PositionData>::iterator it = find(current_x.begin(), current_x.end(), msg.timestamp/32);
00191     if (it==current_x.end())
00192     {
00193     return URBI_CONTINUE;
00194     }
00195     double cx = it->value;
00196     it = find(current_y.begin(), current_y.end(), msg.timestamp/32);
00197     if (it==current_y.end())
00198     {
00199     return URBI_CONTINUE;
00200     }
00201     double cy = it->value;
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   //get ball centroid
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     //, robotG (robotname)
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   //robotG.send("loop {pan: headPan.val& tilt: headTilt.val},");
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     //urbi::exit(1);
00322     argv[1]="127.0.0.1";
00323   }
00324 
00325   BallTrackingHead bt (argv[1]);
00326   // Help GCC understand we really want this variable to be "used".
00327   (void) bt;
00328   urbi::execute();
00329 }

Generated on Tue Apr 10 17:45:45 2007 for URBISDK by  doxygen 1.5.1