/* *  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.ccvoid StageQuit();long int g_bytes_output = 0;long int g_bytes_input = 0;// allocate chunks of 32 pointers for entity storageconst int OBJECT_ALLOC_SIZE = 32;// static member initCEntity* CWorld::root = NULL;///////////////////////////////////////////////////////////////////////////// Default constructorCWorld::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 ); }///////////////////////////////////////////////////////////////////////////// DestructorCWorld::~CWorld(){  if( matrix )    delete matrix;  if( root )    delete root;}///////////////////////////////////////////////////////////////////////////// Parse the command linebool 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 worldvoid 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;