main.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  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: Program entry point
 * Author: Andrew Howard
 * Date: 11 May 2003
 * CVS: $Id: main.cc,v 1.62 2006/02/14 03:00:25 natepak Exp $
 */


/** @page gazebo_server Console-mode server (gazebo)

The basic gazebo server is a console-mode application: it creates no
windows and accepts no user input.  The console-mode server is useful
for running automated tests and batch experiments.

For an interactive GUI interface, see the documentation @ref
gazebo_wx.

The server is run as follows:

@verbatim
$ gazebo [options] <worldfile>
@endverbatim

where [options] is one or more of the following:

- -h            : Print usage message.
- -s &lt;id&gt;       : Use server id &lt;id&gt; (an integer); default is 0
- -f                  : Force usage of the server id (use with caution)
- -d &lt;level&gt;    : Verbose mode: -1 = none, 0 = critical messages (default), 9 = all messages
- -t &lt;sec&gt;      : Timeout and quit after &lt;sec&gt; seconds
- -l &lt;logfile&gt;  : Log messages to &lt;logfile&gt 
- -n                  : Do not do any time control

The server prints some diagnostic information to the console before
starting the main simulation loop.  Check carefully for any warnings
that are printed at this stage; common warnings include:

- Invalid tags or attributes in the world file, which yield "unused
  attribute" warnings:
@verbatim
warning : in worlds/pioneer2at.world:14 unused attribute [xuz]
@endverbatim
  To remove these warnings, fix the world file.

- Left-over files from an earlier instance of the server (that
  crashed).
@verbatim
gz_server.c:111 directory [/tmp/gazebo-ahoward-0] already exists
@endverbatim
  To remove this warning, delete the indicated directory.

While the simulation loop is running, basic status information is
printed on the console:
  
@verbatim
Time 1.542 1.560 0.000 [1.000] 0.220 [ 14%]
@endverbatim

These five fields specify, in order:

- The elapsed real time (sec).

- The elapsed simulation time (sec).

- The accumulated pause time (sec).

- The ratio of elapsed simulation to elaped real time; i.e., the
effective speed of the simulator.  This should hover around target
speed as specified in the world file, but may fall below this value if
your processor is too slow.

- The total CPU time used by the server; useful for performance
monitoring.

- The CPU utilization, measured as ratio of total CPU time to elapsed
real time.  Note that this measures the utilization by the server process
only; it does not measure the processor utilization of the X server,
which may be significant.

The server can be terminated with @c control-C.  Errors, warnings and
messages are appended by default to a file called @c .gazebo located in your
home directory, or to the log file specified with the -l command line option.
*/

#if HAVE_CONFIG_H
  #include <config.h>
#endif

#include <unistd.h>
#include <stdio.h>
#include <time.h>
#include <signal.h>
#include <errno.h>
#include <sys/times.h>
#include <X11/Xlib.h>
#include <GL/glut.h>

#include "gazebo.h"
#include "Global.hh"
#include "Error.hh"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"

// Local funcs
static bool MainIdle();

// Local vars
static World* world;
static WorldFile* worldFile;

// For load measurements
static double cpuStartTime;
static double cpuRealTime;

// Command line options
const char *worldFileName;
const char *optDisplay = NULL;
const char *optLogFileName = NULL;
int optServerId = 0;
bool optServerForce = true;
double optTimeout = -1;
int optMsgLevel = 1;
int optTimeControl = 1;

// X Display
Display *display;

// Quit command from user interface
bool userQuit = false;



void PrintUsage()
{
  fprintf(stderr, "Usage: gazebo [-hv] <worldfile>\n");
  fprintf(stderr, "  -h            : Print this message.\n");
  fprintf(stderr, "  -s <id>       : Use server id <id> (an integer); default is 0.\n");
  fprintf(stderr, "  -f            : Force usage of the server id (use with caution)\n");
  fprintf(stderr, "  -d <-1:9>      : Verbose mode: -1 = none, 0 = critical (default), 9 = all)\n");
  fprintf(stderr, "  -t <sec>      : Timeout and quit after <sec> seconds\n");
  fprintf(stderr, "  -l <logfile>  : Log to indicated file.\n");
  fprintf(stderr, "  -n            : Do not do any time control\n");
  fprintf(stderr, "  <worldfile>   : load the the indicated world file\n");
  return;
}


// Print the version/licence string
void PrintVersion()
{
  fprintf(stderr, "Gazebo multi-robot simulator, version %s\n\n", VERSION);
  fprintf(stderr, "Part of the Player/Stage Project "
          "[http://playerstage.sourceforge.net].\n");
  fprintf(stderr, "Copyright (C) 2003 Nate Koenig, Andrew Howard, and contributors.\n");
  fprintf(stderr, "Released under the GNU General Public License.\n\n");
  return;
}


// Parse the argument list.  Options are placed in static variables.
int ParseArgs(int argc, char **argv)
{
  FILE *tmpFile;
  int ch;
  char *flags = "l:hd:s:fg:xt:nq";

  // Default display string the environment value
  optDisplay = getenv("DISPLAY");

  // Get letter options
  while ((ch = getopt(argc, argv, flags)) != -1)
  {
    switch (ch)
    {
      case 'd':
        // Verbose mode
        optMsgLevel = atoi(optarg);
        break;
 
      case 'f':
        // Force server id
        optServerForce = true;
        break;

      case 'l':
        optLogFileName = optarg;
        break;

      case 's':
        // Server id
        optServerId = atoi(optarg);
        optServerForce = false;
        break;
       
      case 't':
        // Timeout and quit after x seconds
        optTimeout = atof(optarg);
        break;
      case 'n':
        optTimeControl = 0;
      break;

      case 'h':
      default:
        PrintUsage();
        return -1;
    }
  }

  argc -= optind;
  argv += optind;

  if (argc < 1)
  {
    PrintUsage();
    return -1;
  }

  // Get the world file name
  worldFileName = argv[0];
  tmpFile = fopen(worldFileName, "r");

  if (tmpFile == NULL)
  {
    char tmpStr[256];
    sprintf(tmpStr, "WorldFile [%s]", worldFileName);

    perror(tmpStr);
    return  -1;
  }
  fclose(tmpFile);
 
  return 0;
}


// sighandler to shut everything down properly
void SignalHandler( int dummy ) 
{
  userQuit = true;
  return;
}


// Pre-initialization
int PreInit(int *argc, char** argv)
{
  // Initialize error handling in the server
  ErrorInit(optMsgLevel, optLogFileName);

  // Initialize error handling in libgazebo
  gz_error_init(true, optMsgLevel);

  // Print banner
  PRINT_MSG1(0, "** Gazebo %s **", VERSION);
  PRINT_MSG0(0,"* Part of the Player/Stage Project [http://playerstage.sourceforge.net].");
  PRINT_MSG0(0,"* Copyright 2000-2005 Brian Gerkey, Richard Vaughan, Andrew Howard,\n* Nate Koenig and contributors.");
  PRINT_MSG0(0,"* Released under the GNU General Public License.");

  // Establish signal handlers
  if (signal(SIGINT, SignalHandler) == SIG_ERR)
  {
    PRINT_ERR("signal(2) failed while setting up for SIGINT");
    return -1;
  }

  // Get a display handle
  if (optDisplay)
  {
    display = XOpenDisplay(optDisplay);
    PRINT_MSG1(1, "using display [%s]", optDisplay);
  }
  else
  {
    PRINT_WARN("X display variable is not set; some funcions will be disabled");
    display = NULL;
  }

  // Initialize GLUT
  if (display)
    glutInit(argc, argv);
  
  return 0;
}


// Initialize the sim
int Init()
{
  struct tms cpu;

  // Register static models
  ModelFactory::RegisterAll();
  
  // Load the world file
  worldFile = new WorldFile();
  if (worldFile->Load(worldFileName) != 0)
    return -1;

  // Create the world
  world = new World(optServerId, optServerForce);

  // Load the world
  if (world->Load(worldFile) != 0)
    return -1;

  // Initialize
  if (world->Initialize() != 0)
    return -1;

  // Check for unused attributes
  worldFile->WarnUnused();

  // Our server id
  PRINT_MSG1(0, "server id [%d]", world->server_id);

  // Record the start times for accurate CPU loads
  times(&cpu);
  cpuStartTime = (double) (cpu.tms_utime + cpu.tms_stime) / sysconf(_SC_CLK_TCK);    
    
  return 0;
}


// Run the main loop for the sim
void MainLoop()
{
  while (MainIdle());
  return;
}


// Idle-time processing
bool MainIdle()
{
  double realTime, simTime, pauseTime, cpuTime, diffTime;
  struct timespec req, rem;
  struct tms cpu;

  // Advance the world 
  world->Step();

  realTime = world->GetRealTime();
  simTime = world->GetSimTime();
  pauseTime = world->GetPauseTime();

  // Slow down if simulation time is getting ahead of real time
  if (optTimeControl && (simTime + pauseTime) / world->simSpeed > realTime)
  {
    diffTime = (simTime + pauseTime) / world->simSpeed - realTime;
    req.tv_sec = (int) floor(diffTime);
    req.tv_nsec = (int) (fmod(diffTime, 1.0) * 1e9);
    nanosleep(&req, &rem);
    //printf("req %ld %ld\n", req.tv_sec, req.tv_nsec);
    //printf("rem %ld %ld\n", rem.tv_sec, rem.tv_nsec);
  }

  // Compute total CPU time
  times(&cpu);
  cpuTime = (double) (cpu.tms_utime + cpu.tms_stime) / sysconf(_SC_CLK_TCK);
  cpuTime -= ::cpuStartTime;

  if (optMsgLevel >= 0 && realTime - ::cpuRealTime > 0.500)
  {
    printf("Time %.3f %.3f %.3f [%.3f] %.3f [%3.0f%%]\r",
        realTime, simTime, pauseTime,
        (simTime + pauseTime) / (realTime + 1e-6),
        cpuTime, 100 * cpuTime / realTime);
    fflush(stdout);
    ::cpuRealTime = realTime;
  }

  // Exit if we have timed-out
  if (optTimeout > 0 && simTime > optTimeout)
    return false;
  
  // Exit if the user has decided to end the simulation
  if (userQuit)
    return false;

  return true;
}


// Finalize the sim
int Fini()
{
  world->Finalize();
  delete world;
  delete worldFile;

  // Flush the running totals
  ErrorFini();

  return 0;
}



// Program entry point
int main(int argc, char** argv)
{
  userQuit = false;

  // Parse the argument list
  if (ParseArgs(argc, argv) != 0)
    return -1;


  // Pre-initialization
  if (PreInit(&argc, argv) != 0)
    return -1;

  // Initalize
  if (Init() != 0)
  {
    fprintf(stderr,"Initialization failed\n");
    return -1;
  }

  // Run the sim
  MainLoop();
    
  // Finalize
  Fini();

  return 0;
}