server.cc

一个机器人平台

  {
    PRINT_ERR("Player startup failed");
    quit = true;
    return false;
  }
  
  if( m_real_timestep > 0.0 ) // if we're in real-time mode
    this->StartTimer( m_real_timestep ); // start the real-time interrupts going

  if( !start_disabled )
    m_enable = true;  
  
  return true;
}

///////////////////////////////////////////////////////////////////////////
// Start the single instance of player
bool CStageServer::StartupPlayer( void )
{
  PRINT_DEBUG( "** STARTUP PLAYER **" );

  // ----------------------------------------------------------------------
  // fork off a player process to handle robot I/O
  if( (this->player_pid = fork()) < 0 )
  {
    PRINT_ERR1("error forking for player: [%s]", strerror(errno));
    return false;
  }

  // If we are the nmew child process...
  if (this->player_pid == 0)
  {
    // release controlling tty so Player doesn't get signals
    setpgid(0,0);

    // call player like this:
    // player -stage <device directory>
    // player will open every file in the device directory
    // and attempt to memory map it as a device. 

    // Player must be in the current path
    if( execlp( "player", "player",
                "-s", DeviceDirectory(), 
                (strlen(m_auth_key)) ? "-k" : NULL,
                (strlen(m_auth_key)) ? m_auth_key : NULL,
                NULL) < 0 )
    {
      PRINT_ERR1("error executing player: [%s]\n"
                 "Make sure player is in your path.", strerror(errno));
      if(!kill(getppid(),SIGINT))
      {
        PRINT_ERR1("error killing player: [%s]", strerror(errno));
        exit(-1);
      }
    }
  }
  
  //PRINT_DEBUG( "** STARTUP PLAYER DONE **" );
  return true;
}


///////////////////////////////////////////////////////////////////////////
// Stop player instance
void CStageServer::ShutdownPlayer()
{
  int wait_retval;

  if(this->player_pid)
  {
    if(kill(this->player_pid,SIGTERM))
      PRINT_ERR1("error killing player: [%s]", strerror(errno));
    if((wait_retval = waitpid(this->player_pid,NULL,0)) == -1)
      PRINT_ERR1("waitpid failed: [%s]", strerror(errno));
    this->player_pid = 0;
  }
}

///////////////////////////////////////////////////////////////////////////
// Update the world
void CStageServer::Update(void)
{
  //PRINT_DEBUG( "** Server Update **" );
  //assert( arg == 0 );
  
  //while( !quit )

  //printf( "update: enable: %d g_timer_events: %d m_real_timestep: %.3f\n",
  //  m_enable, g_timer_events, m_real_timestep );

  // get and handle any commands and updates from clients

  this->Read();

  // if the sim isn't running, we pause briefly and return
  //if( !m_enable )
  //{
  //  usleep( 100000 );
  //  return;
  // }

  // is it time to stop?
  if(m_stoptime && m_sim_time >= m_stoptime)
    {   
      quit = true;
      return;
    }
      

  // let the entities do anything they want to do between clock increments
  root->Sync(); 
      
  // if the timer has gone off recently or we're in fast mode
  // we increment the clock and do the time-based updates
  if( g_timer_events > 0 || m_real_timestep == 0 )
    {          
      CWorld::Update();
      
      if( g_timer_events > 0 )
	g_timer_events--; // we've handled this timer event
      
      Write(); // write any changes back out to clients
      Output(); // perform console and log output
    }
  
  
  // if there's nothing pending and we're not in fast mode, we let go
  // of the processor (on linux gives us around a 10ms cycle time)
  if( g_timer_events < 1 && m_real_timestep > 0.0 ) 
    usleep( 0 );

  // dump the contents of the matrix to a file for debugging
  //world->matrix->dump();
  //getchar();	
}


// read stuff until we get a continue message on each channel
int CStageServer::Read( void )
{
  //PRINT_DEBUG( "Server::Read()" );

  // look for new connections 
  ListenForConnections();
  
  return CStageIO::Read();  
}


   
void CStageServer::ListenForConnections( void )
{
  int readable = 0;
  
  // poll for connection requests with a very fast timeout
  if((readable = poll( &m_pose_listen, 1, 0 )) == -1)
  {
    if( errno != EINTR ) // timer interrupts are OK
    { 
      perror( "Stage warning: poll error (not EINTR)");
      return;
    }
  }
  
  bool success = true;

  // if the socket had a request
  if( readable && (m_pose_listen.revents & POLLIN ) ) 
  {
    // set up a socket for this connection
    struct sockaddr_in cliaddr;  
    bzero(&cliaddr, sizeof(cliaddr));
#if PLAYER_SOLARIS
    int clilen;
#else
    socklen_t clilen;
#endif

    clilen  = sizeof(cliaddr);
    int connfd = 0;
      
    connfd = accept( m_pose_listen.fd, (SA *) &cliaddr, &clilen);
      

    // set the dirty flag for all entities on this connection
    DirtyEntities( m_pose_connection_count );


    // determine the type of connection, sync or async, by reading
    // the first byte
    char b = 0;
    int r = 0;

    if( (r = read( connfd, &b, 1 )) < 1 ) 
    {
      puts( "failed to read sync type byte. Quitting\n" );
      if( r < 0 ) perror( "read error" );
      exit( -1 );
    }
      
    // record the total bytes input
    g_bytes_input += r; 


    // if this is a syncronized connection, increase the sync counter 
    switch( b )
    {
      case STAGE_SYNC: 
        m_sync_counter++; 
#ifdef VERBOSE      
        printf( "\nStage: SYNC pose connection accepted (id: %d fd: %d)\n", 
                m_pose_connection_count, connfd );
        fflush( stdout );
#endif            
        if( m_external_sync_required ) m_enable = true;

        success = true;
        break;
      case STAGE_ASYNC:
        //default:
#ifdef VERBOSE      
        printf( "\nStage: ASYNC pose connection accepted (id: %d fd: %d)\n", 
                m_pose_connection_count, connfd );
        fflush( stdout );
#endif            
        success = true;
        break;
	  
      default: printf( "Stage: unknown sync on %d. Closing connection\n",
                       connfd  );
        close( connfd );
        success = false;
        break;
    }
   
    if( success )
    {
      // add the new connection to the arrays
      // store the connection type to help us destroy it later
      m_conn_type[ m_pose_connection_count ] = b;
      // record the pollfd data
      m_pose_connections[ m_pose_connection_count ].fd = connfd;
      m_pose_connections[ m_pose_connection_count ].events = POLLIN;
      m_pose_connection_count++;
    }
  }
}


bool CStageServer::Shutdown()
{
  int res = CWorld::Shutdown();

  PRINT_DEBUG( "server shutting down" );

  ShutdownPlayer();

  // zap the clock device and lock file 
  unlink( clockName );
  unlink( m_locks_name );

  // delete the device directory
  if( rmdir( m_device_dir ) != 0 )
    PRINT_WARN2("failed to delete device directory \"%s\" [%s]",
                m_device_dir,
                strerror(errno));

  return res;
}

//////////////////////////////////////////////////////////////////////////
// Set up a file for record locking, 1 byte per entity
// the contents aren't used - we just use fcntl() to lock bytes
bool CStageServer::CreateLockFile( void )
{
  // store the filename
  sprintf( m_locks_name, "%s/%s", m_device_dir, STAGE_LOCK_NAME );

  m_locks_fd = -1;

  if( (m_locks_fd = open( m_locks_name, O_RDWR | O_CREAT | O_TRUNC, 
                          S_IRUSR | S_IWUSR )) < 0 )
  {
    perror("Failed to create lock device" );
    return false;
  } 
  
  // set the file size - 1 byte per entity + 1 for the clock
  off_t sz = GetEntityCount() + 1;
  
  if( ftruncate( m_locks_fd, sz ) < 0 )
  {
    perror( "Failed to set lock file size" );
    return false;
  }

  PRINT_DEBUG3( "Created lock file %s of %d bytes (fd %d)\n", 
		m_locks_name, GetEntityCount(), m_locks_fd );
  
  return true;
}

///////////////////////////////////////////////////////////////////////////
// lock the shared mem
//
bool CStageServer::LockByte( int offset )
{
  assert( this->m_locks_fd > 0 );  // must have a lock file open

  // POSIX RECORD LOCKING METHOD
  struct flock cmd;
  
  cmd.l_type = F_WRLCK; // request write lock
  cmd.l_whence = SEEK_SET; // count bytes from start of file
  cmd.l_start = offset; // lock my unique byte
  cmd.l_len = 1; // lock 1 byte
  
  fcntl( this->m_locks_fd, F_SETLKW, &cmd );
  
#ifdef DEBUG
  // DEBUG: write into the file to show which byte is locked
  // X = locked, '_' = unlocked
  lseek( this->m_locks_fd, offset, SEEK_SET );
  write(  this->m_locks_fd, "X", 1 );
  //printf( "locking byte %d\n", offset );
#endif

  return true;
}

///////////////////////////////////////////////////////////////////////////
// unlock the shared mem
//
bool CStageServer::UnlockByte( int offset )
{
  assert( this->m_locks_fd > 0 ); // if the world must have a locking file open

  // POSIX RECORD LOCKING METHOD
  struct flock cmd;
  
  cmd.l_type = F_UNLCK; // request unlock
  cmd.l_whence = SEEK_SET; // count bytes from start of file
  cmd.l_start = offset; // unlock my unique byte
  cmd.l_len = 1; // unlock 1 byte
  
  fcntl( this->m_locks_fd, F_SETLKW, &cmd );
  
#ifdef DEBUG
  // DEBUG: write into the file to show which byte is locked
  // X = locked, '_' = unlocked
  lseek( this->m_locks_fd, offset, SEEK_SET );
  write(  this->m_locks_fd, "_", 1 );
  //printf( "unlocking byte %d\n", offset );
#endif
  
  return true;
}

double CStageServer::SetClock( double interval, uint32_t step_num )
{
  // set the current time internally 
  double retval = CWorld::SetClock( interval, step_num );

  // set the current time in shared mem
  this->LockByte( clock_lock_byte );
  m_clock->time.tv_sec = m_sim_timeval.tv_sec;
  m_clock->time.tv_usec = m_sim_timeval.tv_usec;
  this->UnlockByte( clock_lock_byte );

  return retval;
}


void CStageServer::Write( void )
{
  //PRINT_DEBUG( "SERVER WRITE" );
  
  // if player has changed the subscription count, we make the property dirty
  
  // fix this - it limits the number of entities
  // just move the memory into the entity
  static int subscribed_last_time[ 10000 ]; 
  static bool init = true;
  
  // is this necessary? can't hurt i guess.
  // first time round, we zero the static buffer
  if( init ) 
  {
    memset( subscribed_last_time, false, 10000 );
    init = false;
  }
  
      // TODO - FIX THIS
  /*
    // manage subscriptions only for CPlayerEntity
    if( RTTI_ISTYPE( CPlayerEntity*, ent ) ) 
    {
    CPlayerEntity* pent = (CPlayerEntity*)ent;
    
    //PRINT_DEBUG1( "checking subscription for entity %d", i );
    
    int currently_subscribed =  pent->Subscribed();
    
      //PRINT_DEBUG3( "Entity %d subscriptions: %d last time: %d\n", 
      //	    i, currently_subscribed, subscribed_last_time[i] );
      
      // if the current subscription state is different from the last time
      if( currently_subscribed != ent->subscribed_last_time[i] )
	    {
	      
	      PRINT_DEBUG2( "Entity %d subscription change (%d subs)\n", 
			    i, pent->Subscribed() );
	      
	      // remember this state for next time
	      subscribed_last_time[i] = currently_subscribed;
	      
	      // mark the subscription property as dirty so we pick it up
	      // below
	      pent->SetDirty( PropPlayerSubscriptions , 1);
	    }
	  
	}
    }
  */  

  CStageIO::Write();
}

/////////////////////////////////////////////////////////////////////////
// Clock device -- create the memory map for IPC with Player 
bool CStageServer::CreateClockDevice( void )
{   
  size_t clocksize = sizeof( stage_clock_t );
  
  sprintf( clockName, "%s/clock", m_device_dir );
  
  int tfd=-1;
  if( (tfd = open( clockName, O_RDWR | O_CREAT | O_TRUNC, 
                   S_IRUSR | S_IWUSR )) < 0 )
  {
    PRINT_ERR1("Failed to open clock device file:%s", strerror(errno) );
    return false;
  } 
  
  // make the file the right size
  off_t sz = clocksize;

  if( ftruncate( tfd, sz ) < 0 )
  {
    PRINT_ERR1( "Failed to set clock file size:%s", strerror(errno) );
    return false;
  }
  
  if( write( tfd, &m_sim_timeval, sizeof(m_sim_timeval)) < 0 )
  {
    PRINT_ERR1( "Failed to write time into clock device:%s", strerror(errno) );
    return false;
  }

  void *map = mmap( NULL, clocksize, 
                    PROT_READ | PROT_WRITE, MAP_SHARED,
                    tfd, (off_t) 0);
  
  if (map == MAP_FAILED )
  {
    PRINT_ERR1( "Failed to map clock device memory:%s", strerror(errno) );
    return false;
  }
  
  // Initialise space
  //
  memset( map, 0, clocksize );

  // store the pointer to the clock
  m_clock = (stage_clock_t*)map;
  
  this->clock_lock_byte = this->GetEntityCount()+1;

  PRINT_DEBUG1( "Clock lock byte: %d\n", this->clock_lock_byte ) ;

  close( tfd ); // can close fd once mapped
  
  PRINT_DEBUG( "Successfully mapped clock device." );
  
  return true;
}

// sleep until a signal goes off
// return the time in seconds we spent asleep
double CStageServer::Pause()
{
  // we're too busy to sleep!
  if( m_real_timestep == 0 || --g_timer_events > 0  )
    return 0;
  
  // otherwise

  double sleep_start = GetRealTime();

  pause(); // wait for the signal

  return( GetRealTime() - sleep_start );
}

void CStageServer::StartTimer( double interval )
{
  // set up the interval timer
  //
  // set a timer to go off every few ms. in realtime mode we'll sleep
  // in between if there's nothing else to do. 

  //install signal handler for timing
  if( signal( SIGALRM, &TimerHandler ) == SIG_ERR )
    {
      PRINT_ERR("failed to install signal handler");
      exit( -1 );
    }

  //printf( "interval: %f\n", interval );

  //start timer with chosen interval (specified in milliseconds)
  struct itimerval tick;
  // seconds
  tick.it_value.tv_sec = tick.it_interval.tv_sec = (long)floor(interval);
  // microseconds
  tick.it_value.tv_usec = tick.it_interval.tv_usec = 
    (long)fmod( interval * MILLION, MILLION); 
  
  if( setitimer( ITIMER_REAL, &tick, 0 ) == -1 )
    {
      PRINT_ERR("failed to set timer");
      exit( -1 );
    }
}