/* *  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.ccCStageIO::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;}