world.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 902 行 · 第 1/2 页

/*
 *  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: The world; all models are collected here
 * Author: Andrew Howard
 * Date: 8 May 2003
 * CVS: $Id: World.cc,v 1.94 2006/02/14 03:00:24 natepak Exp $
 */


/// @addtogroup models 
/// @{ 

/** @defgroup models_global Global Attributes

The @c param:Global tag is used to specify certain global parameters
for the server.  E.g.:

@verbatim
<param:Global>
  <gravity>0.0 0.0 -9.8 </gravity>
  <stepTime>0.020</stepTime>
</param:Global>
@endverbatim

The parameters are as follows:

- speed (float)
  - Target simulation speed (e.g. speed 2 yields twice real time).
  - Default 1.0

- gravity (float vector)
  - The gravity vector (m/sec/sec); the default corresponds to Earth gravity.
  - Default 0 0 -9.8

- stepTime (float)
  - The minimum step time for the simulator.  Reducing the step time
    will increase the fidelity of the physical simulation, but consume
    more CPU time.  If you have particulary complex system that appears to
    be diverging (i.e., objects "explode" when they come into collision), consider
    reducing the step time.
  - Default 0.020

- pause (bool)
  - Set to true to start the simulator in paused mode.
  - Default: false

- dayTime (float)
  - Real time seconds since 12am. Negative number for local time
  - Default: -1.0

- utmZone (integer)
  - UTM (Universal Transverse Mercator) zone for calculating latitudes
    and longitudes from UTM easting and northings.
  - Default 0

- utmOffset (float vector)
  - UTM value (easting and northings) of world origin (used for
    calculating object UTM coordinates).  The z-value can also be used
    to specify an altitude offset.
  - Default 0

- skyColor (float vector) 
  - The color of the "sky", i.e., the default background color.
  - Default 0.7 0.7 0.9

- fogDensity (float)
  - Density of the ambient fog (see OpenGL docs for details).
  - Default 0.25

*/
/// @} 


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

#if HAVE_LIBDL
#include <dlfcn.h>
#endif

#include <assert.h>
#include <dirent.h>
#include <limits.h>
#include <time.h>
#include <sys/time.h>
#include <libgen.h>
#include <unistd.h>

#include "gazebo.h"

#include "Error.hh"
#include "WorldFile.hh"
#include "Body.hh"
#include "Model.hh"
#include "ModelFactory.hh"
#include "World.hh"
#include "ContactParams.hh"


//////////////////////////////////////////////////////////////////////////////
// Constructor
World::World(int serverId, bool serverForce)
{
  //this->worldFile = new WorldFile();    
  this->worldId = dWorldCreate();

  //this->spaceId = dHashSpaceCreate( 0 );
  this->spaceId = dSimpleSpaceCreate( 0 );

  this->contactGroup = dJointGroupCreate( 0 );

  this->modelCount = 0;
  this->modelMaxCount = 0;
  this->models = NULL;

  this->pause = false;
  this->simTime = 0.0;
  this->pauseTime = 0.0;
  this->startTime = 0.0;
  this->dayTime = -1.0;

  this->simSpeed = 1.0;
  this->stepTime = 0.020;
  this->gravity = GzVectorSet(0, 0, -9.8);
  this->utcOffset = 0;
  this->utmZone = GzVectorSet(0, 0, 0);
  this->utmOffset = GzVectorSet(0, 0, 0);
  this->skyColor = GzColor(0.7, 0.7, 0.9, 1.0);
  this->fogEnable = false;
  this->fogDensity = 0.25;

  this->server_id = serverId;
  this->server_force = serverForce;
  this->gz_server = NULL;
  this->gz_sim = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
World::~World()
{
  //dJointGroupDestroy( contactGroup );

  dSpaceDestroy( this->spaceId );
  dWorldDestroy( this->worldId );

  this->spaceId = 0;
  this->worldId = 0;

  // TODO: delete array elements

  if (this->models)
    free(this->models);

  //delete this->worldFile;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the world
int World::Initialize()
{
  int i;
  Model *model;

  assert(this->server_id >= 0);

  this->startTime = this->GetWallTime();
  
  // Create the server object (needs to be done before models initialize)
  this->gz_server = gz_server_alloc();
  if (gz_server_init( this->gz_server, this->server_id, this->server_force ) != 0)
    return -1;

  // Initialize models
  for (i = 0; i < this->modelCount; i++)
  {
    model = this->models[i];
    if (model->Init( worldFile, model->node ) != 0)
      return -1;
  }

  // Create the simulator interface
  this->gz_sim = gz_sim_alloc();
  if (gz_sim_create( this->gz_sim, this->gz_server, "default" ) != 0)
    return -1;

  // Set initial simulator state
  gz_sim_lock(this->gz_sim, 1);
  this->gz_sim->data->pause = this->pause;
  gz_sim_unlock(this->gz_sim);

 
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the world
int World::Finalize()
{
  int i;
  Model *model;

  // Finalize models
  for (i = 0; i < this->modelCount; i++)
  {
    model = this->models[i];
    if (model->Fini() != 0)
      return -1;
  }

  // Done with the external interface
  gz_sim_destroy( this->gz_sim );
  gz_sim_free( this->gz_sim );
  gz_server_fini( gz_server );
  gz_server_free( gz_server );

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load the world
int World::Load( WorldFile *worldFile)
{
  this->worldFile = worldFile;
  
  // Recursively load models
  if (this->LoadModel( this->worldFile->GetRootNode(), NULL ) != 0)
    return -1;

  // Set the gravity
  dWorldSetGravity( this->worldId,
                    this->gravity.x,
                    this->gravity.y, 
                    this->gravity.z );

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load models (recursive)
int World::LoadModel( WorldFileNode *node, Model *parent )
{
  WorldFileNode *cnode;
  Model *model;
  GzPose pose;
  const char *bodyName;
  const char *pluginName;
  Body *body;

  model = NULL;

  if (node->GetNSPrefix())
  {
    // Check for model nodes
    if (strcmp( node->GetNSPrefix(), "model" ) == 0)
    {
      PRINT_MSG1(2, "Model:%s", node->GetName());
            
      // Load plugin *before* instantiating the model
      pluginName = node->GetString("plugin", NULL, 0);
      if (pluginName)
      {
        if (this->LoadPlugin(pluginName, this->worldFile->filename) != 0)
          return -1;
      }

      // Instantiate the model
      model = ModelFactory::NewModel( this, node->GetName() );
      model->SetName(node->GetName());
      if (model == NULL)
      {
        PRINT_WARN1( "unknown model class or class disabled [%s]", node->GetName() );
        return 0;
      }

      // Add the model to our list
      this->AddModel( model );

      // Recall the node this model is attached to, so we can save
      // back the data later.
      model->node = node;

      // Set the id of the model
      model->SetId( node->GetString( "id", NULL, 0 ) );
      if (model->GetId() == NULL)
      {
        model->SetId( node->GetName() );
      }

      if (parent != NULL)
      {
        // Get the body to attach; if we specify no name, we will
        // always end up with the canonical body.
        bodyName = node->GetString("parentBody", "canonical");
        body = parent->GetBody(bodyName);
        if (body == NULL)
        {
          PRINT_ERR1("body [%s] not defined for parent model", bodyName);
          return -1;
        }

        // Set the parent
        model->SetParent(parent, body);
      }

      // Load the model
      if (model->MasterLoad( this->worldFile, node ) != 0)
        return -1;
      
      // Get the position and orientation of the model (relative to parent)
      pose = GzPoseSet(GzVectorZero(), GzQuaternIdent());
      pose.pos = node->GetPosition( "xyz", pose.pos );
      pose.rot = node->GetRotation( "rpy", pose.rot );
      
      // Set the model's pose (relative to parent)
      this->SetModelPose( model, pose );

      // Record the model's initial pose (for reseting)
      model->initPose = pose;

      // Attach model to its parent
      if (parent != NULL)
        model->Attach();
    }

    // TODO: Need to clean this up. Very ugly with this code here
    else if (strcmp( node->GetNSPrefix(), "param" ) == 0)
    {
      if (strcmp( node->GetName(), "Global" ) == 0)
      {
        this->simSpeed = node->GetDouble("speed", this->simSpeed);        
        this->stepTime = node->GetDouble("stepTime", this->stepTime);
        this->gravity = node->GetPosition( "gravity", this->gravity);

        // Get UTC time offset
        this->utcOffset = node->GetDouble("utcOffset", this->utcOffset);
        
        // Get zone for doing utm to lat/lon conversions (GPS)
        this->utmZone.x = node->GetInt("utmZone", (int) this->utmZone.x);

        // Get UTM position of the world origin
        this->utmOffset = node->GetPosition("utmOffset", this->utmOffset);
        
        this->skyColor = node->GetColor( "skyColor", this->skyColor );
        this->fogEnable = node->IsDefined("fogDensity");
        this->fogDensity = node->GetDouble("fogDensity", this->fogDensity);
        this->dayTime = node->GetDouble("dayTime",this->dayTime);

        // Initial pause state
        this->pause = node->GetBool("pause", this->pause);
      }
    }
  }

  // Load children
  for (cnode = node->GetChild(); cnode != NULL; cnode = cnode->GetNext())
  {
    if (this->LoadModel( cnode, model ) != 0)
      return -1;
  }

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Try to load a given plugin, using a particular search algorithm.
// Returns true on success and false on failure.
// This is copied from Player
int World::LoadPlugin(const char* pluginname, const char *worldfilepath)
{
#if HAVE_LIBDL
  void *handle = NULL;
  char fullpath[PATH_MAX];
  gz_plugin_init_fn_t initfunc;
  char *gazebopath;
  char *tmp;
  char *cfgdir;
  char *error;
  unsigned int i,j;

  // see if we got an absolute path
  if (pluginname[0] == '/' || pluginname[0] == '~')
  {
    strcpy(fullpath,pluginname);
    PRINT_MSG1(1, "loading %s...", fullpath);
    if(!(handle = dlopen(fullpath, RTLD_NOW)))
    {
      PRINT_ERR1("error loading plugin: %s", dlerror());
      return -1;
    }
  }

  // we got a relative path, so search for the module

  // did the user set GAZEBOPATH?
  if(!handle && (gazebopath = getenv("GAZEBOPATH")))
  {
    // yep, now parse it, as a colon-separated list of directories
    i = 0;
    while(i<strlen(gazebopath))
    {
      j=i;
      while(j<strlen(gazebopath))
      {
        if(gazebopath[j] == ':')
        {
          break;
        }
        j++;
      }
      
      memset(fullpath,0,PATH_MAX);
      strncpy(fullpath,gazebopath+i,j-i);
      strcat(fullpath,"/");
      strcat(fullpath,pluginname);

      PRINT_MSG1(1, "loading %s...", fullpath);      
      if((handle = dlopen(fullpath, RTLD_NOW)))
      {
        break;
      }
      else