ArMap.h

/*
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
*/

#ifndef ARMAP_H
#define ARMAP_H

#include "ariaTypedefs.h"
#include "ariaUtil.h"
#include "ArFunctor.h"
#include "ArArgumentBuilder.h"
#include "ArMutex.h"

#include <vector>

class ArFileParser;

class ArMapObject;


class ArMap
{
  public:

  enum InfoType {
      // If any InfoType is added, then the ourInfoNames array must also be updated...
      //MAP_INFO,
    META_INFO,
      TASK_INFO,  
      ROUTE_INFO, 
    SCHED_TASK_INFO,
    SCHED_INFO,
      LAST_INFO = SCHED_INFO 
  };

  enum {
      INFO_COUNT = LAST_INFO + 1 
  };


  AREXPORT ArMap(const char *baseDirectory = "./",
         bool addToGlobalConfig = true, 
         const char *configSection = "Files",
         const char *configParam = "Map",
         const char *configDesc = 
         "Map of the environment we'll use to navigate",
         bool ignoreEmptyFileName = true);
  AREXPORT virtual ~ArMap(void);

  AREXPORT bool readFile(const char *fileName, 
             char *errorBuffer = NULL, size_t errorBufferLen = 0);

  AREXPORT bool writeFile(const char *fileName, bool internalCall = false);

  AREXPORT void addMapChangedCB(ArFunctor *functor, 
                ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remMapChangedCB(ArFunctor *functor);

  AREXPORT const char *getBaseDirectory(void) const;
  AREXPORT const char *getFileName(void) const;
  void setIgnoreEmptyFileName(bool ignore) { myIgnoreEmptyFileName = ignore; }
  bool getIgnoreEmptyFileName(void) { return myIgnoreEmptyFileName; }
  AREXPORT void setBaseDirectory(const char *baseDirectory);
  std::list<ArMapObject *> *getMapObjects(void) { return &myMapObjects; }

  AREXPORT ArMapObject *findFirstMapObject(const char *name, const char *type);


  std::list<ArArgumentBuilder *> *getMapInfo(void) { return &myMapInfo; }


  AREXPORT std::list<ArArgumentBuilder *> *getInfo(int infoType);

  std::vector<ArPose> *getPoints(void) { return &myPoints; }
  std::vector<ArLineSegment> *getLines(void) { return &myLines; }
  ArPose getMinPose(void) { return myMin; }
  ArPose getMaxPose(void) { return myMax; }
  int getNumPoints(void) { return myNumPoints; }
  ArPose getLineMinPose(void) { return myLineMin; }
  ArPose getLineMaxPose(void) { return myLineMax; }
  int getNumLines(void) { return myNumLines; }
  int getResolution(void) { return myResolution; }
  AREXPORT ArMapObject *findMapObject(const char *name, 
                      const char *type = NULL);
  ArTime getMapObjectsChanged(void) { return myMapObjectsChanged; }
  ArTime getPointsChanged(void) { return myPointsChanged; }
  ArTime getLinesChanged(void) { return myLinesChanged; }

  ArTime getMapInfoChanged(void) { return myMapInfoChanged; }


  AREXPORT ArTime getInfoChanged(int infoType); 

  AREXPORT void setMapObjects(const std::list<ArMapObject *> *mapObjects); 
  AREXPORT void setPoints(const std::vector<ArPose> *points); 
  AREXPORT void setLines(const std::vector<ArLineSegment> *lines);

  AREXPORT void setMapInfo(const std::list<ArArgumentBuilder *> *mapInfo);


  AREXPORT bool setInfo(int infoType,
                        const std::list<ArArgumentBuilder *> *infoList); 

  AREXPORT void setResolution(int resolution);
      
  /***
  AREXPORT void setRouteInfo(const std::list<ArArgumentBuilder *> *routeInfo); 
  ****/

  AREXPORT void mapChanged(void);

  AREXPORT void writeToFunctor(ArFunctor1<const char *> *functor, 
                   const char *endOfLineChars);


  AREXPORT bool parseLine(char *line);
  AREXPORT void parsingComplete(void);

  AREXPORT int lock() {return(myMutex.lock());}
  AREXPORT int tryLock() {return(myMutex.tryLock());}
  AREXPORT int unlock() {return(myMutex.unlock());}

  

  // When loading a map, returns whether all header, objects, and lines have completed loading.
  AREXPORT bool isLoadingDataStarted() {return myLoadingDataStarted;}

  // When loading a map, returns whether all header and objects have completed loading.
  AREXPORT bool isLoadingLinesAndDataStarted() 
               {return myLoadingLinesAndDataStarted;}


  AREXPORT void writeObjectsToFunctor
                    (ArFunctor1<const char *> *functor, 
                     const char *endOfLineChars);


  AREXPORT void writePointsToFunctor
            (ArFunctor2<int, std::vector<ArPose> *> *functor);

  AREXPORT void writeLinesToFunctor
        (ArFunctor2<int, std::vector<ArLineSegment> *> *functor);

  AREXPORT bool readDataPoint( char *line);
  AREXPORT bool readLineSegment( char *line);
  AREXPORT void loadDataPoint(double x, double y);
  AREXPORT void loadLineSegment(double x1, double y1, double x2, double y2);
  AREXPORT void setMapChangedLogLevel(ArLog::LogLevel level) 
    {  myMapChangedLogLevel = level; }
  AREXPORT ArLog::LogLevel getMapChangedLogLevel(void)
    {  return myMapChangedLogLevel; }
  AREXPORT void addPreMapChangedCB(ArFunctor *functor,
                                  ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remPreMapChangedCB(ArFunctor *functor);

protected:


  // Function for processing the config file
  bool processFile(char *errorBuffer, size_t errorBufferLen);
  // Function to read the 2D-Map
  bool handle2DMap(ArArgumentBuilder *arg);
  // Function to read the minimum pos
  bool handleMinPos(ArArgumentBuilder *arg);
  // Function to read the maximum pos
  bool handleMaxPos(ArArgumentBuilder *arg);
  // Function to read the number of points
  bool handleNumPoints(ArArgumentBuilder *arg);
  // Function to read the line minimum pos
  bool handleLineMinPos(ArArgumentBuilder *arg);
  // Function to read the line maximum pos
  bool handleLineMaxPos(ArArgumentBuilder *arg);
  // Function to read the number of lines
  bool handleNumLines(ArArgumentBuilder *arg);
  // Function to handle the resolution
  bool handleResolution(ArArgumentBuilder *arg);
  // Function to handle the cairns
  bool handleMapObject(ArArgumentBuilder *arg);

  bool handleInfo(ArArgumentBuilder *arg, int info);

  // Function to handle the information about the map
  bool handleMapInfo(ArArgumentBuilder *arg);

  // Function to catch the LINES line signifying data
  bool handleLines(ArArgumentBuilder *arg);
  // Function to catch the DATA line signifying data
  bool handleData(ArArgumentBuilder *arg);
  // Function to snag the map points (mainly for the getMap over the network)
  bool handlePoint(ArArgumentBuilder *arg);
  // Function to snag the line segments (mainly for the getMap over the network)
  bool handleLine(ArArgumentBuilder *arg);


  virtual const char *getInfoName(int infoType);

  static const char *ourInfoNames[INFO_COUNT];

  enum {
    MAX_MAP_NAME_LENGTH = 512
  };

  // lock for our data
  ArMutex myMutex;
  // resets the parser functions and variables (true if good, false otherwise)
  AREXPORT bool reset(void);
  std::string myBaseDirectory;
  std::string myFileName;
  struct stat myReadFileStat;
  ArFileParser *myLoadingParser;

  int myNumInfos;

  // vars for if we got some important info, the other important info
  // is taken care of by the adding of callbacks
  bool myLoadingGot2DMap;
  bool myLoadingGotMaxPos;
  bool myLoadingGotMinPos;
  bool myLoadingGotLineMaxPos;
  bool myLoadingGotLineMinPos;
  ArPose myLoadingMaxFromFile;
  ArPose myLoadingMinFromFile;
  int myLoadingPointsRead;
  ArPose myLoadingLineMaxFromFile;
  ArPose myLoadingLineMinFromFile;
  int myLoadingLinesRead;

  std::string myConfigParam;
  bool myIgnoreEmptyFileName;
  // data from the file
  std::list<ArMapObject *> myLoadingMapObjects;
  std::vector<ArPose> myLoadingPoints;
  std::vector<ArLineSegment> myLoadingLines;

  std::list<ArArgumentBuilder *> *myLoadingInfoArray;
  std::list<ArArgumentBuilder *> myLoadingMapInfo;
  int myLoadingNumPoints;
  int myLoadingNumLines;
  int myLoadingResolution;
  ArPose myLoadingMax;
  ArPose myLoadingMin;
  ArPose myLoadingLineMax;
  ArPose myLoadingLineMin;

  // our good data in memory (could be the same thing)
  std::list<ArMapObject *> myMapObjects;
  ArTime myMapObjectsChanged;
  std::vector<ArPose> myPoints;
  ArTime myPointsChanged;

  std::vector<ArLineSegment> myLines;
  ArTime myLinesChanged;

  std::list<ArArgumentBuilder *> *myInfoArray;
  ArTime *myInfoChangedArray;

  std::list<ArArgumentBuilder *> myMapInfo;
  ArTime myMapInfoChanged;


  int myNumPoints;
  int myNumLines;
  int myResolution;
  ArPose myMax;
  ArPose myMin;
  ArPose myLineMax;
  ArPose myLineMin;


  std::list<ArFunctor *> myMapChangedCBList;
  std::list<ArFunctor *> myPreMapChangedCBList;
  ArTime myMapChangedMapObjects;
  ArTime myMapChangedPoints;

  ArTime *myMapChangedInfoArray;
  ArTime myMapChangedMapInfo;


  // things for our config
  bool myConfigProcessedBefore;
  char myConfigMapName[MAX_MAP_NAME_LENGTH];

  bool myLoadingDataStarted;
  bool myLoadingLinesAndDataStarted;
  ArLog::LogLevel myMapChangedLogLevel;
  // callbacks
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> my2DMapCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myMinPosCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myMaxPosCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myNumPointsCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myLineMinPosCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myLineMaxPosCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myNumLinesCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myResolutionCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myMapObjectCB;


  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myMapInfoCB;
  ArRetFunctor2C<bool, ArMap, ArArgumentBuilder *, int> **myInfoCBArray;

  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myDataCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myLinesCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myPointCB;
  ArRetFunctor1C<bool, ArMap, ArArgumentBuilder *> myLineCB;
  ArRetFunctor2C<bool, ArMap, char *, size_t> myProcessFileCB;
};

class ArMapObject
{
public: 
  AREXPORT ArMapObject(const char *type, ArPose pose, const char *fileName,
               const char *iconName, const char *name,
               bool hasFromTo, ArPose fromPose, ArPose toPose)
    {
      if (type != NULL) myType = type; 
      if (name != NULL) myName = name; 
      myPose = pose; 
      if (iconName != NULL) myIconName = iconName;
      if (fileName != NULL) myFileName = fileName;
      myHasFromTo = hasFromTo; myFromPose = fromPose;  myToPose = toPose;
      if (myHasFromTo)
      {
       double angle = myPose.getTh();
       double sa = ArMath::sin(angle);
       double ca = ArMath::cos(angle);
       double fx = fromPose.getX();
       double fy = fromPose.getY();
       double tx = toPose.getX();
       double ty = toPose.getY();
       ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
       ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
       ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
       ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
       myFromToSegments.push_back(ArLineSegment(P0, P1));
       myFromToSegments.push_back(ArLineSegment(P1, P2));
       myFromToSegments.push_back(ArLineSegment(P2, P3));
       myFromToSegments.push_back(ArLineSegment(P3, P0));
      }
    }
  AREXPORT ArMapObject(const ArMapObject &mapObject) 
    {
      myType = mapObject.myType; myName = mapObject.myName; 
      myPose = mapObject.myPose; myIconName = mapObject.myIconName;
      myFileName = mapObject.myFileName;  myHasFromTo = mapObject.myHasFromTo;
      myFromPose = mapObject.myFromPose; myToPose = mapObject.myToPose;
      myFromToSegments = mapObject.myFromToSegments;
    }

  AREXPORT virtual ~ArMapObject() {}
  const char *getType(void) const { return myType.c_str(); }
  ArPose getPose(void) const { return myPose; }
  const char *getFileName(void) const { return myFileName.c_str(); }
  const char *getIconName(void) const { return myIconName.c_str(); }
  const char *getName(void) const { return myName.c_str(); }
  bool hasFromTo(void) const { return myHasFromTo; }
  ArPose getFromPose(void) const { return myFromPose; }
  ArPose getToPose(void) const { return myToPose; }
  void log(void) { 
    if (myHasFromTo)
      ArLog::log(ArLog::Terse, 
         "Cairn: %s %g %g %g \"%s\" %s \"%s\" %d %d %d %d",
         getType(), myPose.getX(), myPose.getY(), myPose.getTh(), 
         getFileName(), getIconName(), getName(), myFromPose.getX(),
         myFromPose.getY(), myToPose.getX(), myToPose.getY());
    else
      ArLog::log(ArLog::Terse, "Cairn: %s %g %g %g \"%s\" %s \"%s\"",
         getType(), myPose.getX(), myPose.getY(), myPose.getTh(), 
         getFileName(), getIconName(), getName());
  }

  std::list<ArLineSegment> getFromToSegments(void)
    {
      return myFromToSegments;
    }

  /*
  Don't use these, silently changing these values may screw up
  things that assume they don't change.
  void setPose(const ArPose& newpose) { myPose = newpose; }
  void setFromPose(const ArPose& newpose) { myFromPose = newpose; }
  void setToPose(const ArPose& newpose) { myToPose = newpose; }
  */
protected:
  std::string myType;
  std::string myName;
  ArPose myPose;
  std::string myFileName;
  std::string myIconName;
  bool myHasFromTo;
  ArPose myFromPose;
  ArPose myToPose;
  std::list<ArLineSegment> myFromToSegments;
};


#endif // ARMAP_H

---