playerdevice.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: Add player interaction to basic entity class
 * Author: Richard Vaughan, Andrew Howard
 * Date: 7 Dec 2000
 * CVS info: $Id: playerdevice.cc,v 1.50.2.1 2003/05/24 01:11:19 inspectorg Exp $
 */

#if HAVE_CONFIG_H
  #include <config.h>
#endif

#include <sys/types.h>
#include <math.h>
#include <string.h>
#include <netinet/in.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <unistd.h>
#if HAVE_VALUES_H
  #include <values.h>  // for MAXFLOAT
#endif
#include <sys/mman.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/file.h>

#include <iomanip>
#include <iostream>

//#define DEBUG
//#define VERBOSE
//#undef DEBUG
//#undef VERBOSE

#include "playerdevice.hh"
#include "world.hh"
#include "worldfile.hh"
#include "gui.hh"

#include "library.hh"


///////////////////////////////////////////////////////////////////////////
// Minimal constructor
// Requires a pointer to the parent and a pointer to the world.
CPlayerEntity::CPlayerEntity(LibraryItem *libit, CWorld *world, CEntity *parent_entity )
  : CEntity( libit, world, parent_entity )
{
  //PRINT_DEBUG( "CEntity::CEntity()" );
  this->lock_byte = world->GetEntityCount();
  
  // init the player ID structure - subclasses will set this properly
  memset( &m_player, 0, sizeof(m_player) );
  
  // init the player IO structure - subclasses will set these properly
  m_info_len    = sizeof( player_stage_info_t ); // same size for all types
  m_data_len    = 0; // type specific - set in subclasses
  m_command_len = 0; // ditto
  m_config_len  = 0; // ditto
  m_reply_len   = 0; // ditto
  
  m_info_io     = NULL; // instance specific pointers into mmap
  m_data_io     = NULL; 
  m_command_io  = NULL;
  m_config_io   = NULL;
  m_reply_io    = NULL;
  
  m_reqqueue = NULL;
  m_repqueue = NULL;  

  // Set the filename used for this device's mmap
  this->device_filename[0] = 0;
}


///////////////////////////////////////////////////////////////////////////
// Destructor
CPlayerEntity::~CPlayerEntity()
{
}


void CPlayerEntity::Update( double sim_time )
{
  //PRINT_DEBUG1( "subs: %d\n", this->subscribed );

  //int subs = this->Subscribed();

  // if our subscription state has changed, generate an property change event
  //if( this->last_sub != subs )
  // this->SetProperty( 
  
  CEntity::Update( sim_time );
}


///////////////////////////////////////////////////////////////////////////
// Load the entity from the world file
bool CPlayerEntity::Load(CWorldFile *worldfile, int section)
{
  CEntity::Load( worldfile, section );

  // Read the player port (default 0)
  m_player.port = worldfile->ReadInt(section, "port", 0 );
  
  // if the port wasn't set, and our parent is a player device,
  // default to the parent's port
  if (m_player.port == 0 && RTTI_ISPLAYERP(m_parent_entity) )
    {
      m_player.port = dynamic_cast<CPlayerEntity*>(m_parent_entity)->m_player.port;
    }

  // Read the device index
  m_player.index = worldfile->ReadInt(section, "index", 0);
  
  // m_player.code is uniquely set in a subclass's constructor

  //PRINT_DEBUG2( "port: %d index %d", m_player.port, m_player.index );

  if( m_player.port == 0 )
    printf( "\nWarning: Player device (%s:%d:%d) has zero port. Missing 'port' property in world file?\n",
	    this->lib_entry->token, m_player.port, m_player.index );

  return true;
}

bool CPlayerEntity::Save(CWorldFile *worldfile, int section)
{
  return CEntity::Save( worldfile, section );
  // can't currently change port / index dynamically
}


///////////////////////////////////////////////////////////////////////////
// Startup routine
// A virtual function that lets entitys do some initialization after
// everything has been loaded.
bool CPlayerEntity::Startup( void )
{
  CEntity::Startup();

  // we set up a mmapped file in the tmp filesystem to share
  // data with player
  
  player_stage_info_t* playerIO = 0;
  
  size_t mem = SharedMemorySize();
   
  snprintf( this->device_filename, sizeof(this->device_filename), "%s/%d.%d.%d", 
            m_world->m_device_dir, m_player.port, m_player.code, m_player.index );
  
  PRINT_DEBUG1("creating device %s", device_filename);

  int tfd;
  if( (tfd = open( this->device_filename, O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR )) == -1 )
  {
      
    PRINT_DEBUG2( "failed to open file %s for device at %p\n"
                  "assuming this is a client device!\n", 
                  this->device_filename, this );
      
    // make some memory to store the data
    assert( playerIO = (player_stage_info_t*) new char[ mem ] );

    // remove the filename so we don't try to unlink it
    this->device_filename[0] = 0;

    PRINT_DEBUG("successfully created client IO buffers");

  }
  else
  {
    // make the file the right size
    if( ftruncate( tfd, mem ) < 0 )
    {
      PRINT_ERR1( "failed to set file size: %s", strerror(errno) );
      return false;
    }
      
    // it's a little larger than a player_stage_info_t, but that's the 
    // first part of the buffer, so we'll call it that
    playerIO = 
      (player_stage_info_t*)mmap( NULL, mem, PROT_READ | PROT_WRITE, 
                                  MAP_SHARED, tfd, (off_t) 0);
      
    if (playerIO == MAP_FAILED )
    {
      PRINT_ERR1( "Failed to map memory: %s", strerror(errno) );
      return false;
    }
      
    close( tfd ); // can close fd once mapped
    
    //PRINT_DEBUG("successfully mapped shared memory");
    
    //PRINT_DEBUG2( "S: mmapped %d bytes at %p\n", mem, playerIO );
  }
  
  // try a lock
  assert( Lock() );
  
  // Initialise entire space
  memset(playerIO, 0, mem);
  
  // set the pointers into the shared memory region
  m_info_io    = playerIO; // info is the first record
  m_data_io    = (uint8_t*)m_info_io + m_info_len;
  m_command_io = (uint8_t*)m_data_io + m_data_len; 
  m_config_io  = (uint8_t*)m_command_io + m_command_len;
  m_reply_io   = (uint8_t*)(m_config_io + 
                            (m_config_len * sizeof(playerqueue_elt_t)));
  
  m_info_io->len = SharedMemorySize(); // total size of all the shared data
  m_info_io->lockbyte = this->lock_byte; // record lock on this byte
  
  // set the lengths in the info structure
  m_info_io->data_len    =  (uint32_t)m_data_len;
  m_info_io->command_len =  (uint32_t)m_command_len;
  m_info_io->config_len  =  (uint32_t)m_config_len;
  m_info_io->reply_len   =  (uint32_t)m_reply_len;
  
  m_info_io->data_avail    =  0;
  m_info_io->command_avail =  0;
  m_info_io->config_avail  =  0;
  
  m_info_io->player_id.port = m_player.port;
  m_info_io->player_id.index = m_player.index;
  m_info_io->player_id.code = m_player.code;
  m_info_io->subscribed = 0;

  // create the PlayerQueue entitys that we'll use to access requests and
  // replies.  pass in the chunks of memory that are already mmap()ed
  assert(m_reqqueue = new PlayerQueue(m_config_io,m_config_len));
  assert(m_repqueue = new PlayerQueue(m_reply_io,m_reply_len));
  
  // initialize the driver name with something sensible, in case the device
  // doesn't set it
  strncpy((char*)(this->m_info_io->drivername), "stage_device", 
          sizeof(this->m_info_io->drivername));

#ifdef DEBUG
  printf( "\t\t(%p) (%d,%d,%d) IO at %p\n"
          "\t\ttotal: %d\tinfo: %d\tdata: %d (%d)\tcommand: %d (%d)\tconfig: %d (%d)\n ",
          this,
          m_info_io->player_id.port,
          m_info_io->player_id.code,
          m_info_io->player_id.index,
          m_info_io,
          m_info_len + m_data_len + m_command_len + m_config_len,
          m_info_len,
          m_info_io->data_len, m_data_len,
          m_info_io->command_len, m_command_len,
          m_info_io->config_len, m_config_len );
#endif  

  // try  an unlock
  assert( Unlock() );
  
  // store our driver name in the player IO structure.
  // use the worldfile token as the driver name to avoid possible confusion
  this->SetDriverName( (char*)this->lib_entry->token );

  return true;
}


///////////////////////////////////////////////////////////////////////////
// Shutdown routine
void CPlayerEntity::Shutdown()
{
  CEntity::Shutdown();

  if( this->device_filename[0] )
    // remove our name in the filesystem
    if( unlink( this->device_filename ) == -1 )
      PRINT_DEBUG2( "Device failed to unlink it's IO file:%s %s", 
		    this->device_filename, strerror(errno) );
}


///////////////////////////////////////////////////////////////////////////
// Compute the total shared memory size for the device
int CPlayerEntity::SharedMemorySize( void )
{
  return( m_info_len + m_data_len + m_command_len + 
          (m_config_len * sizeof(playerqueue_elt_t)) +
          (m_reply_len * sizeof(playerqueue_elt_t)));
}


// Copy data from the shared memory segment
size_t CPlayerEntity::GetIOData( void* dest, size_t dest_len,  
                                 void* src, uint32_t* avail )
{
  size_t len;
  
  assert( dest ); // the caller must have somewhere to put this data
  if( src == 0 )  // this device doesn't have any data here
    return 0;     // so bail right away

  Lock();

  len = 0;

  // If there is data available...
  if (*avail > 0)
  {
    // If the data fits...
    if ((size_t) *avail <= dest_len)
    {
      memcpy( dest, src, *avail);
      len = *avail;
    }
    else
    {
      PRINT_ERR2("command buffer overflow; (%d > %d)", *avail, dest_len);
      len = 0;
    }
  }

  Unlock();

  return len;
}

size_t CPlayerEntity::GetData( void* data, size_t len )
{
  //PRINT_DEBUG( "f" );
  return GetIOData( data, len, m_data_io, &m_info_io->data_avail );
}

size_t CPlayerEntity::GetCommand( void* data, size_t len )
{
  //PRINT_DEBUG( "f" );
  return GetIOData( data, len, m_command_io, &m_info_io->command_avail );
}


///////////////////////////////////////////////////////////////////////////
// Copy data into the shared memory segment
size_t CPlayerEntity::PutIOData( void* dest, size_t dest_len,  
			   void* src, size_t src_len,
			   uint32_t* ts_sec, uint32_t* ts_usec, 
			   uint32_t* avail )
{  
  Lock();

  assert( dest );
  assert( src );
  assert( src_len > 0 );
  assert( dest_len > 0 );
  
  if( src_len == 0 )
    printf( "WIERD! attempt to write no bytes into a buffer" );

  if( src_len <= dest_len ) // if there is room for the data
  {
    memcpy( dest, src, src_len); // export the data

    // update the availability and timestamps
    *avail    = src_len;
    *ts_sec   = m_world->m_sim_timeval.tv_sec;
    *ts_usec  = m_world->m_sim_timeval.tv_usec;
  }
  else
  {
    PRINT_ERR2( "failed trying to put %d bytes; io buffer is "
                "%d bytes.)\n", src_len, dest_len  );
      
    src_len = 0; // indicate failure
  }
  
  Unlock();
  
  return src_len;
}

////////////////////////////////////////////////////////////////////////////
// Write to the driver name segment of the IO buffer
//
// this is called at the end of CPlayerEntity::Startup() with the
// worldfile token for this device.
void 
CPlayerEntity::SetDriverName( char* name )
{
  Lock();

  assert(name);

  strncpy((char*)(this->m_info_io->drivername), name, 
          sizeof(this->m_info_io->drivername));