ArMap.cpp

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     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

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ariaInternal.h"
#include "ArMap.h"
#include "ArFileParser.h"
#include "ArArgumentBuilder.h"
#include "ArLog.h"
#include <ctype.h>


const char *ArMap::ourInfoNames[INFO_COUNT] = 
{
  "MetaInfo:",
    "TaskInfo:",
    "RouteInfo:",
  "SchedTaskInfo:",
  "SchedInfo:"
};

AREXPORT ArMap::ArMap(const char *baseDirectory, bool addToGlobalConfig,
              const char *configSection, const char *configParam,
              const char *configDesc, bool ignoreEmptyFileName) :
  myNumInfos(INFO_COUNT), // Consider adding this to the ctor so subclasses could increase...
  my2DMapCB(this, &ArMap::handle2DMap),
  myMinPosCB(this, &ArMap::handleMinPos),
  myMaxPosCB(this, &ArMap::handleMaxPos),
  myNumPointsCB(this, &ArMap::handleNumPoints),
  myLineMinPosCB(this, &ArMap::handleLineMinPos),
  myLineMaxPosCB(this, &ArMap::handleLineMaxPos),
  myNumLinesCB(this, &ArMap::handleNumLines),
  myResolutionCB(this, &ArMap::handleResolution),
  myMapObjectCB(this, &ArMap::handleMapObject),
  myMapInfoCB(this, &ArMap::handleMapInfo),
  myInfoCBArray(),
  myDataCB(this, &ArMap::handleData),
  myLinesCB(this, &ArMap::handleLines),
  myPointCB(this, &ArMap::handlePoint),
  myLineCB(this, &ArMap::handleLine),
  myProcessFileCB(this, &ArMap::processFile)
{
  // Allocate the arrays in which to store data for the Info lines
  myLoadingInfoArray = new std::list<ArArgumentBuilder *>[myNumInfos];
  myInfoArray = new std::list<ArArgumentBuilder *> [myNumInfos];
  myInfoChangedArray = new ArTime[myNumInfos];
  myMapChangedInfoArray = new ArTime[myNumInfos];
  myInfoCBArray = new ArRetFunctor2C<bool, ArMap, ArArgumentBuilder *, int> *[myNumInfos];
  myResolution = -1;

  // Create the callbacks for the Info lines
  for (int i = 0; i < myNumInfos; i++) {
    myInfoCBArray[i] = new ArRetFunctor2C<bool, ArMap, ArArgumentBuilder *, int>
                               (this,
                                &ArMap::handleInfo,
                                NULL, i);
  }

  myProcessFileCB.setName("ArMap");
  myMapChangedLogLevel = ArLog::Verbose;
  myConfigParam = configParam;
  myIgnoreEmptyFileName = ignoreEmptyFileName;

#ifndef ARINTERFACE
  if (addToGlobalConfig)
  {
    myConfigMapName[0] = '\0';
    myConfigProcessedBefore = false;
    
    const char *displayHint = "RobotFile:Map Files (*.map)|*.map";

    Aria::getConfig()->addParam(ArConfigArg(configParam, 
                                            myConfigMapName, 
                                                      configDesc,
                                                      sizeof(myConfigMapName)), 
                                        configSection, 
                                        ArPriority::IMPORTANT,
                                displayHint);
    Aria::getConfig()->addProcessFileWithErrorCB(&myProcessFileCB, 100);
  }
#endif //ARINTERFACE
  myLoadingParser = NULL;
  setBaseDirectory(baseDirectory);
  reset();

}

AREXPORT ArMap::~ArMap()
{
  /*
  delete [] myLoadingInfoArray;
  delete [] myInfoArray;
  delete [] myInfoChangedArray;
  delete [] myMapChangedInfoArray;
  for (int i = 0; i < myNumInfos; i++) {
    delete myInfoCBArray[i];
  }
  delete [] myInfoCBArray;
  */


  ArUtil::deleteSet(myMapObjects.begin(), myMapObjects.end());
  ArUtil::deleteSet(myMapInfo.begin(), myMapInfo.end());
  if(myLoadingParser) delete myLoadingParser;
}

bool ArMap::processFile(char *errorBuffer, size_t errorBufferLen)
{

  ArUtil::fixSlashes(myConfigMapName, MAX_MAP_NAME_LENGTH);

  struct stat mapFileStat;
  stat(myConfigMapName, &mapFileStat);
  
  // If file name is empty, clear out all current information
  if (myIgnoreEmptyFileName && myConfigMapName[0] == '\0')
  {
    ArLog::log(ArLog::Normal, "Using an empty map since empty map file name");
    lock();
    reset();
    myFileName = "";
    setMapObjects(NULL);
    setPoints(NULL);
    setLines(NULL);
    setMapInfo(NULL);
    // Clear all of the Info information
    for (int i = 0; i < myNumInfos; i++) {
      setInfo(i, NULL);
    }

    myResolution = -1;
    mapChanged();
    unlock();
    return true;
  }

  // Reload map if this is the first time, or we need to use a new file, or the
  // existing file has changed.
  if (!myConfigProcessedBefore || 
      ArUtil::strcmp(myConfigMapName, myFileName.c_str()) != 0 ||
      mapFileStat.st_mtime != myReadFileStat.st_mtime)
  {
    ArLog::log(ArLog::Verbose, "ArMap: LOADINGMAP because configProcessedBefore %d myFileName \"%s\" configFileName \"%s\" mapFileTime %lu readFileTime %lu (Diff %ld)", 
           myConfigProcessedBefore, myFileName.c_str(), myConfigMapName,
           mapFileStat.st_mtime, myReadFileStat.st_mtime, mapFileStat.st_mtime - myReadFileStat.st_mtime);
    myConfigProcessedBefore = true; 

    if(readFile(myConfigMapName, errorBuffer, errorBufferLen))
    {
      return true;
    }
    else
    {
      ArLog::log(ArLog::Terse, "ArMap: failed to read new map file \"%s\": %s.", myConfigMapName, errorBuffer);
      // TODO: if !myConfigProcessedBefore then the intial map file name was
      // invalid; should we now clear the map file name in ArConfig?
      return false;
    }
  }

  // Otherwise, nothing to do.
  return true;
}

AREXPORT const char *ArMap::getBaseDirectory(void) const
{
  return myBaseDirectory.c_str();
}

AREXPORT const char *ArMap::getFileName(void) const 
{
  return myFileName.c_str();
}

AREXPORT void ArMap::setBaseDirectory(const char *baseDirectory)
{
  if (baseDirectory != NULL && strlen(baseDirectory) > 0)
    myBaseDirectory = baseDirectory;
  else
    myBaseDirectory = "";  
  if (myLoadingParser != NULL)
    myLoadingParser->setBaseDirectory(myBaseDirectory.c_str());
}

AREXPORT std::list<ArArgumentBuilder *> *ArMap::getInfo(int infoType) 
{ 
    if ((infoType >= 0) && (infoType < myNumInfos) && (myInfoArray != NULL)) {
        return &myInfoArray[infoType]; 
    }
    else {
        return NULL;
    }
}

AREXPORT void ArMap::addMapChangedCB(ArFunctor *functor, ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myMapChangedCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myMapChangedCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArMap::addMapChangedCB: Invalid position.");
}

AREXPORT void ArMap::remMapChangedCB(ArFunctor *functor)
{
  myMapChangedCBList.remove(functor);
}


AREXPORT void ArMap::addPreMapChangedCB(ArFunctor *functor, ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myPreMapChangedCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myPreMapChangedCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArMap::addPreMapChangedCB: Invalid position.");
}

AREXPORT void ArMap::remPreMapChangedCB(ArFunctor *functor)
{
  myPreMapChangedCBList.remove(functor);
}

AREXPORT bool ArMap::reset(void)
{

  // clear out the reading in copy

  // get rid of the old parser (cleaner than trying to clear it up)
  if (myLoadingParser != NULL)
    delete myLoadingParser;
  myLoadingParser = new ArFileParser;
  myLoadingParser->setBaseDirectory(myBaseDirectory.c_str());
  myLoadingGotMaxPos = false;
  myLoadingGotMinPos = false;
  myLoadingGot2DMap = false;
  myLoadingNumPoints = -1;
  myLoadingResolution = -1;
  myLoadingPointsRead = 0;
  myLoadingLinesRead = 0;
  myLoadingMax.setX(INT_MIN);
  myLoadingMax.setY(INT_MIN);
  myLoadingMin.setX(INT_MAX);
  myLoadingMin.setY(INT_MAX);
  ArUtil::deleteSet(myLoadingMapObjects.begin(), myLoadingMapObjects.end());
  myLoadingMapObjects.clear();
  myLoadingPoints.clear();
  myLoadingLines.clear();
  ArUtil::deleteSet(myLoadingMapInfo.begin(), myLoadingMapInfo.end());
  myLoadingMapInfo.clear();

  for (int i = 0; i < myNumInfos; i++) {
      
    ArUtil::deleteSet(myLoadingInfoArray[i].begin(), myLoadingInfoArray[i].end());
    myLoadingInfoArray[i].clear();
  }
/***
  ArUtil::deleteSet(myLoadingRouteInfo.begin(), myLoadingRouteInfo.end());
  myLoadingRouteInfo.clear();
***/

  // ??
  myLoadingDataStarted = false;
  myLoadingLinesAndDataStarted = false;

  if (!myLoadingParser->addHandler("2D-Map", &my2DMapCB))
  {
    ArLog::log(ArLog::Terse, "ArMap::reset: could not add 2D-Map");
    return false;
  }

  return true;
}

bool ArMap::handle2DMap(ArArgumentBuilder *arg)
{
  ArLog::log(ArLog::Verbose, "ArMap: got 2D-Map");
  // make sure we can add all our handlers
  if (!myLoadingParser->addHandler("MinPos:", &myMinPosCB) ||
      !myLoadingParser->addHandler("MaxPos:", &myMaxPosCB) ||
      !myLoadingParser->addHandler("NumPoints:", &myNumPointsCB) ||
      !myLoadingParser->addHandler("LineMinPos:", &myMinPosCB) ||
      !myLoadingParser->addHandler("LineMaxPos:", &myMaxPosCB) ||
      !myLoadingParser->addHandler("NumLines:", &myNumPointsCB) ||
      !myLoadingParser->addHandler("Resolution:", &myResolutionCB) ||
      !myLoadingParser->addHandler("Cairn:", &myMapObjectCB) ||
      !myLoadingParser->addHandler("MapInfo:", &myMapInfoCB) ||

      // !myLoadingParser->addHandler("RouteInfo:", &myRouteInfoCB) ||
      !myLoadingParser->addHandler("LINES", &myLinesCB) ||
      !myLoadingParser->addHandler("DATA", &myDataCB))      
  {
    ArLog::log(ArLog::Terse, "ArMap::handle2DMap: could not add handlers");
    return false;
  }  

  for (int i = 0; i < myNumInfos; i++) {
    if (!myLoadingParser->addHandler(getInfoName(i), 
                                                     myInfoCBArray[i])) {
      ArLog::log(ArLog::Terse, "ArMap::handle2DMap: could not add info handlers");
      return false;
    }
  }

  // we added 'em all, remove the 2D-Map handler and return
  myLoadingParser->remHandler("2D-Map");
  myLoadingGot2DMap = true;
  return true;
  
}

bool ArMap::handleMinPos(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 2 || !arg->isArgInt(0) || !arg->isArgInt(1))
  {
    ArLog::log(ArLog::Terse, 
           "ArMap: 'MinPos:' bad arguments, should be two integers x y");
    return false;
  }
  myLoadingMinFromFile.setPose(arg->getArgInt(0), arg->getArgInt(1));
  myLoadingGotMinPos = true;
  return true;
}

bool ArMap::handleMaxPos(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 2 || !arg->isArgInt(0) || !arg->isArgInt(1))
  {
    ArLog::log(ArLog::Terse, 
           "ArMap: 'MaxPos:' bad arguments, should be two integers x y");
    return false;
  }
  myLoadingMaxFromFile.setPose(arg->getArgInt(0), arg->getArgInt(1));
  myLoadingGotMaxPos = true;
  return true;
}

bool ArMap::handleNumPoints(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 1 || !arg->isArgInt(0))
  {
    ArLog::log(ArLog::Terse, 
           "ArMap: 'NumPoints:' bad argument, should be one integer (number of data points)");
    return false;
  }
  myLoadingNumPoints = arg->getArgInt(0);

  if (myLoadingNumPoints >= 0) {
      myLoadingPoints.reserve(myLoadingNumPoints);
  }

  return true;
}

bool ArMap::handleLineMinPos(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 2 || !arg->isArgInt(0) || !arg->isArgInt(1))
  {
    ArLog::log(ArLog::Terse, 
       "ArMap: 'LineMinPos:' bad arguments, should be two integers x y");
    return false;
  }
  myLoadingLineMinFromFile.setPose(arg->getArgInt(0), arg->getArgInt(1));
  myLoadingGotLineMinPos = true;
  return true;
}

bool ArMap::handleLineMaxPos(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 2 || !arg->isArgInt(0) || !arg->isArgInt(1))
  {
    ArLog::log(ArLog::Terse, 
       "ArMap: 'LineMaxPos:' bad arguments, should be two integers x y");
    return false;
  }
  myLoadingLineMaxFromFile.setPose(arg->getArgInt(0), arg->getArgInt(1));
  myLoadingGotLineMaxPos = true;
  return true;
}

bool ArMap::handleNumLines(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 1 || !arg->isArgInt(0))
  {
    ArLog::log(ArLog::Terse, 
           "ArMap: 'NumPoints:' bad argument, should be one integer (number of data points)");
    return false;
  }
  myLoadingNumLines = arg->getArgInt(0);

  if (myLoadingNumLines >= 0) {
      myLoadingLines.reserve(myLoadingNumPoints);
  }

  return true;
}

bool ArMap::handleResolution(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 1 || !arg->isArgInt(0))
  {
    ArLog::log(ArLog::Terse, 
           "ArMap: 'Resolution:' bad argument, should be one integer (resolution in mm)");
    return false;
  }
  myLoadingResolution = arg->getArgInt(0);
  return true;
}

bool ArMap::handleMapObject(ArArgumentBuilder *arg)
{
  ArMapObject *object;
  ArPose pose;
  bool hasFromTo = false;
  ArPose fromPose;
  ArPose toPose;

  if (arg->getArgc() < 7 || !arg->isArgDouble(1) || !arg->isArgDouble(2) ||
      !arg->isArgDouble(3))
  {
    ArLog::log(ArLog::Terse, 
           "ArMap: 'Cairn:' bad format '%s'", arg->getFullString());
    return false;
  }
  // this next block strips out the end quotes of the name
  /*
  char *nameBuffer;
  const char *namePtr;
  size_t nameLen;
  namePtr = arg->getArg(6);
  nameLen = strlen(namePtr);
  nameBuffer = new char[strlen(arg->getArg(6)) + 1];
  strncpy(nameBuffer, &namePtr[1], nameLen - 1);
  nameBuffer[nameLen - 1] = '\0';
  */
  arg->compressQuoted();

  
  size_t fileLen = strlen(arg->getArg(4)) + 1;
  char *fileBuffer = new char[fileLen];
  
  //char *fileBuffer = NULL;
/***/
  if (!ArUtil::stripQuotes(fileBuffer, arg->getArg(4), fileLen))
  {
    ArLog::log(ArLog::Terse, 
           "ArMap: 'Cairn:' couldn't strip quotes from fileName '%s'", 
           arg->getArg(4));
    delete[] fileBuffer;
    return false;
  }


  size_t nameLen = strlen(arg->getArg(6)) + 1;
  char *nameBuffer = new char[nameLen];
  if (!ArUtil::stripQuotes(nameBuffer, arg->getArg(6), nameLen))
  {
    ArLog::log(ArLog::Terse, 
           "ArMap: 'Cairn:' couldn't strip quotes from name '%s'", 
           arg->getArg(6));
    delete[] fileBuffer;
    delete[] nameBuffer;
    return false;
  }
  
  if (arg->getArgc() == 11 && arg->isArgInt(7) && arg->isArgInt(8) && 
      arg->isArgInt(9) && arg->isArgInt(10))
  {
    hasFromTo = true;
    fromPose.setPose(arg->getArgInt(7), arg->getArgInt(8));
    toPose.setPose(arg->getArgInt(9), arg->getArgInt(10));
             
  }
  pose.setPose(arg->getArgDouble(1), arg->getArgDouble(2), 
           arg->getArgDouble(3));
  object = new ArMapObject(arg->getArg(0), pose, fileBuffer,
               arg->getArg(5), nameBuffer, hasFromTo, fromPose, 
               toPose);
  delete [] fileBuffer;
  delete [] nameBuffer;
  myLoadingMapObjects.push_back(object);
  //object->log();
  //arg->log();
  return true;
}

bool ArMap::handleMapInfo(ArArgumentBuilder *arg)
{
  ArArgumentBuilder *mapInfo = NULL;
  mapInfo = new ArArgumentBuilder(*arg);
  myLoadingMapInfo.push_back(mapInfo);
  return true;
}

bool ArMap::handleInfo(ArArgumentBuilder *arg, int info) 
{
  arg->compressQuoted();
  
  if ((info < 0) || (info >= myNumInfos)) {
    return false;
  }
  ArArgumentBuilder *infoBuilder = new ArArgumentBuilder(*arg);
  myLoadingInfoArray[info].push_back(infoBuilder);

  ArLog::log(ArLog::Verbose,
             "ArMap::handleInfo(%i) = %s",
             info, 
             ((arg != NULL) ? arg->getFullString() : "NULL"));

  return true;
}

bool ArMap::handleData(ArArgumentBuilder *arg)
{
  // make sure we got all the important data
  if (!myLoadingGotMinPos || !myLoadingGotMaxPos || myLoadingNumPoints == -1)
  {
    ArLog::log(ArLog::Verbose, "ArMap: got to DATA but didn't get all the important information, using our own anyways, but something is probably wrong");
  }
  // get rid of the old handlers
  myLoadingParser->remHandler(&myMinPosCB);
  myLoadingParser->remHandler(&myMaxPosCB);
  myLoadingParser->remHandler(&myNumPointsCB);
  myLoadingParser->remHandler(&myLineMinPosCB);
  myLoadingParser->remHandler(&myLineMaxPosCB);
  myLoadingParser->remHandler(&myNumLinesCB);
  myLoadingParser->remHandler(&myResolutionCB);
  myLoadingParser->remHandler(&myMapObjectCB);
  myLoadingParser->remHandler(&myMapInfoCB);

  for (int i = 0; i < myNumInfos; i++) {
      myLoadingParser->remHandler(myInfoCBArray[i]);
  }
  // myLoadingParser->remHandler(&myRouteInfoCB);
  myLoadingParser->remHandler(&myResolutionCB);

  //myLoadingParser->remHandler(&myDataCB);
  //myLoadingParser->remHandler(&myLinesCB);
  myLoadingParser->remHandler(&myLineCB);
  myLoadingParser->remHandler(&myPointCB);

  // now we add our handler for the points
  if (!myLoadingParser->addHandler(NULL, &myPointCB))
  {
    ArLog::log(ArLog::Terse, "ArMap: could not add handler for data points");
    return false;
  }
  myLoadingDataStarted = true;
  myLoadingLinesAndDataStarted = false;
  return false;

  // return true;
}

bool ArMap::handleLines(ArArgumentBuilder *arg)
{
  // make sure we got all the important data
  if (!myLoadingGotMinPos || !myLoadingGotMaxPos || myLoadingNumPoints == -1)
  {
    ArLog::log(ArLog::Verbose, "ArMap: got to DATA but didn't get all the important information, using our own anyways, but something is probably wrong");
  }
  // get rid of the old handlers
  myLoadingParser->remHandler(&myMinPosCB);
  myLoadingParser->remHandler(&myMaxPosCB);
  myLoadingParser->remHandler(&myNumPointsCB);
  myLoadingParser->remHandler(&myLineMinPosCB);
  myLoadingParser->remHandler(&myLineMaxPosCB);
  myLoadingParser->remHandler(&myNumLinesCB);
  myLoadingParser->remHandler(&myResolutionCB);
  myLoadingParser->remHandler(&myMapObjectCB);
  myLoadingParser->remHandler(&myResolutionCB);

  for (int i = 0; i < myNumInfos; i++) {
      myLoadingParser->remHandler(myInfoCBArray[i]);
  }

  // Don't remove the data cb because the points follow...
  //myLoadingParser->remHandler(&myDataCB);
  //myLoadingParser->remHandler(&myLinesCB); 

  myLoadingParser->remHandler(&myPointCB);  
  myLoadingParser->remHandler(&myLineCB);  

  // now we add our handler for the points
  if (!myLoadingParser->addHandler(NULL, &myLineCB))
  {
    ArLog::log(ArLog::Terse, "ArMap: could not add handler for data lines");
    return false;
  }
  myLoadingLinesAndDataStarted = true;
  myLoadingDataStarted = false;
  return false;

  // return true;
}

bool ArMap::handlePoint(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 2 || !arg->isArgInt(0) || !arg->isArgInt(1))
  {
    ArLog::log(ArLog::Terse, 
       "ArMap::handlePoint: map point wrong, should be x and y int coords (in mm) but is %s", arg->getFullString());
    return false;
  }

  int x = arg->getArgInt(0);
  int y = arg->getArgInt(1);

  loadDataPoint(x, y);
  return true;

}


bool ArMap::handleLine(ArArgumentBuilder *arg)
{
  if (arg->getArgc() != 4 || !arg->isArgInt(0) || !arg->isArgInt(1) || 
      !arg->isArgInt(2) || !arg->isArgInt(3))
  {
    ArLog::log(ArLog::Verbose, 
       "ArMap::handleLine: line wrong, should be 2 x, y points (in mm) but is %s", arg->getFullString());
    return false;
  }

  int x1 = arg->getArgInt(0);
  int y1 = arg->getArgInt(1);
  int x2 = arg->getArgInt(2);
  int y2 = arg->getArgInt(3);

  loadLineSegment(x1, y1, x2, y2);
  return true;

}


AREXPORT ArTime ArMap::getInfoChanged(int infoType)
{
  if ((infoType >= 0) && (infoType < myNumInfos) && (myInfoChangedArray != NULL)) {
    return myInfoChangedArray[infoType]; 
  }
  else { // invalid info type
    // ArTime is initialized to now.  Set the secs and msecs to 0 for default value.
    ArTime defaultTime;
    defaultTime.setSec(0);
    defaultTime.setMSec(0);
    return defaultTime;
  }
}

AREXPORT void ArMap::setMapObjects(const std::list<ArMapObject *> *mapObjects)
{
  std::list<ArMapObject *>::const_iterator it;
  ArUtil::deleteSet(myMapObjects.begin(), myMapObjects.end());
  myMapObjects.clear();
  myMapObjectsChanged.setToNow();
  if (mapObjects == NULL)
    return;
  for (it = mapObjects->begin(); it != mapObjects->end(); it++)
  {
    myMapObjects.push_back(new ArMapObject(*(*it)));
  }
}

AREXPORT void ArMap::setPoints(const std::vector<ArPose> *points)
{
  std::vector<ArPose>::const_iterator it;
  ArPose pose;
  /***/
  myPoints.clear();
  myMax.setX(INT_MIN);
  myMax.setY(INT_MIN);
  myMin.setX(INT_MAX);
  myMin.setY(INT_MAX);
  myNumPoints = 0;


  myPointsChanged.setToNow();
  if (points == NULL) {
    return;
  }


  myPoints.reserve(points->size());

  for (it = points->begin(); it != points->end(); it++)
  {
    pose = (*it);
    if (pose.getX() > myMax.getX())
      myMax.setX(pose.getX());
    if (pose.getY() > myMax.getY())
      myMax.setY(pose.getY());

    if (pose.getX() < myMin.getX())
      myMin.setX(pose.getX());
    if (pose.getY() < myMin.getY())
      myMin.setY(pose.getY());
    
    myNumPoints++;
    myPoints.push_back(pose);
  }
  

}


AREXPORT void ArMap::setLines(const std::vector<ArLineSegment> *lines)
{
  std::vector<ArLineSegment>::const_iterator it;
  ArLineSegment line;
  /***/
  myLines.clear();
  myLineMax.setX(INT_MIN);
  myLineMax.setY(INT_MIN);
  myLineMin.setX(INT_MAX);
  myLineMin.setY(INT_MAX);
  myNumLines = 0;


  myLinesChanged.setToNow();
  if (lines == NULL) {
    return;
  }


  myLines.reserve(lines->size());

  for (it = lines->begin(); it != lines->end(); it++)
  {
    line = (*it);


    if (line.getX1() > myLineMax.getX())
      myLineMax.setX(line.getX1());
    if (line.getY1() > myLineMax.getY())
      myLineMax.setY(line.getY1());

    if (line.getX1() < myLineMin.getX())
      myLineMin.setX(line.getX1());
    if (line.getY1() < myLineMin.getY())
      myLineMin.setY(line.getY1());


    if (line.getX2() > myLineMax.getX())
      myLineMax.setX(line.getX2());
    if (line.getY2() > myLineMax.getY())
      myLineMax.setY(line.getY2());

    if (line.getX2() < myLineMin.getX())
      myLineMin.setX(line.getX2());
    if (line.getY2() < myLineMin.getY())
      myLineMin.setY(line.getY2());
    
    myNumLines++;
    myLines.push_back(line);
  }
  

}

AREXPORT void ArMap::setMapInfo(const std::list<ArArgumentBuilder *> *mapInfo)
{
  std::list<ArArgumentBuilder *>::const_iterator it;
  ArUtil::deleteSet(myMapInfo.begin(), myMapInfo.end());
  myMapInfo.clear();
  myMapInfoChanged.setToNow();
  if (mapInfo == NULL)
    return;
  for (it = mapInfo->begin(); it != mapInfo->end(); it++)
  {
    myMapInfo.push_back(new ArArgumentBuilder(*(*it)));
  }  
}

AREXPORT bool ArMap::setInfo(int infoType,
                             const std::list<ArArgumentBuilder *> *infoList)
{
  if ((infoType < 0) || (infoType >= myNumInfos)) {
    return false;
  }
  // Should never happen, but check just in case...
  if ((myInfoArray == NULL) || (myInfoChangedArray == NULL)) {
      return false;
  }
  std::list<ArArgumentBuilder *>::const_iterator it;
  ArUtil::deleteSet(myInfoArray[infoType].begin(), myInfoArray[infoType].end());
  myInfoArray[infoType].clear();
  myInfoChangedArray[infoType].setToNow();

  if (infoList == NULL)
    return true;

  for (it = infoList->begin(); it != infoList->end(); it++)
  {
    myInfoArray[infoType].push_back(new ArArgumentBuilder(*(*it)));
  }
  return true;
}

AREXPORT void ArMap::setResolution(int resolution)
{
    myResolution = resolution;
}


/***
AREXPORT void ArMap::setRouteInfo(const std::list<ArArgumentBuilder *> *routeInfo)
{
  std::list<ArArgumentBuilder *>::const_iterator it;
  ArUtil::deleteSet(myRouteInfo.begin(), myRouteInfo.end());
  myRouteInfo.clear();
  myRouteInfoChanged.setToNow();
  if (routeInfo == NULL)
    return;
  for (it = routeInfo->begin(); it != routeInfo->end(); it++)
  {
    myRouteInfo.push_back(new ArArgumentBuilder(*(*it)));
  }  
}
**/

AREXPORT bool ArMap::readFile(const char *fileName, 
                  char *errorBuffer, size_t errorBufferLen)
{
 
  lock();
  stat(fileName, &myReadFileStat);
  FILE *file;

  char line[10000];
  std::string realFileName;
  if (fileName[0] == '/' || fileName[0] == '\\' || 
      fileName[0] == '.')
  {
    realFileName = fileName;
  }
  else
  {
    realFileName = myBaseDirectory;
    realFileName += fileName;
  }

  ArLog::log(ArLog::Verbose, "Opening file %s from fileName given %s", realFileName.c_str(), fileName);
    
  if ((file = fopen(realFileName.c_str(), "r")) == NULL)
  {
    ArLog::log(ArLog::Terse, "Cannot open file '%s'", realFileName.c_str());
    if (errorBuffer != NULL)
      snprintf(errorBuffer, errorBufferLen, 
           "%s invalid: cannot open file '%s'",
           myConfigParam.c_str(), fileName);
    //printf("!     %s\n", errorBuffer);

    unlock();
    return false;
  }

  if (!reset() || !myLoadingParser->parseFile(file, line, 10000, false))
  {
    
    if (!myLoadingDataStarted && !myLoadingLinesAndDataStarted) 
    {
      reset();
      if (errorBuffer != NULL)
    snprintf(errorBuffer, errorBufferLen, 
         "%s invalid: '%s' not a valid map",
         myConfigParam.c_str(), fileName);
      unlock();
      ArLog::log(ArLog::Terse, "Could not load map file '%s'", fileName);
      
      return false;
    }
  } // end if parse success
  
  if (!myLoadingGot2DMap)
  {
    reset();
    if (errorBuffer != NULL)
      snprintf(errorBuffer, errorBufferLen, 
           "%s invalid, '%s' was not a map file",
           myConfigParam.c_str(), fileName);
    
    unlock();
    ArLog::log(ArLog::Terse, 
           "Could not load map file '%s' it was not a 2D-map", fileName);
    return false;
  }

  // if we're on the lines, read until the end of lines or end of file
  if (myLoadingLinesAndDataStarted)
  {
    //printf("Reading lines\n");
    while (fgets(line, sizeof(line), file) != NULL)
    {
      if (strncasecmp(line, "DATA", strlen("DATA")) == 0)
      {
    myLoadingDataStarted = true;
    break;
      }
      if (!readLineSegment(line))
      {
    continue;
      }
    }
  }

  //printf("reading data\n");
  // read until the end of the file
  while (fgets(line, sizeof(line), file) != NULL)
  {
    if (!readDataPoint(line))
    {
      continue;
    }
  }
  
  fclose(file);
  
  // move the stuff over from reading to new
  myFileName = fileName;
  setMapObjects(&myLoadingMapObjects);
  setPoints(&myLoadingPoints);
  setLines(&myLoadingLines);
  setMapInfo(&myLoadingMapInfo);
  
  for (int i = 0; i < myNumInfos; i++) {
      setInfo(i, &myLoadingInfoArray[i]);
  }
  // setRouteInfo(&myLoadingRouteInfo);

  myResolution = myLoadingResolution;
  // call our callbacks
  ArLog::log(ArLog::Terse, "Loaded map file '%s'", fileName);

  reset();
  
  mapChanged();
  unlock();
  return true;
}

bool ArMap::readDataPoint(char *line) 
{
  if (line == NULL) {
    return false;
  }
  
  bool isNumber = true;
  int firstSpace = -1;
  
  unsigned int startNum = 0;
  
  for (unsigned int i = 0; i < strlen(line); i++) {
    if (!isdigit(line[i]) && !(i == startNum && line[i] == '-')) {
      
      if (isspace(line[i])) {
    line[i] = '\0';
    if (firstSpace < 0) {
      firstSpace = i;
      startNum = i + 1;
    }
    else {
      break;
    }
      }
      else {
    isNumber = false;
      }
      break;
    }
  } // end for each
  
  int x = atoi(line);
  int y = atoi(&line[firstSpace + 1]);
  
  loadDataPoint(x, y);

  return true;

} // end method readDataPoint




void ArMap::loadDataPoint(double x, double y)
{
  if (x > myLoadingMax.getX())
    myLoadingMax.setX(x);
  if (y > myLoadingMax.getY())
    myLoadingMax.setY(y);
  
  if (x < myLoadingMin.getX())
    myLoadingMin.setX(x);
  if (y < myLoadingMin.getY())
    myLoadingMin.setY(y);
  
  myLoadingPoints.push_back(ArPose(x, y));
  myLoadingPointsRead++;
  
} // end method loadDataPoint


bool ArMap::readLineSegment(char *line) 
{
  if (line == NULL) {
    return false;
  }
  
  /* MPL optomize this like kathleen did above, later */
  ArArgumentBuilder builder;
  builder.add(line);
  
  if (builder.getArgc() < 4)
    return false;

  loadLineSegment(builder.getArgInt(0), builder.getArgInt(1),
          builder.getArgInt(2), builder.getArgInt(3));

  return true;

} 

void ArMap::loadLineSegment(double x1, double y1, double x2, double y2)
{
  if (x1 > myLoadingLineMax.getX())
    myLoadingLineMax.setX(x1);
  if (y1 > myLoadingLineMax.getY())
    myLoadingLineMax.setY(y1);
  
  if (x1 < myLoadingLineMin.getX())
    myLoadingLineMin.setX(x1);
  if (y1 < myLoadingLineMin.getY())
    myLoadingMin.setY(y1);

  if (x2 > myLoadingLineMax.getX())
    myLoadingLineMax.setX(x2);
  if (y2 > myLoadingLineMax.getY())
    myLoadingLineMax.setY(y2);
  
  if (x2 < myLoadingLineMin.getX())
    myLoadingLineMin.setX(x2);
  if (y2 < myLoadingLineMin.getY())
    myLoadingMin.setY(y2);
  
  myLoadingLines.push_back(ArLineSegment(x1, y1, x2, y2));
  myLoadingLinesRead++;
  
} 

AREXPORT void ArMap::parsingComplete(void)
{
  lock();
  setMapObjects(&myLoadingMapObjects);
  setPoints(&myLoadingPoints);
  setLines(&myLoadingLines);
  setMapInfo(&myLoadingMapInfo);

  for (int i = 0; i < myNumInfos; i++) {
      setInfo(i, &myLoadingInfoArray[i]);
  }
  //setRouteInfo(&myLoadingRouteInfo);

  myResolution = myLoadingResolution;
  ArLog::log(ArLog::Normal, "Loaded map from parsing");
  myFileName = "";

  reset();

  mapChanged();
  unlock();
}

AREXPORT void ArMap::mapChanged(void)
{
  std::list<ArFunctor *>::iterator it;

  ArLog::LogLevel level = myMapChangedLogLevel;
  ArFunctor *functor;

  bool isAnyInfoChanged = false;
  for (int i = 0; i < myNumInfos; i++) {
      if (abs(myMapChangedInfoArray[i].mSecSince(myInfoChangedArray[i])) > 0) {
          isAnyInfoChanged = true;
          break;
      }
  }
  ArLog::log(level, "ArMap: Calling mapChanged callbacks");
  if (abs(myMapChangedMapObjects.mSecSince(myMapObjectsChanged)) > 0 || 
      abs(myMapChangedPoints.mSecSince(myPointsChanged)) > 0 || 
      abs(myMapChangedMapInfo.mSecSince(myMapInfoChanged)) > 0 ||
      isAnyInfoChanged) 
      //abs(myMapChangedRouteInfo.mSecSince(myRouteInfoChanged)) > 0)
  {
    for (it = myPreMapChangedCBList.begin(); 
     it != myPreMapChangedCBList.end(); 
     it++)
    {
      functor = (*it);
      if (functor->getName() != NULL && functor->getName()[0] != '\0')
    ArLog::log(level, "ArMap: Calling preMapChanged functor '%s'", 
           functor->getName());
      else
    ArLog::log(level, "ArMap: Calling unnamed preMapChanged functor");
      
      functor->invoke();
    }

    for (it = myMapChangedCBList.begin(); it != myMapChangedCBList.end(); it++)
    {
      functor = (*it);
      if (functor->getName() != NULL && functor->getName()[0] != '\0')
    ArLog::log(level, "ArMap: Calling mapChanged functor '%s'", 
           functor->getName());
      else
    ArLog::log(level, "ArMap: Calling unnamed mapChanged functor");
      
      functor->invoke();
    }
  }
  myMapChangedMapObjects = myMapObjectsChanged;
  myMapChangedPoints = myPointsChanged;
  myMapChangedMapInfo = myMapInfoChanged;
  //myMapChangedRouteInfo = myRouteInfoChanged;
  for (int i = 0; i < myNumInfos; i++) {
      myMapChangedInfoArray[i] = myInfoChangedArray[i];
  }
  ArLog::log(level, "ArMap: Done calling mapChanged callbacks");
}


AREXPORT void ArMap::writeToFunctor(ArFunctor1<const char *> *functor,
                    const char *endOfLineChars)
{
  // Write the header information and Cairn objects...
  writeObjectsToFunctor(functor, endOfLineChars);

  if (myLines.begin() != myLines.end())
  {
    // Write the map data points in text format....
    std::vector<ArLineSegment>::iterator lineIt;
    
    ArUtil::functorPrintf(functor, "LINES%s", endOfLineChars);
    
    for (lineIt = myLines.begin(); 
     lineIt != myLines.end();
     lineIt++)
    {
      ArUtil::functorPrintf(functor, "%.0f %.0f %.0f %.0f%s", 
                (*lineIt).getX1(), (*lineIt).getY1(), 
                (*lineIt).getX2(), (*lineIt).getY2(),
                endOfLineChars);
    }
  }
  
  ArUtil::functorPrintf(functor, "DATA%s", endOfLineChars);
  
  if (myPoints.begin() != myPoints.end())
  {
    // Write the map data points in text format....
    std::vector<ArPose>::iterator pointIt;
    
    for (pointIt = myPoints.begin(); 
     pointIt != myPoints.end();
     pointIt++)
    {
      ArUtil::functorPrintf(functor, "%.0f %.0f%s", (*pointIt).getX(), 
                (*pointIt).getY(), endOfLineChars);
    }
  }
}


void ArMap::writeObjectsToFunctor
                (ArFunctor1<const char *> *functor, 
                    const char *endOfLineChars)
{
  std::list<ArMapObject *>::iterator mapObjectIt;
  //std::vector<ArPose>::iterator pointIt;
  std::list<ArArgumentBuilder *>::iterator mapInfoIt;
  //std::list<ArArgumentBuilder *>::iterator routeInfoIt;

  // toss out the general data
  ArUtil::functorPrintf(functor, "2D-Map%s", endOfLineChars);
  if (myNumPoints != 0)
  {
    ArUtil::functorPrintf(functor, "MinPos: %.0f %.0f%s", 
              myMin.getX(), myMin.getY(),  endOfLineChars);
    ArUtil::functorPrintf(functor, "MaxPos: %.0f %.0f%s", 
              myMax.getX(), myMax.getY(),  endOfLineChars);
    ArUtil::functorPrintf(functor, "NumPoints: %d%s", 
              myNumPoints, endOfLineChars);
  }
  if (myResolution != -1)
    ArUtil::functorPrintf(functor, "Resolution: %d%s", myResolution, 
              endOfLineChars);
  if (myNumLines != 0)
  {
    ArUtil::functorPrintf(functor, "LineMinPos: %.0f %.0f%s", 
              myLineMin.getX(), myLineMin.getY(),  endOfLineChars);
    ArUtil::functorPrintf(functor, "LineMaxPos: %.0f %.0f%s", 
              myLineMax.getX(), myLineMax.getY(),  endOfLineChars);
    ArUtil::functorPrintf(functor, "NumLines: %d%s", 
              myNumLines, endOfLineChars);
  }
  for (mapInfoIt = myMapInfo.begin(); 
       mapInfoIt != myMapInfo.end(); 
       mapInfoIt++)
    ArUtil::functorPrintf(functor, "MapInfo: %s%s", 
              (*mapInfoIt)->getFullString(), 
        endOfLineChars);

  for (int i = 0; i < myNumInfos; i++) {
      const char *infoName = getInfoName(i);
      if (infoName == NULL) {
        ArLog::log(ArLog::Normal, "ArMap: Cannot find Info tag for index %i, not saving...",
                   i);
        continue;
      }

      for (std::list<ArArgumentBuilder *>::iterator infoIt = myInfoArray[i].begin();
           infoIt != myInfoArray[i].end();
           infoIt++) {

        ArUtil::functorPrintf(functor, "%s %s%s", 
                              infoName,
                              (*infoIt)->getFullString(), 
                              endOfLineChars);

      } // end for each info in list
  } // end for each info type

  for (mapObjectIt = myMapObjects.begin(); 
       mapObjectIt != myMapObjects.end(); 
       mapObjectIt++)
  {
    ArMapObject *object = (*mapObjectIt);
    if (object->hasFromTo())
      ArUtil::functorPrintf(functor, 
          "Cairn: %s %g %g %g \"%s\" %s \"%s\" %.0f %.0f %.0f %.0f%s",
          object->getType(), object->getPose().getX(), 
          object->getPose().getY(), object->getPose().getTh(), 
          object->getFileName(), object->getIconName(), 
          object->getName(), object->getFromPose().getX(),
          object->getFromPose().getY(),
          object->getToPose().getX(), object->getToPose().getY(), 
          endOfLineChars);
    else
      ArUtil::functorPrintf(functor, 
          "Cairn: %s %g %g %g \"%s\" %s \"%s\"%s",
          object->getType(), object->getPose().getX(), 
          object->getPose().getY(), object->getPose().getTh(), 
          object->getFileName(), object->getIconName(), 
          object->getName(), endOfLineChars);
  }
 
}


void ArMap::writePointsToFunctor
(ArFunctor2<int, std::vector<ArPose> *> *functor)
{
    functor->invoke(myNumPoints, &myPoints);
}

void ArMap::writeLinesToFunctor
(ArFunctor2<int, std::vector<ArLineSegment> *> *functor)
{
    functor->invoke(myNumLines, &myLines);
}



AREXPORT bool ArMap::writeFile(const char *fileName, 
                   bool internalCall)
{
  FILE *file;
  if (!internalCall)
    lock();
  // later this'll have a prefix
  std::string realFileName;

  if (fileName[0] == '/' || fileName[0] == '\\')
  {
    realFileName = fileName;
  }
  else
  {
    realFileName = myBaseDirectory;
    realFileName += fileName;
  }
  
  // make sure its there
  if ((file = fopen(realFileName.c_str(), "w")) == NULL)
  {
    ArLog::log(ArLog::Terse, "ArMap: Cannot open file '%s' for writing",
           realFileName.c_str());
    if (!internalCall)
      unlock();
    return false;
  }

  ArGlobalFunctor2<const char *, FILE *> functor(&ArUtil::writeToFile, "", file);
  writeToFunctor(&functor, "\n");
  
  fclose(file);
  ArLog::log(ArLog::Normal, "Saved map file %s", fileName);
  if (!internalCall)
    unlock();
  return true;
}

AREXPORT bool ArMap::parseLine(char *line)
{
  return myLoadingParser->parseLine(line);
}


AREXPORT ArMapObject *ArMap::findMapObject(const char *name, 
                       const char *type)
{
  std::list<ArMapObject *>::iterator objIt;
  ArMapObject* obj;

  for (objIt = getMapObjects()->begin(); 
       objIt != getMapObjects()->end(); 
       objIt++)
  {
    obj = (*objIt);
    if(obj == NULL)
      return NULL;
    // if we're searching any type or its the right type then check the name
    if (type == NULL || strcasecmp(obj->getType(), type) == 0)
    {
      if(name == NULL || strcasecmp(obj->getName(), name) == 0)
      {
    return obj;
      }
    }
  }

  // if we get down here we didn't find it
  return NULL;
}

const char *ArMap::getInfoName(int infoType)
{
    if ((infoType >= 0) && (infoType < INFO_COUNT)) {
        return ourInfoNames[infoType];
    }
    else {
        return NULL;
    }
}

AREXPORT ArMapObject *ArMap::findFirstMapObject(const char *name, 
                       const char *type)
{
  std::list<ArMapObject *>::iterator objIt;
  ArMapObject* obj;

  for (objIt = getMapObjects()->begin(); 
       objIt != getMapObjects()->end(); 
       objIt++)
  {
    obj = (*objIt);
    if(obj == NULL)
      return NULL;
    // if we're searching any type or its the right type then check the name
    if (type == NULL || strcasecmp(obj->getType(), type) == 0)
    {
      if(name == NULL || strcasecmp(obj->getName(), name) == 0)
      {
        return obj;
      }
    }
  }

  // if we get down here we didn't find it
  return NULL;
}

---