/* *  Stage : a multi-robot simulator. *  Copyright (C) 2001, 2002 Richard Vaughan, Andrew Howard and Brian Gerkey. * *  This program is free software; you can redistribute it and/or modify *  it under the terms of the GNU General Public License as published by *  the Free Software Foundation; either version 2 of the License, or *  (at your option) any later version. * *  This program is distributed in the hope that it will be useful, *  but WITHOUT ANY WARRANTY; without even the implied warranty of *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the *  GNU General Public License for more details. * *  You should have received a copy of the GNU General Public License *  along with this program; if not, write to the Free Software *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA * *//* * Desc: Device to simulate the ACTS vision system. * Author: Richard Vaughan, Andrew Howard * Date: 28 Nov 2000 * CVS info: $Id: visiondevice.cc,v 1.3 2002/11/11 08:21:40 rtv Exp $ */#include <math.h>#include <iostream>#include "world.hh"#include "visiondevice.hh"#include "ptzdevice.hh"#include "raytrace.hh"#define DEBUG///////////////////////////////////////////////////////////////////////////// Default constructorCVisionDevice::CVisionDevice(LibraryItem* libit,CWorld *world, CPtzDevice *parent)        : CPlayerEntity( libit, world, parent ){  // set the Player IO sizes correctly for this type of Entity  m_data_len    = sizeof( player_blobfinder_data_t );   m_command_len = 0;  m_config_len  = 0;  m_reply_len  = 0;   m_player.code = PLAYER_BLOBFINDER_CODE;  m_interval = 0.1; // 10Hz - the real cam is around this  // If the parent is a ptz device we will use it, otherwise  // we will operate as a naked vision device.  if (parent->m_player.code == PLAYER_PTZ_CODE )    m_ptz_device = parent;  else    m_ptz_device = NULL;    cameraImageWidth = 160;  cameraImageHeight = 120;  m_scan_width = 160;  m_pan = 0;  m_tilt = 0;  m_zoom = DTOR(60);  m_max_range = 8.0;  // Set the default channel-color mapping for ACTS.  Let's start with  // a reasonable set of color channels these get overwritten when  // specifiedby the worldfile.  this->channels[0] = ::LookupColor( "red" );  this->channels[1] = ::LookupColor( "green" );  this->channels[2] = ::LookupColor( "blue" );  this->channels[3] = ::LookupColor( "yellow" );  this->channels[4] = ::LookupColor( "cyan" );  this->channels[5] = ::LookupColor( "magenta" );  this->channel_count = 6;  numBlobs = 0;  memset( blobs, 0, MAXBLOBS * sizeof( ColorBlob ) );#ifdef INCLUDE_RTK2  vision_fig = NULL;#endif}///////////////////////////////////////////////////////////////////////////// Startup routine//bool CVisionDevice::Startup(){  if (!CPlayerEntity::Startup())    return false;  return true;}///////////////////////////////////////////////////////////////////////////// Load the entity from the worldfilebool CVisionDevice::Load(CWorldFile *worldfile, int section){  if (!CPlayerEntity::Load(worldfile, section))    return false;  // Read the vision channel/color mapping  for (int i = 0; true; i++)  {    const char *color = worldfile->ReadTupleString(section, "channels", i, NULL);    if( color ) // if the string looks ok    {      //printf( "[%d:%s]", i, color );      this->channels[i] = ::LookupColor(color);      this->channel_count = i + 1;    }    else      break;  }    return true;}///////////////////////////////////////////////////////////////////////////// Update the laser data//void CVisionDevice::Update( double sim_time ){  ASSERT(m_world != NULL);  CPlayerEntity::Update( sim_time );  // Dont update anything if we are not subscribed  if( Subscribed() < 1 )    return;    // See if its time to recalculate vision  if( sim_time - m_last_update < m_interval )    return;  m_last_update = sim_time;    //RTK_TRACE0("generating new data");    // Generate the scan-line image  UpdateScan();    // Generate ACTS data    player_blobfinder_data_t data;  memset( &data, 0, sizeof(data) );  UpdateACTS( &data );    // Copy data to the output buffer  PutData( &data, sizeof(data) );}///////////////////////////////////////////////////////////////////////////// Generate the scan-line imagevoid CVisionDevice::UpdateScan(){  // Get the camera's global pose  double ox, oy, oth;  GetGlobalPose(ox, oy, oth);  // Get the ptz settings  if (m_ptz_device != NULL)    m_ptz_device->GetPTZ(m_pan, m_tilt, m_zoom);  // Compute starting angle  oth = oth + m_pan + m_zoom / 2;  // Compute fov, range, etc  double dth = m_zoom / m_scan_width;  // Make sure the data buffer is big enough  ASSERT((size_t)m_scan_width<=sizeof(m_scan_channel)/sizeof(m_scan_channel[0]));  // TODO  //int skip = (int) (m_world->m_vision_res / m_scan_res - 0.5);  // Do each scan  // Note that the scan is taken *clockwise*  // i'm scanning this as half-resolution for a significant speed-up  StageColor col;    for (int s = 0; s < m_scan_width; s++)  {    //printf( "scan %d of %d\n", s, m_scan_width );    // indicate no valid color found (MSB normally unused in 32bit RGB value)    col = 0xFF000000;           // Compute parameters of scan line    double px = ox;    double py = oy;    double pth = oth - s * dth;       CLineIterator lit( px, py, pth, m_max_range,                        m_world->ppm, m_world->matrix, PointToBearingRange );          CEntity* ent;    double range = m_max_range;          while( (ent = lit.GetNextEntity()) )     {      //printf( "ent %p (%s), vision_return %d\n",      //ent, ent->lib_entry->token, ent->vision_return );      //ent->Print( "" );      // Ignore itself, its ancestors and its descendents      if( ent == this || this->IsDescendent(ent) || ent->IsDescendent(this))	  continue;           // Ignore transparent things      if( !ent->vision_return )	  continue;      range = lit.GetRange(); // it's this far away      // get the color of the entity      memcpy( &col, &(ent->color), sizeof( StageColor ) );	        break;    }	    // initialize the reading     m_scan_channel[s] = 0; // channel 0 is no-blob    m_scan_range[s] = 0;    // if we found a color on this ray    if( !(col & 0xFF000000) )     {      // look up this color in the color/channel mapping array      for( int c=0; c < this->channel_count; c++ )      {        if( this->channels[c] == col)        {          //printf("color %d is channel %d\n", col, c);          //printf("m_scan_channel[%d] = %d\n", s, c+1);          m_scan_channel[s] = c + 1; // channel 0 is no-blob          m_scan_range[s] = range;          break;        }      }    }  }}///////////////////////////////////////////////////////////////////////////// Generate ACTS data from scan-line imagesize_t CVisionDevice::UpdateACTS( player_blobfinder_data_t* data ){  PRINT_DEBUG( "entered" );  assert( data );  // now the colors and ranges are filled in - time to do blob detection  float yRadsPerPixel = m_zoom / cameraImageHeight;  int blobleft = 0, blobright = 0;  unsigned char blobcol = 0;  int blobtop = 0, blobbottom = 0;  numBlobs = 0;  // scan through the samples looking for color blobs  for( int s=0; s < m_scan_width; s++ )  {    if( m_scan_channel[s] != 0 && m_scan_channel[s] <         PLAYER_BLOBFINDER_MAX_CHANNELS)