/* *  Stage : a multi-robot simulator. *  Copyright (C) 2001, 2002 Richard Vaughan, Andrew Howard and Brian Gerkey. * *  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 in the world file. * Authors: Andrew Howard <ahoward@usc.edu> *          Richard Vaughan <vaughan@hrl.com> *          Douglas S. Blank <dblank@brynmawr.edu> * * Date: 15 Nov 2001 * CVS info: $Id: worldfile.cc,v 1.25.4.1 2003/04/17 23:40:10 rtv Exp $ */#include <assert.h>#include <ctype.h>#include <errno.h>#include <limits.h> // for PATH_MAX#include <stdio.h>#include <stdlib.h>#include <string.h>#include <unistd.h>//#define DEBUG#include "replace.h" // for dirname(3)#include "stage_types.hh"#include "colors.hh"#include "worldfile.hh"// the isblank() macro is not standard - it's a GNU extension// and it doesn't work for me, so here's an implementation - rtv#ifndef isblank#define isblank(a) (a == ' ' || a == '\t')#endif///////////////////////////////////////////////////////////////////////////// Useful macros for dumping parser errors#define TOKEN_ERR(z, l) \  PRINT_ERR2("%s:%d : " z, this->filename, l)#define PARSE_ERR(z, l) \  PRINT_ERR2("%s:%d : " z, this->filename, l)///////////////////////////////////////////////////////////////////////////// Default constructorCWorldFile::CWorldFile() {  this->filename = NULL;  this->token_size = 0;  this->token_count = 0;  this->tokens = NULL;  this->macro_count = 0;  this->macro_size = 0;  this->macros = NULL;  this->entity_count = 0;  this->entity_size = 0;  this->entities = NULL;  this->property_count = 0;  this->property_size = 0;  this->properties = NULL;  // Set defaults units  this->unit_length = 1.0;  this->unit_angle = M_PI / 180;}///////////////////////////////////////////////////////////////////////////// DestructorCWorldFile::~CWorldFile(){  ClearProperties();  ClearMacros();  ClearEntities();  ClearTokens();  if (this->filename)    free(this->filename);}FILE *CWorldFile::FileOpen(const char *filename, const char* method){   FILE *fp = fopen(filename, method);   // if this opens, then we will go with it:   if (fp) {     PRINT_DEBUG1( "Loading: %s", filename);     return fp;   }   // else, search other places, and set this->filename   // accordingly if found:   char *stagepath = getenv("STAGEPATH");   char *token = strtok(stagepath, ":");   char *fullpath = (char*) malloc(PATH_MAX);   char *tmp = strdup(filename);   char *base = basename(tmp);   while (token != NULL) {     // for each part of the path, try it:     memset( fullpath, 0, PATH_MAX);     strcat( fullpath, token);     strcat( fullpath, "/" );     strcat( fullpath, base);     assert(strlen(fullpath) + 1 < PATH_MAX);     fp = fopen(fullpath, method);     if (fp) {       this->filename = fullpath;       PRINT_DEBUG1( "Loading: %s", filename);       free(tmp);       return fp;     }     token = strtok(NULL, ":");   }   free(tmp);   return NULL;}///////////////////////////////////////////////////////////////////////////// Load world from filebool CWorldFile::Load(const char *filename){  // Shouldnt call load more than once,  // so this should be null.  assert(this->filename == NULL);  this->filename = strdup(filename);  // Open the file  //FILE *file = fopen(this->filename, "r");  FILE *file = FileOpen(this->filename, "r");  if (!file)  {    PRINT_ERR2("unable to open world file %s : %s",               this->filename, strerror(errno));    return false;  }  ClearTokens();    // Read tokens from the file  if (!LoadTokens(file, 0))  {    //DumpTokens();    return false;  }  // Parse the tokens to identify entities  if (!ParseTokens())  {    //DumpTokens();    return false;  }  // Dump contents and exit if this file is meant for debugging only.  if (ReadInt(0, "test", 0) != 0)  {    PRINT_ERR("this is a test file; quitting");    DumpTokens();    DumpMacros();    DumpEntities();    DumpProperties();    return false;  }    // Work out what the length units are  const char *unit = ReadString(0, "unit_length", "m");  if (strcmp(unit, "m") == 0)    this->unit_length = 1.0;  else if (strcmp(unit, "cm") == 0)    this->unit_length = 0.01;  else if (strcmp(unit, "mm") == 0)    this->unit_length = 0.001;  // Work out what the angle units are  unit = ReadString(0, "unit_angle", "degrees");  if (strcmp(unit, "degrees") == 0)    this->unit_angle = M_PI / 180;  else if (strcmp(unit, "radians") == 0)    this->unit_angle = 1;    return true;}///////////////////////////////////////////////////////////////////////////// Save world to filebool CWorldFile::Save(const char *filename){  // Debugging  //DumpProperties();    // If no filename is supplied, use default  if (!filename)    filename = this->filename;  // Open file  //FILE *file = fopen(filename, "w+");  FILE *file = FileOpen(filename, "w+");  if (!file)  {    PRINT_ERR2("unable to open world file %s : %s",               filename, strerror(errno));    return false;  }  // Write the current set of tokens to the file  if (!SaveTokens(file))  {    fclose(file);    return false;  }  fclose(file);  return true;}///////////////////////////////////////////////////////////////////////////// Check for unused properties and print warningsbool CWorldFile::WarnUnused(){  bool unused = false;  for (int i = 0; i < this->property_count; i++)  {    CProperty *property = this->properties + i;    if (!property->used)    {      unused = true;      PRINT_WARN3("worldfile %s:%d : property [%s] is defined but not used",                  this->filename, property->line, property->name);    }  }  return unused;}///////////////////////////////////////////////////////////////////////////// Load tokens from a file.bool CWorldFile::LoadTokens(FILE *file, int include){  int ch;  int line;  char token[256];    line = 1;  while (true)  {    ch = fgetc(file);    if (ch == EOF)      break;        if ((char) ch == '#')    {      ungetc(ch, file);      if (!LoadTokenComment(file, &line, include))        return false;    }    else if (isalpha(ch))    {      ungetc(ch, file);      if (!LoadTokenWord(file, &line, include))        return false;    }    else if (strchr("+-.0123456789", ch))    {      ungetc(ch, file);      if (!LoadTokenNum(file, &line, include))        return false;    }    else if (isblank(ch))    {      ungetc(ch, file);      if (!LoadTokenSpace(file, &line, include))        return false;    }    else if (ch == '"')    {      ungetc(ch, file);      if (!LoadTokenString(file, &line, include))        return false;    }    else if (strchr("(", ch))    {      token[0] = ch;      token[1] = 0;      AddToken(TokenOpenEntity, token, include);    }    else if (strchr(")", ch))    {      token[0] = ch;      token[1] = 0;      AddToken(TokenCloseEntity, token, include);    }    else if (strchr("[", ch))    {      token[0] = ch;      token[1] = 0;      AddToken(TokenOpenTuple, token, include);    }    else if (strchr("]", ch))    {      token[0] = ch;      token[1] = 0;      AddToken(TokenCloseTuple, token, include);    }    else if (ch == '\n')    {      line++;      AddToken(TokenEOL, "\n", include);    }    else    {      TOKEN_ERR("syntax error", line);      return false;    }  }  return true;}///////////////////////////////////////////////////////////////////////////// Read in a comment tokenbool CWorldFile::LoadTokenComment(FILE *file, int *line, int include){  char token[256];  int len;  int ch;  len = 0;  memset(token, 0, sizeof(token));    while (true)  {    ch = fgetc(file);    if (ch == EOF)    {      AddToken(TokenComment, token, include);      return true;    }    else if (ch == '\n')    {      ungetc(ch, file);      AddToken(TokenComment, token, include);      return true;    }    else      token[len++] = ch;  }  return true;}///////////////////////////////////////////////////////////////////////////// Read in a word tokenbool CWorldFile::LoadTokenWord(FILE *file, int *line, int include){  char token[256];  int len;  int ch;    len = 0;  memset(token, 0, sizeof(token));    while (true)  {    ch = fgetc(file);    if (ch == EOF)    {      AddToken(TokenWord, token, include);      return true;    }    else if (isalpha(ch) || isdigit(ch) || strchr(".-_[]", ch))    {      token[len++] = ch;    }    else    {      if (strcmp(token, "include") == 0)      {        ungetc(ch, file);        AddToken(TokenWord, token, include);        if (!LoadTokenInclude(file, line, include))          return false;      }      else      {        ungetc(ch, file);        AddToken(TokenWord, token, include);      }      return true;    }  }  assert(false);  return false;}///////////////////////////////////////////////////////////////////////////// Load an include token; this will load the include file.bool CWorldFile::LoadTokenInclude(FILE *file, int *line, int include){  int ch;  const char *filename;  char *fullpath;  ch = fgetc(file);  if (ch == EOF)  {    TOKEN_ERR("incomplete include statement", *line);    return false;  }  else if (!isblank(ch))  {    TOKEN_ERR("syntax error in include statement", *line);    return false;  }  ungetc(ch, file);  if (!LoadTokenSpace(file, line, include))    return false;  ch = fgetc(file);    if (ch == EOF)  {    TOKEN_ERR("incomplete include statement", *line);    return false;  }  else if (ch != '"')  {    TOKEN_ERR("syntax error in include statement", *line);    return false;  }  ungetc(ch, file);  if (!LoadTokenString(file, line, include))    return false;  // This is the basic filename  filename = GetTokenValue(this->token_count - 1);  // Now do some manipulation.  If its a relative path,  // we append the path of the world file.  if (filename[0] == '/' || filename[0] == '~')  {    fullpath = strdup(filename);  }  else if (this->filename[0] == '/' || this->filename[0] == '~')  {    // Note that dirname() modifies the contents, so    // we need to make a copy of the filename.    // There's no bounds-checking, but what the heck.    char *tmp = strdup(this->filename);    fullpath = (char*) malloc(PATH_MAX);    memset(fullpath, 0, PATH_MAX);    strcat( fullpath, dirname(tmp));    strcat( fullpath, "/" );     strcat( fullpath, filename );    assert(strlen(fullpath) + 1 < PATH_MAX);    free(tmp);  }  else  {    // Note that dirname() modifies the contents, so    // we need to make a copy of the filename.    // There's no bounds-checking, but what the heck.    char *tmp = strdup(this->filename);    fullpath = (char*) malloc(PATH_MAX);    getcwd(fullpath, PATH_MAX);    strcat( fullpath, "/" );     strcat( fullpath, dirname(tmp));    strcat( fullpath, "/" );     strcat( fullpath, filename );    assert(strlen(fullpath) + 1 < PATH_MAX);    free(tmp);  }  printf( "[Include %s]", filename );  fflush( stdout );  // Open the include file  FILE *infile = FileOpen(fullpath, "r");  if (!infile)  {    PRINT_ERR2("unable to open include file %s : %s",               fullpath, strerror(errno));    free(fullpath);    return false;  }  // Terminate the include line  AddToken(TokenEOL, "\n", include);        // Read tokens from the file  if (!LoadTokens(infile, include + 1))  {    //DumpTokens();    free(fullpath);    return false;  }  free(fullpath);  return true;}///////////////////////////////////////////////////////////////////////////// Read in a number tokenbool CWorldFile::LoadTokenNum(FILE *file, int *line, int include){  char token[256];  int len;  int ch;    len = 0;  memset(token, 0, sizeof(token));    while (true)  {    ch = fgetc(file);    if (ch == EOF)    {      AddToken(TokenNum, token, include);      return true;    }    else if (strchr("+-.0123456789", ch))    {      token[len++] = ch;    }    else    {      AddToken(TokenNum, token, include);      ungetc(ch, file);      return true;    }  }  assert(false);  return false;}///////////////////////////////////////////////////////////////////////////// Read in a string tokenbool CWorldFile::LoadTokenString(FILE *file, int *line, int include){