factory.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: Model for a Factory
 * Author: Nate Koenig 
 * Date: 14 Nov 2003
 * CVS: $Id: Factory.cc,v 1.17 2006/02/22 14:58:57 natepak Exp $
 */

/// @addtogroup models
/// @{ 
/** @defgroup Factory Factory

The Factory model maintains no physical characteristics, but instead
allows the user to dynamically create models at runtime. A model is
instantiated by passing an XML character string to a Factory model.
The character string uses the same model format as the .world files. Note
that the string should only contain the model description, and no
other tags. The XML string is passed into Gazebo through the %gz_factory
interface which has the same interface as the Player speech device.  

@par libgazebo interfaces

This model supports the @ref factory interface.

@par Player drivers

A new model is created using the %gz_factory driver.

@par Attributes

The following attributes are supported.

- id (string)
  - The model ID string (used by libgazebo to identify models).
  - Default: empty

@par Bodies

This model does not have a physical representation.

@par Example

@verbatim
<model:Factory>
  <id>factory0</id>
</model:Factory>
@endverbatim

@par Authors

Chris Jones

*/
/// @}


#include <assert.h>
#include "gazebo.h"
#include "Body.hh"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "Factory.hh"


//////////////////////////////////////////////////////////////////////////////
// Register this class
GZ_REGISTER_STATIC("Factory", Factory);


//////////////////////////////////////////////////////////////////////////////
// Constructor
Factory::Factory( World *world )
    : Model( world )
{
  // Prefix and suffix for xml files
  this->xmlPrefix = "<?xml version='1.0'?> <gz:world xmlns:gz='http://playerstage.sourceforge.net/gazebo/xmlschema/#gz' xmlns:model='http://playerstage.sourceforge.net/gazebo/xmlschema/#model'> ";

  this->xmlSuffix = "</gz:world>";

  this->body = NULL;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
Factory::~Factory()
{

  delete [] this->xmlPrefix;
  delete [] this->xmlSuffix;

  if (this->body)
    delete this->body;

  this->xmlPrefix = NULL;
  this->xmlSuffix = NULL;
  this->body = NULL;
  return;
}

//////////////////////////////////////////////////////////////////////////////
// Load the model
int Factory::Load( WorldFile *file, WorldFileNode *node )
{
  // Create a dummy body to which to attach geoms
  this->body = new Body(this->world, NULL, true);
  this->AddBody(this->body, true);

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Factory::Init( WorldFile *file, WorldFileNode *node )
{
  // Initialize external interface
  if (this->IfaceInit() != 0)
    return -1;

  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Factory::Fini()
{
  // Finalize external interface  
  this->IfaceFini();

  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Update model
void Factory::Update( double /*step*/ )
{
  // Get commands from the external interface
  this->IfaceGetCmd();
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the external interface
int Factory::IfaceInit()
{
  this->iface = gz_factory_alloc();
  assert(this->iface);

  if (gz_factory_create(this->iface, this->world->gz_server, this->GetId(),
                        "Factory", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;
  
  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Finalize the external interface
int Factory::IfaceFini()
{
  gz_factory_destroy( this->iface );
  gz_factory_free( this->iface );
  this->iface = NULL;
  
  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Get commands from the external interface
void Factory::IfaceGetCmd()
{

  // If there is a string, then add the contents to the world
  if (strcmp((const char*)this->iface->data->string,"")!=0)
  {
    // Create the complete xml string
    char *xmlString = new char[strlen(this->xmlPrefix) +
                               strlen(this->xmlSuffix) + 
                               strlen((const char*)this->iface->data->string)];

    // Create the world file
    WorldFile *worldFile = new WorldFile();

    // Copy in all the data to the xml string
    strcpy(xmlString,this->xmlPrefix);
    strcat(xmlString,(const char*)this->iface->data->string);
    strcat(xmlString,this->xmlSuffix);

    // Load the XML tree from the given string
    worldFile->LoadString( xmlString );

    // Add the new models into the World
    this->world->LoadModel( worldFile->GetRootNode(), NULL );

    // Cleanup
    delete worldFile;
    worldFile = NULL;

    delete [] xmlString;
    xmlString = NULL;

    //strcpy((char*)this->iface->data,"");
    strcpy((char*)this->iface->data->string,"");
  }

  return;
}