visiondevice.cc

一个机器人平台

/*
 *  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 constructor
CVisionDevice::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 worldfile
bool 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 image
void 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 image
size_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)