server.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: 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 func
void 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 file
bool 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 file
bool 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 CONNECTIONS
bool 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_dir
bool 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() )