world.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: top level class that contains everything
 * Author: Richard Vaughan, Andrew Howard
 * Date: 7 Dec 2000
 * CVS info: $Id: world.cc,v 1.137.2.1 2002/12/10 00:28:18 rtv Exp $
 */
#if HAVE_CONFIG_H
  #include <config.h>
#endif
#if HAVE_STRINGS_H
  #include <strings.h>
#endif

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

#include <errno.h>
#include <sys/time.h>
#include <sys/wait.h>
#include <signal.h>
#include <math.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <semaphore.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <pwd.h>
#include <stdio.h>
#include <netdb.h>

#include <fstream>
#include <iostream>

#include "world.hh"
#include "playerdevice.hh"
#include "library.hh"
#include "gui.hh"
#include "models/bitmap.hh"

bool usage = false;

void PrintUsage(); // defined in main.cc
void StageQuit();

long int g_bytes_output = 0;
long int g_bytes_input = 0;


// allocate chunks of 32 pointers for entity storage
const int OBJECT_ALLOC_SIZE = 32;


// static member init
CEntity* CWorld::root = NULL;

///////////////////////////////////////////////////////////////////////////
// Default constructor
CWorld::CWorld( int argc, char** argv, Library* lib )
{
  // store the params locally
  this->argc = argc;
  this->argv = argv;
  this->lib = lib;

  // seed the random number generator
#if HAVE_SRAND48
  srand48( time(NULL) );
#endif

  // Initialise configuration variables
  this->ppm = 20;

  // matrix is created by a StageIO object
  this->matrix = NULL;

  // if we are a server, this gets set in the server's constructor
  this->worldfile = NULL;

  // invalid file descriptor initially
  m_log_fd = -1;

  m_instance = 0;

  // just initialize stuff here

  this->entities = NULL;
  this->entity_count = 0;
  this->entities_size = 0;

  m_log_output = false; // enable with -l <filename>
  m_console_output = false; // enable with -o
    
  // real time mode by default
  // if real_timestep is zero, we run as fast as possible
  m_real_timestep = 0.1; //seconds
  m_sim_timestep = 0.1; //seconds; - 10Hz default rate 

  m_step_num = 0;

  // start paused
  //
  m_enable = false;

  if( gethostname( m_hostname, HOSTNAME_SIZE ) == -1)
    {
      perror( "Stage: couldn't get hostname. Quitting." );
      exit( -1 );
    }

  /* now strip off domain */
  char* first_dot;
  strncpy(m_hostname_short, m_hostname,HOSTNAME_SIZE);
  if( (first_dot = strchr(m_hostname_short,'.') ))
    *first_dot = '\0';
      
  // get the IP of our host
  struct hostent* info = gethostbyname( m_hostname );
  
  if( info )
    { // make sure this looks like a regular internet address
      assert( info->h_length == 4 );
      assert( info->h_addrtype == AF_INET );
      
      // copy the address out
      memcpy( &m_hostaddr.s_addr, info->h_addr_list[0], 4 ); 
    }
  else
    {
      PRINT_ERR1( "failed to resolve IP for local hostname \"%s\"\n", 
		  m_hostname );
    }
  
  // Run the gui by default
  this->enable_gui = true;

  // default color database file
  strcpy( m_color_database_filename, COLOR_DATABASE );

  // Initialise clocks
  m_start_time = m_sim_time = 0;
  memset( &m_sim_timeval, 0, sizeof( struct timeval ) );
    
  // Initialise entity list
  this->entity_count = 0;
  
  // start with no key
  bzero(m_auth_key,sizeof(m_auth_key));
 

  assert(lib);
  //lib->Print();


  // Construct a fixed obstacle representing the boundary of the 
  // the environment - this the root for all other entities
  assert( root = (CEntity*)new CBitmap( lib->LibraryItemFromToken( "bitmap" ), this, NULL) );
  
  // give the command line a chance to override the default values
  // we just set
  if( !ParseCmdLine( this->argc, this->argv )) 
  {
    quit = true;
    return;
  }

  // give the GUI a go at the command line too
  if( enable_gui )  GuiInit( argc, argv ); 
}


///////////////////////////////////////////////////////////////////////////
// Destructor
CWorld::~CWorld()
{
  if( matrix )
    delete matrix;

  if( root )
    delete root;
}

///////////////////////////////////////////////////////////////////////////
// Parse the command line
bool CWorld::ParseCmdLine(int argc, char **argv)
{
  for( int a=1; a<argc; a++ )
    {   
      // USAGE
      if( (strcmp( argv[a], "-?" ) == 0) || 
	  (strcmp( argv[a], "--help") == 0) )
	{
	  PrintUsage();
	  exit(0); // bail right here
	}
      
      // LOGGING
      if( strcmp( argv[a], "-l" ) == 0 )
	{
	  m_log_output = true;
	  strncpy( m_log_filename, argv[a+1], 255 );
	  printf( "[Logfile %s (undocumented/experimental)]", m_log_filename );
	
	  //store the command line for logging later
	  memset( m_cmdline, 0, sizeof(m_cmdline) );
	
	  for( int g=0; g<argc; g++ )
	    {
	      strcat( m_cmdline, argv[g] );
	      strcat( m_cmdline, " " );
	    }
	
	  a++;
	
	  // open the log file and write out a header
	  LogOutputHeader();
	}
    
      // DIS/ENABLE GUI
      if( strcmp( argv[a], "-g" ) == 0 )
	{
	  this->enable_gui = false;
	  printf( "[No GUI]" );
	}
          
      // SET GOAL REAL CYCLE TIME
      // Stage will attempt to update at this speed
      if( strcmp( argv[a], "-u" ) == 0 )
	{
	  m_real_timestep = atof(argv[a+1]);
	  printf( "[Real time per cycle %f sec]", m_real_timestep );
	  a++;
	}
      
      // SET SIMULATED UPDATE CYCLE
      // one cycle simulates this much time
      else if( strcmp( argv[a], "-v" ) == 0 )
	{
	  m_sim_timestep = atof(argv[a+1]);
	  printf( "[Simulated time per cycle %f sec]", m_sim_timestep );
	  a++;
	}
      
      // DISABLE console output
      if( strcmp( argv[a], "-o" ) == 0 )
	{
	  m_console_output = true;
	  printf( "[Console Output]" );
	}
      
      //else if( strcmp( argv[a], "-id" ) == 0 )
      //{
      //  memset( m_hostname, 0, 64 );
      //  strncpy( m_hostname, argv[a+1], 64 );
      //  printf( "[ID %s]", m_hostname ); fflush( stdout );
      //  a++;
      //}
    }

    
  return true;
}


///////////////////////////////////////////////////////////////////////////
// Startup routine 
bool CWorld::Startup()
{  
  PRINT_DEBUG( "** STARTUP **" );
  
  // we must have at least one entity to play with!
  // they should have been created by server or client before here
  if( this->entity_count < 1 )
    {
      puts( "\nStage: No entities defined in world file. Nothing to simulate!" );
      return false;
    }

  // Initialise the real time clock
  // Note that we really do need to set the start time to zero first!
  m_start_time = 0;
  m_start_time = GetRealTime();
    
  // Initialise the rate counter
  m_update_ratio = 1;
  m_update_rate = 0;
      
#ifdef DEBUG
  //root->Print( "" );
#endif

  // use the generic hook to start the GUI
  if( this->enable_gui ) GuiWorldStartup( this );

  // Startup all the entities. they will create and initialize their
  // device files and Gui stuff
  if( !root->Startup() )
    {
      PRINT_ERR("Root Entity startup failed" );
      quit = true;
      return false;
    }


  PRINT_DEBUG( "** STARTUP DONE **" );
  return true;
}
    

///////////////////////////////////////////////////////////////////////////
// Shutdown routine 
bool CWorld::Shutdown()
{
  PRINT_DEBUG( "world shutting down" );
  
  // use the hook to shutdown the GUI
  if( this->enable_gui ) GuiWorldShutdown( this );
  
  // Shutdown all the entities
  // Devices will unlink their device files
  root->Shutdown();

  return true;
}

///////////////////////////////////////////////////////////////////////////
// Update the world
void CWorld::Update(void)
{
  PRINT_DEBUG( "** Update **" );
      
  // let the entities do anything they want to do between clock increments
  //root->Sync(); 
      
  // set the current time and update the entities managed by this host
  if( this->m_enable )
    {
      root->Update(this->SetClock( m_sim_timestep, m_step_num ));
      
      // increase the time step counter
      m_step_num++;    
    }
  
  // all the entities are done, now let's draw the graphics
  if( this->enable_gui )  GuiWorldUpdate( this );
}


double CWorld::SetClock( double interval, uint32_t step_num )
{	  
  // Update the simulation time (in both formats)
  m_sim_time = step_num * interval;