model_blobfinder.cc
Go to the documentation of this file.
00001 
00002 //
00003 // File: model_blobfinder.c
00004 // Author: Richard Vaughan
00005 // Date: 10 June 2004
00006 //
00007 // CVS info:
00008 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_blobfinder.cc,v $
00009 //  $Author: rtv $
00010 //  $Revision$
00011 //
00013 
00014 //#define DEBUG 
00015 
00016 #include <sys/time.h>
00017 
00018 #include "stage.hh"
00019 #include "option.hh"
00020 #include "worldfile.hh"
00021 using namespace Stg;
00022 
00023 static const watts_t DEFAULT_BLOBFINDERWATTS = 2.0;
00024 static const meters_t DEFAULT_BLOBFINDERRANGE = 12.0;
00025 static const radians_t DEFAULT_BLOBFINDERFOV = M_PI/3.0;
00026 static const radians_t DEFAULT_BLOBFINDERPAN = 0.0;
00027 static const unsigned int DEFAULT_BLOBFINDERINTERVAL_MS = 100;
00028 static const unsigned int DEFAULT_BLOBFINDERRESOLUTION = 1;
00029 static const unsigned int DEFAULT_BLOBFINDERSCANWIDTH = 80;
00030 static const unsigned int DEFAULT_BLOBFINDERSCANHEIGHT = 60;
00031 
00032 
00082   ModelBlobfinder::ModelBlobfinder( World* world, 
00083                                                                                                 Model* parent,
00084                                                                                                 const std::string& type ) : 
00085   Model( world, parent, type ),
00086                                                 vis( world ),
00087                                                 blobs(),
00088                                                 colors(),
00089                                                 fov( DEFAULT_BLOBFINDERFOV ),
00090                                                 pan( DEFAULT_BLOBFINDERPAN ),
00091                                                 range( DEFAULT_BLOBFINDERRANGE ),
00092                                                 scan_height( DEFAULT_BLOBFINDERSCANHEIGHT ),
00093                                                 scan_width( DEFAULT_BLOBFINDERSCANWIDTH )
00094 {
00095   PRINT_DEBUG2( "Constructing ModelBlobfinder %d (%s)\n", 
00096                                          id, typestr ); 
00097   ClearBlocks();
00098   
00099   AddVisualizer( &this->vis, true );
00100 }
00101 
00102 
00103 ModelBlobfinder::~ModelBlobfinder( void )
00104 {
00105 }
00106 
00107 static bool blob_match( Model* candidate, 
00108                                                                 Model* finder,
00109                                                                 const void* dummy )
00110 { 
00111   (void)dummy; // avoid warning about unused var
00112 
00113   return( ! finder->IsRelated( candidate ));
00114 }       
00115 
00116 
00117 static bool ColorMatchIgnoreAlpha( Color a, Color b )
00118 {
00119   double epsilon = 1e-5; // small
00120   return( fabs(a.r - b.r) < epsilon &&
00121                   fabs(a.g - b.g) < epsilon &&
00122                   fabs(a.b - b.b) < epsilon );
00123 }
00124 
00125 void ModelBlobfinder::ModelBlobfinder::AddColor( Color col )
00126 {
00127         colors.push_back( col );
00128 }
00129 
00131 void ModelBlobfinder::RemoveColor( Color col )
00132 {
00133         FOR_EACH( it, colors )
00134                 {
00135                         if( (*it) ==  col  )
00136                                 it = colors.erase(it);
00137                 }
00138 }
00139 
00142 void ModelBlobfinder::RemoveAllColors()
00143 {
00144         colors.clear();
00145 }
00146 
00147 void ModelBlobfinder::Load( void )
00148 {  
00149   Model::Load();
00150   
00151   Worldfile* wf = world->GetWorldFile();
00152   
00153   wf->ReadTuple( wf_entity, "image", 0, 2, "uu", &scan_width, &scan_height );
00154 
00155   range = wf->ReadFloat( wf_entity, "range", range );
00156   fov = wf->ReadAngle( wf_entity, "fov", fov );
00157   pan = wf->ReadAngle( wf_entity, "pan", pan );
00158   
00159   if( wf->PropertyExists( wf_entity, "colors" ) )
00160     {
00161                 RemoveAllColors(); // empty the color list to start from scratch
00162 
00163                 unsigned int count = wf->ReadInt( wf_entity, "colors_count", 0 );
00164 
00165                 for( unsigned int c=0; c<count; c++ )
00166                   {
00167                     char* colorstr = NULL;
00168                     wf->ReadTuple( wf_entity, "colors", c, 1, "s", &colorstr );
00169                     
00170                     if( ! colorstr )
00171                       break;
00172                     else
00173                       AddColor( Color( colorstr ));
00174                   }
00175     }    
00176 }
00177 
00178 
00179 void ModelBlobfinder::Update( void )
00180 {     
00181         // generate a scan for post-processing into a blob image
00182         
00183         RaytraceResult* samples = new RaytraceResult[scan_width];
00184 
00185         Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width, false );
00186 
00187         // now the colors and ranges are filled in - time to do blob detection
00188         double yRadsPerPixel = fov / scan_height;
00189 
00190         blobs.clear();
00191 
00192         // scan through the samples looking for color blobs
00193         for(unsigned int s=0; s < scan_width; s++ )
00194           {
00195                  if( samples[s].mod == NULL  )
00196                         continue; // we saw nothing
00197                  
00198                  unsigned int right = s;
00199                  Color blobcol = samples[s].color;
00200                  
00201                  //printf( "blob start %d color %X\n", blobleft, blobcol );
00202                  
00203                  // loop until we hit the end of the blob
00204                  // there has to be a gap of >1 pixel to end a blob
00205                  // this avoids getting lots of crappy little blobs
00206                  while( s < scan_width && samples[s].mod && 
00207                                   ColorMatchIgnoreAlpha( samples[s].color, blobcol) )
00208                         {
00209                           //printf( "%u blobcol %X block %p %s color %X\n", s, blobcol, samples[s].block, samples[s].block->Model()->Token(), samples[s].block->Color() );
00210                           s++;
00211                         }
00212                  
00213                  unsigned int left = s - 1;
00214 
00215                 //if we have color filters in place, check to see if we're looking for this color
00216                 if( colors.size() )
00217                 {
00218                         bool found = false;
00219 
00220                         for( unsigned int c=0; c<colors.size(); c++ )
00221                                 if( ColorMatchIgnoreAlpha( blobcol, colors[c]))
00222                                 {
00223                                         found = true;
00224                                         break;
00225                                 }
00226                         if( ! found )
00227                                 continue; // continue scanning array for next blob
00228                 }
00229 
00230                 //printf( "blob end %d %X\n", blobright, blobcol );
00231 
00232                 double robotHeight = 0.6; // meters
00233 
00234                 // find the average range to the blob;
00235                 meters_t range = 0;
00236                 for( unsigned int t=right; t<=left; t++ )
00237                         range += samples[t].range;
00238                 range /= left-right + 1;
00239 
00240                 double startyangle = atan2( robotHeight/2.0, range );
00241                 double endyangle = -startyangle;
00242                 int blobtop = scan_height/2 - (int)(startyangle/yRadsPerPixel);
00243                 int blobbottom = scan_height/2 -(int)(endyangle/yRadsPerPixel);
00244 
00245                 blobtop = std::max( blobtop, 0 );
00246                 blobbottom = std::min( blobbottom, (int)scan_height );
00247 
00248                 // fill in an array entry for this blob
00249                 Blob blob;
00250                 blob.color = blobcol;
00251                 blob.left = scan_width - left - 1;
00252                 blob.top = blobtop;
00253                 blob.right = scan_width - right - 1;;
00254                 blob.bottom = blobbottom;
00255                 blob.range = range;
00256 
00257                 //printf( "Robot %p sees %d xpos %d ypos %d\n",
00258                 //  mod, blob.color, blob.xpos, blob.ypos );
00259 
00260                 // add the blob to our stash
00261                 //g_array_append_val( blobs, blob );
00262                 blobs.push_back( blob );
00263         }
00264 
00265         delete [] samples;
00266 
00267         Model::Update();
00268 }
00269 
00270 
00271 void ModelBlobfinder::Startup(  void )
00272 { 
00273         Model::Startup();
00274 
00275         PRINT_DEBUG( "blobfinder startup" );
00276 
00277         // start consuming power
00278         SetWatts( DEFAULT_BLOBFINDERWATTS );
00279 }
00280 
00281 void ModelBlobfinder::Shutdown( void )
00282 { 
00283 
00284         PRINT_DEBUG( "blobfinder shutdown" );
00285 
00286         // stop consuming power
00287         SetWatts( 0 );
00288 
00289         // clear the data - this will unrender it too
00290         blobs.clear();
00291 
00292         Model::Shutdown();
00293 }
00294 
00295 //******************************************************************************
00296 // visualization
00297 
00298 //TODO make instance attempt to register an option (as customvisualizations do)
00299 // Option ModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "", true, NULL );
00300 
00301 ModelBlobfinder::Vis::Vis( World* world ) 
00302   : Visualizer( "Blobfinder", "blobfinder_vis" )
00303 {
00304   
00305   //world->RegisterOption( &showArea );
00306   //world->RegisterOption( &showStrikes );
00307   //world->RegisterOption( &showFov );
00308   //world->RegisterOption( &showBeams );                  
00309 }
00310 
00311 void ModelBlobfinder::Vis::Visualize( Model* mod, Camera* cam )
00312 {
00313   ModelBlobfinder* bf( dynamic_cast<ModelBlobfinder*>(mod) );
00314   
00315   if( bf->debug )
00316         {
00317           // draw the FOV
00318           GLUquadric* quadric = gluNewQuadric();
00319           
00320           bf->PushColor( 0,0,0,0.2  );
00321           
00322           gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
00323           gluPartialDisk( quadric,
00324                                                         0, 
00325                                                         bf->range,
00326                                                         20, // slices   
00327                                                         1, // loops
00328                                                         rtod( M_PI/2.0 + bf->fov/2.0 - bf->pan), // start angle
00329                                                         rtod(-bf->fov) ); // sweep angle
00330           
00331           gluDeleteQuadric( quadric );
00332           bf->PopColor();
00333         }
00334   
00335   if( bf->subs < 1 )
00336          return;
00337   
00338   glPushMatrix();
00339 
00340         // return to global rotation frame
00341   Pose gpose( bf->GetGlobalPose() );
00342   glRotatef( rtod(-gpose.a),0,0,1 );
00343   
00344   // place the "screen" a little away from the robot
00345   glTranslatef( -2.5, -1.5, 0.5 );
00346   
00347   // rotate to face screen
00348   float yaw, pitch;
00349   pitch = - cam->pitch();
00350   yaw = - cam->yaw();
00351   float robotAngle = -rtod(bf->pose.a);
00352   glRotatef( robotAngle - yaw, 0,0,1 );
00353   glRotatef( -pitch, 1,0,0 );
00354   
00355   // convert blob pixels to meters scale - arbitrary
00356   glScalef( 0.025, 0.025, 1 );
00357   
00358   // draw a white screen with a black border
00359   bf->PushColor( 1,1,1,1 );
00360   glRectf( 0,0, bf->scan_width, bf->scan_height );
00361   bf->PopColor();
00362   
00363   glTranslatef(0,0,0.01 );
00364   
00365   glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
00366   bf->PushColor( 1,0,0,1 );
00367   glRectf( 0,0, bf->scan_width, bf->scan_height );
00368   bf->PopColor();
00369   glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
00370   
00371   // draw the blobs on the screen
00372   for( unsigned int s=0; s<bf->blobs.size(); s++ )
00373          {
00374                 Blob* b = &bf->blobs[s];
00375                 //blobfinder_blob_t* b = 
00376                 //&g_array_index( blobs, blobfinder_blob_t, s);
00377                 
00378                 bf->PushColor( b->color );
00379                 glRectf( b->left, b->top, b->right, b->bottom );
00380 
00381                 //printf( "%u l %u t%u r %u b %u\n", s, b->left, b->top, b->right, b->bottom );
00382                 bf->PopColor();
00383          }
00384   
00385   glPopMatrix();
00386 }
00387 
00388 


stage
Author(s): Richard Vaughan , Brian Gerkey , Reed Hedges , Andrew Howard , Toby Collett , Pooya Karimian , Jeremy Asher , Alex Couture-Beil , Geoff Biggs , Rich Mattes , Abbas Sadat
autogenerated on Thu Aug 27 2015 15:20:57