model_blobfinder.cc

Go to the documentation of this file.

//
// File: model_blobfinder.c
// Author: Richard Vaughan
// Date: 10 June 2004
//
// CVS info:
//  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_blobfinder.cc,v $
//  $Author: rtv $
//  $Revision$
//

//#define DEBUG 

#include <sys/time.h>

#include "stage.hh"
#include "option.hh"
#include "worldfile.hh"
using namespace Stg;

static const watts_t DEFAULT_BLOBFINDERWATTS = 2.0;
static const meters_t DEFAULT_BLOBFINDERRANGE = 12.0;
static const radians_t DEFAULT_BLOBFINDERFOV = M_PI/3.0;
static const radians_t DEFAULT_BLOBFINDERPAN = 0.0;
static const unsigned int DEFAULT_BLOBFINDERINTERVAL_MS = 100;
static const unsigned int DEFAULT_BLOBFINDERRESOLUTION = 1;
static const unsigned int DEFAULT_BLOBFINDERSCANWIDTH = 80;
static const unsigned int DEFAULT_BLOBFINDERSCANHEIGHT = 60;


  ModelBlobfinder::ModelBlobfinder( World* world, 
                                                                                                Model* parent,
                                                                                                const std::string& type ) : 
  Model( world, parent, type ),
                                                vis( world ),
                                                blobs(),
                                                colors(),
                                                fov( DEFAULT_BLOBFINDERFOV ),
                                                pan( DEFAULT_BLOBFINDERPAN ),
                                                range( DEFAULT_BLOBFINDERRANGE ),
                                                scan_height( DEFAULT_BLOBFINDERSCANHEIGHT ),
                                                scan_width( DEFAULT_BLOBFINDERSCANWIDTH )
{
  PRINT_DEBUG2( "Constructing ModelBlobfinder %d (%s)\n", 
                                         id, typestr ); 
  ClearBlocks();
  
  AddVisualizer( &this->vis, true );
}


ModelBlobfinder::~ModelBlobfinder( void )
{
}

static bool blob_match( Model* candidate, 
                                                                Model* finder,
                                                                const void* dummy )
{ 
  (void)dummy; // avoid warning about unused var

  return( ! finder->IsRelated( candidate ));
}       


static bool ColorMatchIgnoreAlpha( Color a, Color b )
{
  double epsilon = 1e-5; // small
  return( fabs(a.r - b.r) < epsilon &&
                  fabs(a.g - b.g) < epsilon &&
                  fabs(a.b - b.b) < epsilon );
}

void ModelBlobfinder::ModelBlobfinder::AddColor( Color col )
{
        colors.push_back( col );
}

void ModelBlobfinder::RemoveColor( Color col )
{
        FOR_EACH( it, colors )
                {
                        if( (*it) ==  col  )
                                it = colors.erase(it);
                }
}

void ModelBlobfinder::RemoveAllColors()
{
        colors.clear();
}

void ModelBlobfinder::Load( void )
{  
  Model::Load();
  
  Worldfile* wf = world->GetWorldFile();
  
  wf->ReadTuple( wf_entity, "image", 0, 2, "uu", &scan_width, &scan_height );

  range = wf->ReadFloat( wf_entity, "range", range );
  fov = wf->ReadAngle( wf_entity, "fov", fov );
  pan = wf->ReadAngle( wf_entity, "pan", pan );
  
  if( wf->PropertyExists( wf_entity, "colors" ) )
    {
                RemoveAllColors(); // empty the color list to start from scratch

                unsigned int count = wf->ReadInt( wf_entity, "colors_count", 0 );

                for( unsigned int c=0; c<count; c++ )
                  {
                    char* colorstr = NULL;
                    wf->ReadTuple( wf_entity, "colors", c, 1, "s", &colorstr );
                    
                    if( ! colorstr )
                      break;
                    else
                      AddColor( Color( colorstr ));
                  }
    }    
}


void ModelBlobfinder::Update( void )
{     
        // generate a scan for post-processing into a blob image
        
        RaytraceResult* samples = new RaytraceResult[scan_width];

        Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width, false );

        // now the colors and ranges are filled in - time to do blob detection
        double yRadsPerPixel = fov / scan_height;

        blobs.clear();

        // scan through the samples looking for color blobs
        for(unsigned int s=0; s < scan_width; s++ )
          {
                 if( samples[s].mod == NULL  )
                        continue; // we saw nothing
                 
                 unsigned int right = s;
                 Color blobcol = samples[s].color;
                 
                 //printf( "blob start %d color %X\n", blobleft, blobcol );
                 
                 // loop until we hit the end of the blob
                 // there has to be a gap of >1 pixel to end a blob
                 // this avoids getting lots of crappy little blobs
                 while( s < scan_width && samples[s].mod && 
                                  ColorMatchIgnoreAlpha( samples[s].color, blobcol) )
                        {
                          //printf( "%u blobcol %X block %p %s color %X\n", s, blobcol, samples[s].block, samples[s].block->Model()->Token(), samples[s].block->Color() );
                          s++;
                        }
                 
                 unsigned int left = s - 1;

                //if we have color filters in place, check to see if we're looking for this color
                if( colors.size() )
                {
                        bool found = false;

                        for( unsigned int c=0; c<colors.size(); c++ )
                                if( ColorMatchIgnoreAlpha( blobcol, colors[c]))
                                {
                                        found = true;
                                        break;
                                }
                        if( ! found )
                                continue; // continue scanning array for next blob
                }

                //printf( "blob end %d %X\n", blobright, blobcol );

                double robotHeight = 0.6; // meters

                // find the average range to the blob;
                meters_t range = 0;
                for( unsigned int t=right; t<=left; t++ )
                        range += samples[t].range;
                range /= left-right + 1;

                double startyangle = atan2( robotHeight/2.0, range );
                double endyangle = -startyangle;
                int blobtop = scan_height/2 - (int)(startyangle/yRadsPerPixel);
                int blobbottom = scan_height/2 -(int)(endyangle/yRadsPerPixel);

                blobtop = std::max( blobtop, 0 );
                blobbottom = std::min( blobbottom, (int)scan_height );

                // fill in an array entry for this blob
                Blob blob;
                blob.color = blobcol;
                blob.left = scan_width - left - 1;
                blob.top = blobtop;
                blob.right = scan_width - right - 1;;
                blob.bottom = blobbottom;
                blob.range = range;

                //printf( "Robot %p sees %d xpos %d ypos %d\n",
                //  mod, blob.color, blob.xpos, blob.ypos );

                // add the blob to our stash
                //g_array_append_val( blobs, blob );
                blobs.push_back( blob );
        }

        delete [] samples;

        Model::Update();
}


void ModelBlobfinder::Startup(  void )
{ 
        Model::Startup();

        PRINT_DEBUG( "blobfinder startup" );

        // start consuming power
        SetWatts( DEFAULT_BLOBFINDERWATTS );
}

void ModelBlobfinder::Shutdown( void )
{ 

        PRINT_DEBUG( "blobfinder shutdown" );

        // stop consuming power
        SetWatts( 0 );

        // clear the data - this will unrender it too
        blobs.clear();

        Model::Shutdown();
}

//******************************************************************************
// visualization

//TODO make instance attempt to register an option (as customvisualizations do)
// Option ModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "", true, NULL );

ModelBlobfinder::Vis::Vis( World* world ) 
  : Visualizer( "Blobfinder", "blobfinder_vis" )
{
  
  //world->RegisterOption( &showArea );
  //world->RegisterOption( &showStrikes );
  //world->RegisterOption( &showFov );
  //world->RegisterOption( &showBeams );                  
}

void ModelBlobfinder::Vis::Visualize( Model* mod, Camera* cam )
{
  ModelBlobfinder* bf( dynamic_cast<ModelBlobfinder*>(mod) );
  
  if( bf->debug )
        {
          // draw the FOV
          GLUquadric* quadric = gluNewQuadric();
          
          bf->PushColor( 0,0,0,0.2  );
          
          gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
          gluPartialDisk( quadric,
                                                        0, 
                                                        bf->range,
                                                        20, // slices   
                                                        1, // loops
                                                        rtod( M_PI/2.0 + bf->fov/2.0 - bf->pan), // start angle
                                                        rtod(-bf->fov) ); // sweep angle
          
          gluDeleteQuadric( quadric );
          bf->PopColor();
        }
  
  if( bf->subs < 1 )
         return;
  
  glPushMatrix();

        // return to global rotation frame
  Pose gpose( bf->GetGlobalPose() );
  glRotatef( rtod(-gpose.a),0,0,1 );
  
  // place the "screen" a little away from the robot
  glTranslatef( -2.5, -1.5, 0.5 );
  
  // rotate to face screen
  float yaw, pitch;
  pitch = - cam->pitch();
  yaw = - cam->yaw();
  float robotAngle = -rtod(bf->pose.a);
  glRotatef( robotAngle - yaw, 0,0,1 );
  glRotatef( -pitch, 1,0,0 );
  
  // convert blob pixels to meters scale - arbitrary
  glScalef( 0.025, 0.025, 1 );
  
  // draw a white screen with a black border
  bf->PushColor( 1,1,1,1 );
  glRectf( 0,0, bf->scan_width, bf->scan_height );
  bf->PopColor();
  
  glTranslatef(0,0,0.01 );
  
  glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
  bf->PushColor( 1,0,0,1 );
  glRectf( 0,0, bf->scan_width, bf->scan_height );
  bf->PopColor();
  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
  
  // draw the blobs on the screen
  for( unsigned int s=0; s<bf->blobs.size(); s++ )
         {
                Blob* b = &bf->blobs[s];
                //blobfinder_blob_t* b = 
                //&g_array_index( blobs, blobfinder_blob_t, s);
                
                bf->PushColor( b->color );
                glRectf( b->left, b->top, b->right, b->bottom );

                //printf( "%u l %u t%u r %u b %u\n", s, b->left, b->top, b->right, b->bottom );
                bf->PopColor();
         }
  
  glPopMatrix();
}