00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00013
00014
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;
00112
00113 return( ! finder->IsRelated( candidate ));
00114 }
00115
00116
00117 static bool ColorMatchIgnoreAlpha( Color a, Color b )
00118 {
00119 double epsilon = 1e-5;
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();
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
00182
00183 RaytraceResult* samples = new RaytraceResult[scan_width];
00184
00185 Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width, false );
00186
00187
00188 double yRadsPerPixel = fov / scan_height;
00189
00190 blobs.clear();
00191
00192
00193 for(unsigned int s=0; s < scan_width; s++ )
00194 {
00195 if( samples[s].mod == NULL )
00196 continue;
00197
00198 unsigned int right = s;
00199 Color blobcol = samples[s].color;
00200
00201
00202
00203
00204
00205
00206 while( s < scan_width && samples[s].mod &&
00207 ColorMatchIgnoreAlpha( samples[s].color, blobcol) )
00208 {
00209
00210 s++;
00211 }
00212
00213 unsigned int left = s - 1;
00214
00215
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;
00228 }
00229
00230
00231
00232 double robotHeight = 0.6;
00233
00234
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
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
00258
00259
00260
00261
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
00278 SetWatts( DEFAULT_BLOBFINDERWATTS );
00279 }
00280
00281 void ModelBlobfinder::Shutdown( void )
00282 {
00283
00284 PRINT_DEBUG( "blobfinder shutdown" );
00285
00286
00287 SetWatts( 0 );
00288
00289
00290 blobs.clear();
00291
00292 Model::Shutdown();
00293 }
00294
00295
00296
00297
00298
00299
00300
00301 ModelBlobfinder::Vis::Vis( World* world )
00302 : Visualizer( "Blobfinder", "blobfinder_vis" )
00303 {
00304
00305
00306
00307
00308
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
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,
00327 1,
00328 rtod( M_PI/2.0 + bf->fov/2.0 - bf->pan),
00329 rtod(-bf->fov) );
00330
00331 gluDeleteQuadric( quadric );
00332 bf->PopColor();
00333 }
00334
00335 if( bf->subs < 1 )
00336 return;
00337
00338 glPushMatrix();
00339
00340
00341 Pose gpose( bf->GetGlobalPose() );
00342 glRotatef( rtod(-gpose.a),0,0,1 );
00343
00344
00345 glTranslatef( -2.5, -1.5, 0.5 );
00346
00347
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
00356 glScalef( 0.025, 0.025, 1 );
00357
00358
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
00372 for( unsigned int s=0; s<bf->blobs.size(); s++ )
00373 {
00374 Blob* b = &bf->blobs[s];
00375
00376
00377
00378 bf->PushColor( b->color );
00379 glRectf( b->left, b->top, b->right, b->bottom );
00380
00381
00382 bf->PopColor();
00383 }
00384
00385 glPopMatrix();
00386 }
00387
00388