worldfile.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: A class for reading and writing the world file.
 * Author: Andrew Howard, Nate Koenig
 * Date: 15 Nov 2001
 * CVS info: $Id: WorldFile.cc,v 1.29 2004/11/14 07:39:04 inspectorg Exp $
 */

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

#include <assert.h>
#include <ctype.h>
#include <limits.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>

#include <sys/types.h>
#include <sys/stat.h>

#include "replace.h"
#include "Error.hh"
#include "WorldFile.hh"


////////////////////////////////////////////////////////////////////////////
// Standard constructor
WorldFile::WorldFile()
{
  this->filename = NULL;
  this->root = NULL;
  return;
}


////////////////////////////////////////////////////////////////////////////
// Standard destructor
WorldFile::~WorldFile()
{
  if (this->root)
    delete this->root;
  
  if (this->filename)
    free(this->filename);

  return;
}


////////////////////////////////////////////////////////////////////////////
// Load world from file
int WorldFile::Load( const char *filename )
{
  assert( this->filename == NULL );
  this->filename = strdup( filename );

  // Enable line numbering
  xmlLineNumbersDefault( 1 );

  // Load the file
  this->xmlDoc = xmlParseFile( this->filename );
  if (xmlDoc == NULL)
  {
    PRINT_ERR1( "unable to parse [%s]", this->filename );
    return -1;
  }

  // Create wrappers for all the nodes (recursive)
  this->root = this->CreateNodes( NULL, xmlDocGetRootElement(this->xmlDoc) );
  if (this->root == NULL)
  {
    PRINT_ERR1( "empty document [%s]", this->filename );
    return -1;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Save world back into file
// Set filename to NULL to save back into the original file
int WorldFile::Save( const char *filename )
{
  if (filename == NULL)
    filename = this->filename;
    
  if (xmlSaveFile(filename, this->xmlDoc) < 0)
  {
    PRINT_ERR1("unable to save [%s]", filename);
    return -1;
  }
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Print a warning message with unused attributes (if any).
bool WorldFile::WarnUnused()
{
  return this->root->WarnUnused();
}


////////////////////////////////////////////////////////////////////////////
// Load world from a string
int WorldFile::LoadString( const char *str )
{
  // Enable line numbering
  xmlLineNumbersDefault( 1 );

  // Load the file
  this->xmlDoc = xmlParseDoc( (xmlChar*)(str) );
  if (xmlDoc == NULL)
  {
    PRINT_ERR1( "unable to parse [%s]", str );
    return -1;
  }

  // Create wrappers for all the nodes (recursive)
  this->root = this->CreateNodes( NULL, xmlDocGetRootElement(this->xmlDoc) );
  if (this->root == NULL)
  {
    PRINT_ERR1( "empty document [%s]", str );
    return -1;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////
// Get the root node
WorldFileNode *WorldFile::GetRootNode() const
{
  return this->root;
}


////////////////////////////////////////////////////////////////////////////
// Create wrappers
WorldFileNode *WorldFile::CreateNodes( WorldFileNode *parent, 
                                       xmlNodePtr xmlNode )
{
  WorldFileNode *self = NULL;

  // No need to create wrappers around text and blank nodes
  if( !xmlNodeIsText( xmlNode ) && !xmlIsBlankNode( xmlNode )
      && xmlNode->type != XML_COMMENT_NODE )
  {

    // Create a new node
    self = new WorldFileNode(this, parent, xmlNode,xmlDoc);

    // Create our children
    for ( xmlNode = xmlNode->xmlChildrenNode; xmlNode != NULL; 
         xmlNode = xmlNode->next)
      this->CreateNodes( self, xmlNode );
  }

  return self;
}



////////////////////////////////////////////////////////////////////////////
// Constructor
WorldFileNode::WorldFileNode( WorldFile *file, WorldFileNode *parent, 
                              xmlNodePtr xmlNode, xmlDocPtr xmlDoc )
{
  this->file = file;

  this->parent = parent;
  this->prev = NULL;
  this->next = NULL;
  this->child_first = NULL;
  this->child_last = NULL;

  // Link ourself to our parent
  if (this->parent)
  {
    if (!this->parent->child_first)
      this->parent->child_first = this;

    this->prev = this->parent->child_last;
    this->parent->child_last = this;
  }

  // Link our self to our siblings
  if (this->prev)
    this->prev->next = this;

  this->xmlNode = xmlNode;
  this->xmlDoc = xmlDoc;

  this->used = false;

  return;
}


////////////////////////////////////////////////////////////////////////////
// Destructor
WorldFileNode::~WorldFileNode()
{
  // Delete all our children first
  if (this->child_first)
    delete this->child_first;
    
  //assert(this->child_first == NULL);
  //assert(this->child_last == NULL);

  // Unlink ourself from our siblings
  if (this->prev)
    this->prev->next = this->next;

  if (this->next)
    this->next->prev = this->prev;

  // Unlink ourself from our parent
  if (this->parent)
  {
    if (this->parent->child_first == this)
      this->parent->child_first = this->next;

    if (this->parent->child_last == this)
      this->parent->child_last = this->prev;
  }

  /*  if(this->contentStr)
      xmlFree(this->contentStr);
      this->contentStr=NULL;
      */

  return;
}


////////////////////////////////////////////////////////////////////////////
// Get the node name
const char *WorldFileNode::GetName()
{
  return (const char*) this->xmlNode->name;
}

////////////////////////////////////////////////////////////////////////////
// Get the Name Space Prefix
const char *WorldFileNode::GetNSPrefix()
{
  if( !this->xmlNode->ns )
    return NULL;

  this->used = true;
  return (const char*) this->xmlNode->ns->prefix;
}

////////////////////////////////////////////////////////////////////////////
// Get the next sibling of this node
WorldFileNode *WorldFileNode::GetNext()
{
  return this->next;
}


////////////////////////////////////////////////////////////////////////////
// Get the first child of this node
WorldFileNode *WorldFileNode::GetChild()
{
  return this->child_first;
}


////////////////////////////////////////////////////////////////////////////
// Get a child based on a name. Returns null if not found
WorldFileNode *WorldFileNode::GetChild( const char *name )
{
  WorldFileNode *tmp;
  for (tmp = this->child_first; tmp != NULL && 
      strcmp( (const char*)tmp->xmlNode->name,name)!=0; tmp = tmp->GetNext() );

  return tmp;
}

WorldFileNode *WorldFileNode::GetChildByNSPrefix( const char *prefix )
{
  WorldFileNode *tmp;

  for (tmp = this->child_first; tmp != NULL; tmp = tmp->GetNext() )
  {
    if (tmp->xmlNode->ns && strcmp( (const char*)tmp->xmlNode->ns->prefix,prefix)==0)
      break;
  }

  return tmp;
}

////////////////////////////////////////////////////////////////////////////
// Print (for debugging purposes)
void WorldFileNode::Print()
{
  WorldFileNode *node;
  //xmlChar *value;

  printf( "name = [%s]\n", (const char*) this->xmlNode->name );

  //value = xmlNodeListGetString(this->doc, this->node, 1);
  //if (value)
  //  printf("value = [%s]\n", (const char*) value);
  /*if( this->xmlNode->content != NULL)
    printf("content = [%s]\n", (const char*) this->xmlNode->content);
  */

  printf( "id = [%s]\n", xmlGetProp(this->xmlNode, (xmlChar*) "id") );

  // Recurse
  for (node = this->child_first; node != NULL; node = node->next)
    node->Print();

  return;
}


////////////////////////////////////////////////////////////////////////////
// Print a warning message with unused attributes (if any).
bool WorldFileNode::WarnUnused()
{
  bool unused;
  WorldFileNode *node;  

  unused = false;
  
  if (!this->used)
  {
    PRINT_WARN3("in %s:%d unused attribute [%s]",
                this->file->filename, (int) xmlGetLineNo(this->xmlNode), this->GetName());
    unused = true;
  }

  // Call child nodes recursively
  for (node = this->child_first; node != NULL; node = node->next)
    unused |= node->WarnUnused();
  
  return unused;
}


////////////////////////////////////////////////////////////////////////////
// Get a value associated with a node.
char *WorldFileNode::GetNodeValue( const char *key )
{
  xmlChar *value=NULL;

  // First check if the key is an attribute
  if (xmlHasProp( this->xmlNode, (xmlChar*) key ))
  {
    assert(false);
    value = xmlGetProp( this->xmlNode, (xmlChar*) key );
    this->used = true;
  }

  else if (strcmp( this->GetName(), key)==0)
  {
    value = xmlNodeGetContent(this->xmlNode);
    this->used = true;
  }

  // If not an attribute, then it should be a child node
  else
  {
    WorldFileNode *currNode;

    // Loop through children
    for (currNode = this->child_first; currNode; currNode = currNode->next)
    {
      // If the name matches, then return its value
      if (strcmp( currNode->GetName(), key ) == 0)
      {
        value = xmlNodeGetContent( currNode->xmlNode );
        currNode->used = true;
        break;
      }
    }
  }
  
  return (char*) value;
}


////////////////////////////////////////////////////////////////////////////
// Set a value associated with a node.
void WorldFileNode::SetNodeValue( const char *key, const char *value )
{
  // First check if the key is an attribute
  if (xmlHasProp( this->xmlNode, (xmlChar*) key ))
  {
    //printf("set attr : %s\n", key);
    xmlSetProp( this->xmlNode, (xmlChar*) key, (xmlChar*) value );
  }

  // If not an attribute, then it should be a child node
  else
  {
    WorldFileNode *currNode;

    // Loop through children
    for (currNode = this->child_first; currNode; currNode = currNode->next)
    {
      // If the name matches, then return its value
      if (strcmp( currNode->GetName(), key ) == 0)
      {
        //printf("set child: %s\n", key);
        xmlNodeSetContent( currNode->xmlNode, (xmlChar*) value);
        break;
      }
    }
  }
  return;
}


////////////////////////////////////////////////////////////////////////////
// Get a string value.
const char *WorldFileNode::GetString( const char *key, const char *def, int require)
{
  char *value = this->GetNodeValue( key );

  if (!value && require)
  {
    PRINT_ERR3( "in %s:%d missing attribute [%s]",
        this->file->filename, (int) xmlGetLineNo(this->xmlNode), key );
    return NULL;
  }