/* *  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: This class implements the server, or main, instance of Stage. * Author: Richard Vaughan, Andrew Howard * Date: 6 Jun 2002 * CVS info: $Id: server.cc,v 1.43.4.1 2003/12/05 16:56:25 gerkey Exp $ */#if HAVE_CONFIG_H  #include <config.h>#endif#if HAVE_STRINGS_H  #include <strings.h>#endif#include <arpa/inet.h>#include <errno.h>#include <fcntl.h>#include <math.h>#include <netdb.h>#include <netinet/in.h>//#include <pthread.h>#include <pwd.h>#include <signal.h>#include <stdio.h>#include <stdlib.h>#include <string.h>#include <sys/mman.h>#include <sys/socket.h>#include <sys/stat.h>#include <sys/time.h>#include <sys/types.h>#include <sys/wait.h>#include <termios.h>#include <unistd.h>#include <fstream>#include <iostream>//#include <sstream>#include <iomanip>//using namespace std;//#define DEBUG//#define VERBOSE#include "server.hh"//#include "boxobstacle.hh"#include "playerdevice.hh"//#include "library.hh"//extern Library* lib;extern long int g_bytes_output;extern long int g_bytes_input;const int LISTENQ = 128;//const long int MILLION = 1000000L;int g_timer_events = 0;// dummy timer signal funcvoid TimerHandler( int val ){  //puts( "TIMER HANDLER" );  g_timer_events++;  // re-install signal handler for timing  if( signal( SIGALRM, &TimerHandler ) == SIG_ERR )    {      PRINT_ERR("failed to install signal handler");      exit( -1 );    }  //printf( "\ng_timer_expired: %d\n", g_timer_expired );}  void CatchSigPipe( int signo ){#ifdef VERBOSE  puts( "** SIGPIPE! **" );#endif  exit( -1 );}CStageServer::CStageServer( int argc, char** argv, Library* lib )   : CStageIO( argc, argv, lib ){   // enable player services by default, the command lines may change this  m_run_player = true;      //  one of our parent's constructors may have failed and set this flag  if( quit ) return;  m_clock = NULL;    // stop time of zero means run forever  m_stoptime = 0;    // this can be set true with a cmdline switch  this->start_disabled = false;  // create the object that loads and parses the file  assert( worldfile = new CWorldFile() );  if(!ParseCmdLine(argc, argv))  {    quit = true;    return;  }  //printf( "WORLDFILE: %s\n", argv[argc-1]  );}CStageServer::~CStageServer( void ){  // do nothing}  // get the world description out of the world filebool CStageServer::Load( void ){  //////////////////////////////////////////////////////////////////////  // FIGURE OUT THE WORLD FILE NAME    this->worldfilename[0] = 0;  // find the world file name (it's the last argument)  strcpy( this->worldfilename, argv[argc-1] );  // make sure something happened there...  assert(  this->worldfilename[0] != 0 );  // reassuring console output  printf( "[World %s]", this->worldfilename ); fflush(stdout);  //////////////////////////////////////////////////////////////////////  // FIGURE OUT THE DEFAULT FULL HOST NAME & ADDRESS  // the default hostname is this host's name  char current_hostname[ HOSTNAME_SIZE ];  strncpy( current_hostname, this->m_hostname, HOSTNAME_SIZE );    // maintain a connection to the nameserver - speeds up lookups  sethostent( true );    struct hostent* info = gethostbyname( current_hostname );  assert( info );  struct in_addr current_hostaddr;    // 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( &current_hostaddr.s_addr, info->h_addr_list[0], 4 );     //printf( "\nHOSTNAME %s NAME %s LEN %d IP %s\n",   //  current_hostname,  //  info->h_name,   //  info->h_length,   //  inet_ntoa( current_hostaddr ) );  /////////////////////////////////////////////////////////////////  // NOW LOAD THE WORLD FILE, CREATING THE ENTITIES AS WE GO    // Load and parse the world file  if (!this->worldfile->Load(worldfilename))    return false;  // set the top-level matrix resolution  this->ppm = 1.0 / this->worldfile->ReadLength( 0, "resolution", 1.0 / this->ppm );    // Get the authorization key to pass to player  const char *authkey = this->worldfile->ReadString(0, "auth_key", "");  strncpy(m_auth_key, authkey, sizeof(m_auth_key));  m_auth_key[sizeof(m_auth_key)-1] = '\0';    // Get the real update interval  m_real_timestep = this->worldfile->ReadFloat(0, "real_timestep", 					      m_real_timestep);  // Get the simulated update interval  m_sim_timestep = this->worldfile->ReadFloat(0, "sim_timestep", m_sim_timestep);    // Iterate through sections and create entities as needs be  for (int section = 1; section < this->worldfile->GetEntityCount(); section++)  {    // Find out what type of entity this is,    // and what line it came from.    const char *type = this->worldfile->GetEntityType(section);    int line = this->worldfile->ReadInt(section, "line", -1);    // Ignore some types, since we already have dealt will deal with them    if (strcmp(type, "gui") == 0)      continue;    // if this section defines the current host, we load it here    // this breaks the context-free-ness of the syntax, but it's     // a simple way to do this. - RTV    if( strcmp(type, "host") == 0 )    {      char candidate_hostname[ HOSTNAME_SIZE ];      strncpy( candidate_hostname,                worldfile->ReadString( section, "hostname", 0 ),               HOSTNAME_SIZE );	      // if we found a name      if( strlen(candidate_hostname) > 0  )      {        struct hostent* cinfo = 0;	            printf( "CHECKING HOSTNAME %s\n", candidate_hostname );        //lookup this host's IP address:        if( (cinfo = gethostbyname( candidate_hostname ) ) ) 	      {          // make sure this looks like a regular internet address          assert( cinfo->h_length == 4 );          assert( cinfo->h_addrtype == AF_INET );		          // looks good - we'll use this host from now on          strncpy( current_hostname, candidate_hostname, HOSTNAME_SIZE );          // copy the address out          memcpy( &current_hostaddr.s_addr, cinfo->h_addr_list[0], 4 ); 		          printf( "LOADING HOSTNAME %s NAME %s LEN %d IP %s\n",                   current_hostname,                  cinfo->h_name,                   cinfo->h_length,                   inet_ntoa( current_hostaddr ) );	      }        else // failed lookup - stick with the last host	      {          printf( "Can't resolve hostname \"%s\" in world file."                  " Sticking with \"%s\".\n",                   candidate_hostname, current_hostname );        }      }      else        puts( "No hostname specified. Ignoring." );      continue;    }        // otherwise it's a device so we handle those...    // Find the parent entity    CEntity *parent = root->FindSectionEntity( this->worldfile->GetEntityParent(section) );        // Work out whether or not its a local device if any if this    // device's host IPs match this computer's IP, it's local    bool local = m_hostaddr.s_addr == current_hostaddr.s_addr;    // Create the entity - it attaches itself to its parent's child_list    CEntity *entity = NULL;        assert( (entity = lib->CreateEntity( (char*)type, this, parent )) );     if (entity != NULL)    {            // these pokes should really be in the entities, but it's a loooooot      // of editing to change each constructor...      // store the IP of the computer responsible for updating this      memcpy( &entity->m_hostaddr, &current_hostaddr,sizeof(current_hostaddr));            // if true, this instance of stage will update this entity      entity->m_local = local;      //printf( "ent: %p host: %s local: %d\n",      //    entity, inet_ntoa(entity->m_hostaddr), entity->m_local );            // Store which section it came from (so we know where to      // save it to later).      entity->worldfile_section = section;       // Let the entity load itself      if (!entity->Load(this->worldfile, section))      {        PRINT_ERR1( "Failed to load entity %d from world file",                     GetEntityCount() );         return false;      }    }    else      PRINT_ERR2("line %d : unrecognized type [%s]", line, type);  }    // disconnect from the nameserver  endhostent();  // see if the world size is specified explicitly  double global_maxx = worldfile->ReadTupleLength(0, "size", 0, 0.0 );  double global_maxy = worldfile->ReadTupleLength(0, "size", 1, 0.0 );  // find the maximum dimensions of all objects and grow the  // world size if anything starts out of bounds    double xmin, ymin;  // the bounding box routine stretches the given dimensions to fit  // the entity and its children  xmin = ymin = 999999.9;  root->GetBoundingBox( xmin, ymin, global_maxx, global_maxy );    // Initialise the matrix, now that we know how big it has to be  int w = (int) ceil( global_maxx * this->ppm);  int h = (int) ceil( global_maxy * this->ppm);    assert( this->matrix = new CMatrix(w, h, 1) );      // draw a rectangle of boundary pixels around the outside of the matrix    Rect r;  r.toplx = 1;  r.toply = 1;  r.botlx = 1;  r.botly = h-2;  r.toprx = w-2;  r.topry = 1;  r.botrx = w-2;  r.botry = h-2;  matrix->draw_rect( r, root, true );  #ifdef DEBUG  // prints a minimal description of the ent tree on the console  root->Print( "" );#endif  // inherit  CWorld::Load();  return true;}//////////////////////////////////////////////////////////////////////// Save the world filebool CStageServer::Save( void ){  // Store the new name of the world file (for Save As).  //if (filename != NULL)  //{  //this->worldfilename[0] = 0;  //strcpy(this->worldfilename, filename);  //assert(this->worldfilename[0] != 0);  // }    PRINT_MSG1("saving world to [%s]", this->worldfilename);    // Let each entity save itself  if (!root->Save(this->worldfile, root->worldfile_section))    return false;    // Save everything  if (!this->worldfile->Save(this->worldfilename))    return false;  return CWorld::Save();}//////////////////////////////////////////////////////////////////////// SET UP THE SERVER TO ACCEPT INCOMING CONNECTIONSbool CStageServer::SetupConnectionServer( void ){  m_pose_listen.fd = socket(AF_INET, SOCK_STREAM, 0);  m_pose_listen.events = POLLIN; // notify me when a connection request happens    struct sockaddr_in servaddr;    bzero(&servaddr, sizeof(servaddr));    servaddr.sin_family      = AF_INET;  servaddr.sin_addr.s_addr = htonl(INADDR_ANY);  servaddr.sin_port        = htons(m_port);    // switch on the re-use-address option  const char on = 1;  setsockopt( m_pose_listen.fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on) );    if( bind(m_pose_listen.fd, (SA *) &servaddr, sizeof(servaddr) )  < 0 )  {    perror("CStageServer::SetupConnectionServer()");    std::cout << "Port " << m_port          << " is in use. Quitting (but try again in a few seconds)."          << std::endl;    exit( -1 );  }    // catch signals generated by socket closures  signal( SIGPIPE, CatchSigPipe );    // listen for requests on this socket  // we poll it in ListenForPoseConnections()  assert( listen( m_pose_listen.fd, LISTENQ) == 0 );  return true;}////////////////////////////////////////////////////////////////////////// CREATE THE DEVICE DIRECTORY for external IO// everything makes its external devices in directory m_device_dirbool CStageServer::CreateDeviceDirectory( void ){  // get the user's name  struct passwd* user_info = getpwuid( getuid() );    // get the pid  pid_t pid = getpid();    // make a device directory from a base name and the user's name  //sprintf( m_device_dir, "%s.%s.%d",            //IOFILENAME, user_info->pw_name, m_instance++ );  sprintf( m_device_dir, "%s.%s.%d.%d",           IOFILENAME, user_info->pw_name, pid ,m_instance++ );    if( mkdir( m_device_dir, S_IRWXU | S_IRWXG | S_IRWXO ) == -1 )  {    if( errno == EEXIST )      PRINT_WARN( "Device directory exists. Possible error?" );    else     {      PRINT_ERR1( "Failed to make device directory:%s", strerror(errno) );      return false;    }        }  return true;}bool CStageServer::ParseCmdLine( int argc, char** argv ){  // we've loaded the worldfile, now give the cmdline a chance to change things  for( int a=1; a<argc-1; a++ )  {    // DIS/ENABLE Player    if((strcmp( argv[a], "-n" ) == 0 )||        (strcmp( argv[a], "--noplayer" ) == 0))    {      m_run_player = false;      printf( "[No Player]" );    }    // FAST MODE - run as fast as possible - don't attempt t match real time    if((strcmp( argv[a], "--fast" ) == 0 ) ||        (strcmp( argv[a], "-f" ) == 0))    {      m_real_timestep = 0.0;      printf( "[Fast]" );    }    // set the stop time    if(!strcmp(argv[a], "-t"))      {	m_stoptime = atoi(argv[++a]);	printf("[Stop time: %d]",m_stoptime);      }        // START WITH CLOCK STOPPED    if( strcmp( argv[a], "-s" ) == 0 )      {	this->start_disabled = true;	printf( "[Clock stopped (start with SIGUSR1)]" );      }  }    return true;}// do all the special server startup, then call the parent's Startup()bool CStageServer::Startup( void ){        ///////////////////////////////////////////////////////////////////////////  // Create the device directory, clock and lock devices   if( !CreateDeviceDirectory() )  {    PRINT_ERR( "Failed to create device directory" );    quit = true;    return false;  }  if( !CreateClockDevice() )  {    PRINT_ERR( "Failed to create clock device" );    quit = true;    return  false;  }    if( !CreateLockFile() )  {    PRINT_ERR( "Failed to create lock file" );    quit = true;    return  false;  }    //////////////////////////////////////////////////////////////////////  // SET UP THE SERVER TO ACCEPT INCOMING CONNECTIONS  /*    if( !SetupConnectionServer() )  {    quit = true;    return false;  }  // just to be reassuring, print the host details  printf( "[Server %s:%d]",  m_hostname, m_port );    */  // inherit parent's method  CWorld::Startup();  // See if there was anything we didnt understand in the world file  this->worldfile->WarnUnused();  ////////////////////////////////////////////////////////////////////  // STARTUP PLAYER  //puts( "" ); // end the startup line, flush stdout before starting player  if( m_run_player && !StartupPlayer() )