stageio.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: Stage client/server IO class
 * Author: Richard Vaughan
 * Date: 7 Dec 2000
 * CVS info: $Id: stageio.cc,v 1.22 2002/11/09 02:32:34 rtv Exp $
 */
#if HAVE_CONFIG_H
  #include <config.h>
#endif

//#define DEBUG
//#define VERBOSE

#include "server.hh"
#include "entity.hh"
#include "world.hh"
//#include "bitmap.hh"
#include "library.hh"

#include <unistd.h>
#include <string.h>
#include <signal.h>

#include "playerdevice.hh"

//extern int g_timer_expired;

void CatchSigPipe( int signo ); // from server.cc

CStageIO::CStageIO( int argc, char** argv, Library* lib )
  : CWorld( argc, argv, lib )
{
  m_port = DEFAULT_POSE_PORT;
  
  for( int a=1; a<argc-1; a++ )
  {
    // get the port number, overriding the default
    if( strcmp( argv[a], "-p" ) == 0 )
    {
      m_port = atoi(argv[a+1]);
      printf( "[Port %d]", m_port );
      a++;
    }
  }

  // catch signals generated by socket closures
  signal( SIGPIPE, CatchSigPipe );
 
  // init the pose server data structures
  m_pose_connection_count = 0;
  m_sync_counter = 0;
  m_external_sync_required = false;

  // init the arrays
  memset( &m_pose_connections, 0, sizeof(struct pollfd)*MAX_POSE_CONNECTIONS); 
  memset( m_dirty_subscribe, 0, sizeof(char) * MAX_POSE_CONNECTIONS );
  memset( m_conn_type, 0, sizeof(char) * MAX_POSE_CONNECTIONS );
}  

CStageIO::~CStageIO( void )
{
  // close all connections
  for( int i=0; i<m_pose_connection_count; i++ )
    DestroyConnection( i );

//close( m_pose_connections[i].fd );
}

int CStageIO::WriteHeader( int fd, HeaderType type, uint32_t data )
{      
  stage_header_t hdr;
  
  hdr.type = type;
  hdr.data = data;
 
  //puts( "attempting to write a header" );

  int len = WritePacket( fd, (char*)&hdr, sizeof(hdr) );

  //printf( "wrote %d header bytes\n", len );

  return len;
}

int CStageIO::WriteCommand( int fd, cmd_t cmd )
{
  //puts( "attempting to write a command" );
  int len = WriteHeader( fd, StageCommand, cmd );
  //printf( "wrote %d command bytes\n", len );
  return len;
}

int CStageIO::WritePacket( int fd, char* data, size_t len )
{
  size_t writecnt = 0;
  int thiswritecnt;
  
  //printf( "attempting to write %d byte packet\n", len );

  while(writecnt < len )
  {
    thiswritecnt = write( fd, data+writecnt, len-writecnt);
      
    // check for error on write
    if( thiswritecnt == -1 )
      return -1; // fail
      
    writecnt += thiswritecnt;
  }

  //printf( "wrote %d/%d packet bytes\n", writecnt, len );

  return len; //success
}


int CStageIO::ReadPacket( int fd, char* buf, size_t len )
{
  //printf( "attempting to read a %d byte packet\n", len );

  assert( buf ); // data destination must be good

  int recv = 0;
  // read a header so we know what's coming
  while( recv < (int)len )
  {
    //printf( "Reading on %d\n", fd );
      
    /* read will block until it has some bytes to return */
    int r = read( fd, buf+recv,  len - recv );
      
    if( r == -1 ) // ERROR
    {
      if( errno != EINTR )
	    {
	      printf( "ReadPacket: read returned %d\n", r );
	      perror( "code" );
	      break;
	    }
    }
    else if( r == 0 ) // EOF
      break; 
    else
      recv += r;
  }      

  //printf( "read %d/%d bytes\n", recv, len );

  return recv; // the number of bytes read
}

int CStageIO::WriteProperty( int fd, stage_property_t* prop, 
			    char* data, size_t len )
{
  int res;
  
  assert( prop );
  assert( data );
  assert( len > 0 ); // no point sending a property with no data
  assert( prop->len == len ); // data size should match header len field
  assert( prop->property < ENTITY_LAST_PROPERTY );

  //puts( "attempting to write a property" );
  
  res = WritePacket( fd, (char*)prop, sizeof(stage_property_t) );
  
  //printf( "write %d/%d property header bytes\n", res,sizeof(stage_property_t));
  //printf( "writing %d bytes of data\n", prop->len );
  
  res = WritePacket( fd, data, len );

  //printf( "wrote %d/%d bytes of property data\n", res, len );

  return 0;
}

int CStageIO::ReadProperty( int fd, stage_property_t* prop, 
			   char* data, size_t len )
{
  int res;

  assert( prop );
  assert( data );

  //puts( "attempting to read a property" );
  
  res = ReadPacket( fd, (char*)prop, sizeof(stage_property_t) );

  //printf( "read %d/%d property header bytes\n", res, sizeof(stage_property_t));
  //printf( "expecting %d bytes of data\n", prop->len );

  // validity checks 
  assert( prop->property < ENTITY_LAST_PROPERTY );
  assert( prop->len > 0 );
  assert( prop->len < MAX_PROPERTY_DATA_LEN );
  assert( prop->len < len ); // must be enough space for the data

  // read data into the buffer
  res = ReadPacket( fd, data, prop->len );

  //printf( "read %d/%d bytes of property data\n", res, prop->len );

  return 0;
}

int CStageIO::ReadProperties( int con, int fd, int num )
{
  stage_property_t prop;
  char data[MAX_PROPERTY_DATA_LEN]; // XX define this somewhere
  
  for( int i=0; i<num; i++ )
  {
    ReadProperty( fd, &prop, data, MAX_PROPERTY_DATA_LEN );
      
    //printf( "attempting to set property %d of object %d"
    //    "with %d bytes of data starting at %p\n",
    //    prop.property, prop.id, prop.len, data );
      
    // set the property
    CEntity* ent = NULL;

    printf( "looking up entity number %d\n", prop.id );

    assert( ent = GetEntity( prop.id ) );
  
    //ent->SetProperty( con, prop.property, (void*)data, prop.len );
  }
  
  return 0;
}


int CStageIO::ReadEntity( int fd, stage_entity_t* ent )
{
  assert( ent );
  
  int res = ReadPacket( fd, (char*)ent, sizeof(stage_entity_t) );
 
  return res;
}


int CStageIO::ReadEntities( int fd, int num )
{
  stage_entity_t ent;
  
  for( int i=0; i<num; i++ )
  {
    ReadEntity( fd, &ent );
     
    PRINT_DEBUG3( "attempting to create entity %d:%d:%s\n",
		  ent.id, ent.parent, ent.token );
    
    //CEntity* obj = 0;
    if( ent.parent == -1 )
      //assert( root = lib->CreateEntity( ent.type, this, 0 ) );
      puts( "read root - no ent created" );
    else
      assert( lib->CreateEntity( ent.token, this,
				 GetEntity(ent.parent)));
  }
  
  return 0;
}

int CStageIO::ReadBackground( int fd )
{
  PRINT_DEBUG( "" );

  stage_background_t w;
  
  int res = ReadPacket( fd, (char*)&w, sizeof(w) );
  

  if( res == sizeof(w) )
  {
    // Construct a single fixed obstacle representing
    // the environment.
    //assert( this->root = (CEntity*)new CBitmap(this, NULL ) );
    
    // poke the scale in from the data packet(ugly!)
    //((CBitmap*)this->root)->scale = w.scale;
    
    assert(this->matrix); // gotta have it before we can load the wall
      
    // read in the pixel array
    int num_pixels =  w.sizex * w.sizey;
    unsigned char* pixels = 0;
    assert( pixels = new unsigned char[ num_pixels ] );
      
    printf( "Attempting to read %d pixels into %p on %d\n",
            num_pixels, &pixels, fd );

    int res2 = ReadPacket( fd, (char*)pixels, num_pixels );
      
    if( res2 == num_pixels )
    {
      // create an image from the data
      //assert( ((CBitmap*)this->root)->image = 
      //      new Nimage( pixels, w.sizex, w.sizey ) );            
    }
    else
    {
      PRINT_ERR2( "short read (%d/%d pixels)\n", res2, num_pixels );
      return -1;
    }
  }
  else
    PRINT_ERR2( "short read (%d/%d bytes)\n", res, sizeof(w) );
  
  return res;
}

int CStageIO::WriteBackground( int fd )
{
  PRINT_DEBUG( "" );

  /*
  CBitmap* fix = dynamic_cast<CBitmap*>(this->root);
  
  assert( fix );
  //assert( fix->image );
  
  assert( fix->bitmap_rects.size() > 0 );
  
  // announce that a matrix is on its way
  WriteHeader( fd, BackgroundPacket, 0);
  
  stage_background_t b;
  
  b.sizex = fix->image->get_width();
  b.sizey = fix->image->get_height();
  b.scale = fix->scale;
  //b.pixel_count = this->root->image->count_pixels();

  printf( "Downloading background (%p)\n", &b ); 
  
  int res = WritePacket( fd, (char*)&b, sizeof(b) );
  
  // write the Nimage data as a big packet - this should be improved
  // to send only non-zero pixels to save bandwidth for large, sparse
  // images

  int num_pixels = b.sizex * b.sizey;
  int res2 = WritePacket( fd, 
			  (char*)fix->image->data, 
			  num_pixels );

  assert( res2 == num_pixels );
  */

  int res = 0;
  return res;
}


int CStageIO::ReadMatrix( int fd )
{
  if( this->matrix )
    delete this->matrix;

  stage_matrix_t m;
  
  int res = ReadPacket( fd, (char*)&m, sizeof(m) );
 
  if( res == sizeof(m) )
    this->matrix = new CMatrix( m.sizex, m.sizey, 10 );
  else
    PRINT_ERR2( "short read (%d/%d bytes)", res, sizeof(m) );
 
  return res;
}

int CStageIO::WriteMatrix( int fd )
{
  assert( this->matrix );

  // announce that a matrix is on its way
  WriteHeader( fd,  MatrixPacket, 0);
 
  stage_matrix_t m;
  
  m.sizex = this->matrix->width;
  m.sizey = this->matrix->height;

  printf( "Downloading matrix (%p)\n", &m ); 
  
  int res = WritePacket( fd, (char*)&m, sizeof(m) );
  
  return res;
}