world.cc

一个机器人平台

  m_sim_timeval.tv_sec = (long)floor(m_sim_time);
  m_sim_timeval.tv_usec = (long)((m_sim_time-floor(m_sim_time)) * MILLION); 

  return m_sim_time;
}

///////////////////////////////////////////////////////////////////////////
// Get the sim time
// Returns time in sec since simulation started
double CWorld::GetTime()
{
  return m_sim_time;
}


///////////////////////////////////////////////////////////////////////////
// Get the real time
// Returns time in sec since simulation started
double CWorld::GetRealTime()
{
  struct timeval tv;
  gettimeofday( &tv, NULL );
  double time = tv.tv_sec + (tv.tv_usec / 1000000.0);
  return time - m_start_time;
}


///////////////////////////////////////////////////////////////////////////
// Set a rectangle in the world grid
void CWorld::SetRectangle(double px, double py, double pth,
                          double dx, double dy, CEntity* ent, bool add)
{
  Rect rect;

  dx /= 2.0;
  dy /= 2.0;

  double cx = dx * cos(pth);
  double cy = dy * cos(pth);
  double sx = dx * sin(pth);
  double sy = dy * sin(pth);
    
  rect.toplx = (int) ((px + cx - sy) * ppm);
  rect.toply = (int) ((py + sx + cy) * ppm);

  rect.toprx = (int) ((px + cx + sy) * ppm);
  rect.topry = (int) ((py + sx - cy) * ppm);

  rect.botlx = (int) ((px - cx - sy) * ppm);
  rect.botly = (int) ((py - sx + cy) * ppm);

  rect.botrx = (int) ((px - cx + sy) * ppm);
  rect.botry = (int) ((py - sx - cy) * ppm);
    
  //printf( "draw_rect %d,%d %d,%d %d,%d %d,%d\n",
  //  rect.toplx, rect.toply,
  //  rect.toprx, rect.topry,
  //  rect.botlx, rect.botly,
  //  rect.botrx, rect.botry );

  matrix->draw_rect( rect, ent, add );
}


///////////////////////////////////////////////////////////////////////////
// Set a circle in the world grid
void CWorld::SetCircle(double px, double py, double pr, CEntity* ent,
                       bool add )
{
  // Convert from world to image coords
  int x = (int) (px * ppm);
  int y = (int) (py * ppm);
  int r = (int) (pr * ppm);
    
  matrix->draw_circle( x,y,r,ent, add);
}


///////////////////////////////////////////////////////////////////////////
// Add an entity to the array
// returns its array index which the entity uses as a unique ID
stage_id_t CWorld::RegisterEntity( CEntity* ent )
{
  // if we have no space left in the array, allocate another chunk
  if (this->entity_count >= this->entities_size)
    { 
      this->entities_size += 100; // a good chunk - one realloc will do for most people
      this->entities = (CEntity**)
	realloc(this->entities, this->entities_size * sizeof(this->entities[0]));
    }
  
  stage_id_t id = entity_count++; // record the id BEFORE incrementing it
  
  this->entities[id] = ent; // store the pointer
  
  return id; // return the unique id/index
}

// return the entity with matching id/index, or NULL if no match
CEntity* CWorld::GetEntity( int id )
{ 
  // bounds check
  if( id < 0 || id >= this->entity_count )
    return NULL;
  
  return( this->entities[id] );
}

// returns true if the given hostname matches our hostname, false otherwise
//  bool CWorld::CheckHostname(char* host)
//  {
//    //printf( "checking %s against (%s and %s) ", 
//    //  host, m_hostname, m_hostname_short ); 

//    if(!strcmp(m_hostname,host) || !strcmp(m_hostname_short,host))
//    {
//      //PRINT_DEBUG( "TRUE" );
//      return true;
//    }
//    else
//    {
//      //PRINT_DEBUG( "FALSE" );
//      return false;
//    }
//  }


void CWorld::Output()
{
  // comms used
  static unsigned long last_input = 0;
  static unsigned long last_output = 0;
  unsigned int bytes_in = g_bytes_input - last_input;
  unsigned int bytes_out = g_bytes_output - last_output;
  static int bytes_accumulator = 0;
  
  // count the data
  bytes_accumulator += bytes_in + bytes_out;

  // measure frequency & bandwidth
  static double freq = 0.0;
  static double bandw = 0.0;

  static int updates = 0;
  static double lasttime = GetRealTime();
  double interval = GetRealTime() - lasttime;

  // count this update
  updates++;
  
  if( interval > 2.0 ) // measure freq + bandwidth every 2 seconds
    {
      lasttime += interval;

      bandw = (double)bytes_accumulator / interval;
      bytes_accumulator = 0;
      
      freq = (double)updates / interval;
      updates = 0;    
    }
  
  if( m_console_output )
    ConsoleOutput( freq, bytes_in, bytes_out, bandw );
  
  
  if( m_log_output ) 
    LogOutput( freq, bytes_in, bytes_out, g_bytes_input, g_bytes_output );
  
  last_input = g_bytes_input;
  last_output = g_bytes_output; 
}

void CWorld::ConsoleOutput( double freq, 
			    unsigned int bytes_in, unsigned int bytes_out,
			    double avg_data)
{
  char lineend = '\r';
  //char lineend = '\n';

  printf( " Step: %u Time: %8.1f - %7.1fHz - [%4u/%4u] %8.2f b/sec%c", 
	  m_step_num,
	  m_sim_time, 
          freq,
	  bytes_in, bytes_out, 
          avg_data,
	  lineend );
  
  fflush( stdout );
  
}

bool CWorld::Load( void )
{
  ///////////////////////////////////////////////////////////////////////
  // LOAD THE CONFIGURATION FOR THE GUI 
  // we call this *after* the world has loaded, so we can configure the menus
  // correctly

  PRINT_DEBUG( "WORLD LOAD" );

  if(this->enable_gui ) GuiLoad( this );
  else
    PRINT_DEBUG( "NOT LOADING GUI" );
  
  return true; // success
}

bool CWorld::Save( )
{
  if( this->enable_gui ) GuiSave( this );

  return true; // success
}

void CWorld::LogOutput( double freq,
			unsigned int bytes_in, unsigned int bytes_out, 
			unsigned int total_bytes_in, 
			unsigned int total_bytes_out )
{  
  assert( m_log_fd > 0 );
  
  char line[512];
  sprintf( line,
           "%u\t\t%.3f\t\t%u\t%u\t%u\t%u\n", 
           m_step_num, m_sim_time, // step and time
           //loop_duration, // real cycle time in ms
           //sleep_duration, // real sleep time in ms
           //m_sim_timestep / sleep_duration, // ratio
           bytes_in, // bytes in this cycle
           bytes_out, // bytes out this cycle
           total_bytes_in,  // total bytes in
           total_bytes_out); // total bytes out
  
  write( m_log_fd, line, strlen(line) );
}


void CWorld::LogOutputHeader( void )  
{
  int log_instance = 0;
  while( m_log_fd < 0 )
    {
      char fname[256];
      sprintf( fname, "%s.%d", m_log_filename, log_instance++ );
      m_log_fd = open( fname, O_CREAT | O_EXCL | O_WRONLY, 
		       S_IREAD | S_IWRITE );
    }

  struct timeval t;
  gettimeofday( &t, 0 );
      
  // count the locally managed entities
  int m=0;
      
  char* tmstr = ctime((const time_t*)&t.tv_sec);
  tmstr[ strlen(tmstr)-1 ] = 0; // delete the newline
      
  char line[512];
  sprintf( line,
           "# Stage output log\n#\n"
           "# Command:\t%s\n"
           "# Date:\t\t%s\n"
           "# Host:\t\t%s\n"
           //"# Bitmap:\t%s\n"
           "# Timestep(ms):\t%d\n"
           "# Entities:\t%d of %d\n#\n"
           "#STEP\t\tSIMTIME(s)"
	   //"\tINTERVAL(s)\tSLEEP(s)\tRATIO\t"
           "\tINPUT\tOUTPUT\tITOTAL\tOTOTAL\n",
           m_cmdline, 
           tmstr, 
           m_hostname, 
           //worldfilename,
           (int)(m_sim_timestep * 1000.0),
           m, 
           this->entity_count );
      
  write( m_log_fd, line, strlen(line) );
}

// return the entity nearest the specified point, but not more than range m away,
// that has the specified parent 
CEntity* CWorld::GetNearestChildWithinRange( double x, double y, double range, 
					     CEntity* parent )
{

  //printf( "Searching from %.2f,%.2f for children of %p\n", x, y, parent );

  CEntity* nearest = NULL;
  
  double px, py, pth;
  double d;

  for( int c=0; c<this->entity_count; c++ )
    {
      CEntity* ent = this->entities[c];
      
      // we can only select items with root a parent
      if( ent->m_parent_entity != parent )
	continue;
      
      ent->GetGlobalPose( px, py, pth );
      
      d = hypot( py - y, px - x );
      
      //printf( "Entity type %s is %.2fm away at  %.2f,%.2f\n", 
      //      ent->GetToken(), d, px, py );
      
      if( d < range )
	{
	  range = d;
	  nearest = ent;
	}
    }
  
  //if( nearest )
  //printf ( "Nearest is type %s\n", nearest->GetToken() );
  //else
  //puts( "no entity within range" );

  return nearest;
}

// return the entity nearest the specified point, but not more than range m away,
// that has the specified parent 
CEntity* CWorld::GetNearestEntityWithinRange( double x, double y, double range )		
{
  CEntity* nearest = NULL;
  double px, py, pth;
  double d;
  
  for( int c=0; c<this->entity_count; c++ )
    {
      CEntity* ent = this->entities[c];
      
      ent->GetGlobalPose( px, py, pth );
      
      d = hypot( py - y, px - x );
      
      //printf( "Entity type %s is %.2fm away at  %.2f,%.2f\n", 
      //      ent->token, d, px, py );
      
      if( d < range )
	{
	  range = d;
	  nearest = ent;
	}
    }
  
  if( nearest )
    printf ( "Nearest is %s\n", nearest->lib_entry->token );
  else
    puts( "no entity within range" );
  
  return nearest;
}

///////////////////////////////////////////////////////////////////////////
// lock the shared mem
//
bool CWorld::LockByte( int offset )
{
  return true; // success
}

///////////////////////////////////////////////////////////////////////////
// unlock the shared mem
//
bool CWorld::UnlockByte( int offset )
{
  return true; // success
}