ArServerHandlerMap.cpp

Go to the documentation of this file.

/*
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 "Aria.h"
#include "ArExport.h"
#include "ArServerHandlerMap.h"
#ifdef WIN32
#include <sys/types.h>
#include <sys/stat.h>
#endif


AREXPORT ArServerHandlerMap::ArServerHandlerMap(ArServerBase *server, 
                                                ArMap *arMap,
                                                DataToSend dataToSend) :
  myGetMapNameCB(this, &ArServerHandlerMap::serverGetMapName),
  myGetMapCB(this, &ArServerHandlerMap::serverGetMap),
  myGetMapBinaryCB(this, &ArServerHandlerMap::serverGetMapBinary),
  myGetGoalsCB(this, &ArServerHandlerMap::serverGetGoals),
  myProcessFileCB(this, &ArServerHandlerMap::processFile),
  myMapChangedCB(this, &ArServerHandlerMap::mapChanged)
{
  myServer = server;
  myOwnMap = false;
  myMap = arMap;
  setDataToSend(dataToSend);
  myMapChangedCB.setName("ArServerHandlerMap");
  myProcessFileCB.setName("ArServerHandlerMap");
  if (myMap != NULL)
  {
    strcpy(myMapFileName, myMap->getFileName());
    myAlreadyLoaded = false;
    myOwnMap = false;
    myMap->addMapChangedCB(&myMapChangedCB, ArListPos::FIRST);
  }
  else
  {
    myMapFileName[0] = '\0';
    Aria::getConfig()->addParam(ArConfigArg("Map", myMapFileName, 
                                            "map file to load", 
                                            sizeof(myMapFileName)),
              "Files", 
              ArPriority::IMPORTANT);
    Aria::getConfig()->setSectionComment("Files", 
                                         "The files where important data is stored");
    Aria::getConfig()->addProcessFileCB(&myProcessFileCB, 95);
  }


  if (myServer != NULL)
  {
    myServer->addData("getMapName", "gets the name of the map being used",
                      &myGetMapNameCB, "none", "string: map name (empty string if no maps)", "Map", "RETURN_SINGLE");

    // The "getMapBinary" request replaces the old text-based "getMap" request.
    myServer->addData("getMapBinary", "gets the map objects as ascii and the data points as binary", 
                      &myGetMapBinaryCB, 
                      "none", 
                      "packets of '<string>: line' for header, followed by packets of '<byte4>: numPtsInPacket, (<double>:x, <double>:y)*' until numPtsInPacket == 0",
                      "Map", "RETURN_UNTIL_EMPTY");
        myServer->addData("getMap", "gets the map as a set of ascii lines", 
                            &myGetMapCB, "none", 
                          "packets of '<string>: line' followed by a packet with an empty string to denote end (if only empty string then no map)",
                          "Map", "RETURN_UNTIL_EMPTY");
    myServer->addData("mapUpdated", "a single packet is sent to this when the map is updated and this denotes a new getMap and getMapName should be requested", 
                      NULL, "none", "none", 
                      "Map", "RETURN_SINGLE");
    myServer->addData("getGoals", "gets the list of goals", 
                      &myGetGoalsCB, "none", 
                      "<repeat> string: goal", "Map", "RETURN_SINGLE");
    myServer->addData("goalsUpdated", "a single packet is sent to this when the goals are updated and this denotes a new getGoals should be requested", 
                      NULL, "none", "none", 
                      "Map", "RETURN_SINGLE");

  }
}

AREXPORT ArServerHandlerMap::~ArServerHandlerMap()
{
  
}

AREXPORT bool ArServerHandlerMap::loadMap(const char *mapFile)
{
  bool ret;
  ArNetPacket emptyPacket;
  if (myMap != NULL)
  {
    if (myOwnMap)
      delete myMap;
    myMap = NULL;
  }
  myMap = new ArMap(NULL, false);
  myMapName = mapFile;
  myOwnMap = true;
  ret = myMap->readFile(mapFile);
  myServer->broadcastPacketTcp(&emptyPacket, "mapUpdated");
  myServer->broadcastPacketTcp(&emptyPacket, "goalsUpdated");
  return ret;
}

AREXPORT void ArServerHandlerMap::useMap(ArMap *mapObj, 
                                         bool takeOwnershipOfMap)
{
  ArNetPacket emptyPacket;
  if (myMap != NULL)
  {
    if (myOwnMap)
      delete myMap;
    myMap = NULL;
  }
  myMap = mapObj;
  myMapName = myMap->getFileName();
  myOwnMap = takeOwnershipOfMap;
  myServer->broadcastPacketTcp(&emptyPacket, "mapUpdated");
  myServer->broadcastPacketTcp(&emptyPacket, "goalsUpdated");
}

AREXPORT ArMap *ArServerHandlerMap::getMap(void)
{
  return myMap;
}

AREXPORT void ArServerHandlerMap::serverGetMapName(ArServerClient *client, 
                                            ArNetPacket *packet)
{
  ArNetPacket sendPacket;
  if (myMap == NULL)
  {
    sendPacket.strToBuf("");
    client->sendPacketTcp(&sendPacket);
  }
  else
  {
    sendPacket.strToBuf(myMap->getFileName());
    client->sendPacketTcp(&sendPacket);
  }
}

AREXPORT void ArServerHandlerMap::writeMapToClient(const char *line,
                                         ArServerClient *client)
{
  ArNetPacket sendPacket;
  sendPacket.strToBuf(line);
  client->sendPacketTcp(&sendPacket);
}

AREXPORT void ArServerHandlerMap::writePointsToClient(
        int pointCount,
        std::vector<ArPose> *points,
        ArServerClient *client)
{
  ArNetPacket sendPacket;
  
  // Should never happen...
  if (points == NULL) {
    // Send 0 points just so the client doesn't hang
    sendPacket.byte4ToBuf(0);
    client->sendPacketTcp(&sendPacket);
    return;
  }
  
  ArLog::log(ArLog::Verbose, 
             "ArServerHandlerMap::writePointsToClient() pointCount = %i, vector size = %i", 
             pointCount, (int) points->size());
  
  // Neither should this, but just in case...
  if (pointCount > (int) points->size()) {
    pointCount = points->size();
  }
  
  int maxInPacketCount = 1000; // Maximum number of points sent in a packet
  int currentCount = 0;         // Number put into the current packet 
  int remainingCount = pointCount; // Total number of points remaining to be sent
  bool isStartPacket = true;    // Whether a new packet is being started
  
  int totalCount = 0;
  
  for (std::vector<ArPose>::iterator pointIt = points->begin(); 
       pointIt != points->end();
       pointIt++) {
    
    // Start a new packet if the previous one was sent (or there is no 
    // previous one)
    if (isStartPacket) {
      
      isStartPacket = false;
      
      sendPacket.empty();
      currentCount = 0;
      
      // Leftover points...
      if (maxInPacketCount > remainingCount) {
        maxInPacketCount = remainingCount;
      }
      
      // The first item in the packet is the number of points contained
      sendPacket.byte4ToBuf(maxInPacketCount);
      
    } // end if starting a new packet
    
    // Add the current point to the packet (think that the test should 
    // always be true)
    if (currentCount < maxInPacketCount) {
      
      currentCount++;
      remainingCount--;
      
      sendPacket.byte4ToBuf(
              ArMath::roundInt((*pointIt).getX()));
      sendPacket.byte4ToBuf(
              ArMath::roundInt((*pointIt).getY()));
    }
    
    // If the packet is full, send it and then start a new one
    if (currentCount == maxInPacketCount) {
      
      totalCount += currentCount;
      
      client->sendPacketTcp(&sendPacket);
      //ArUtil::sleep(1);
      
      isStartPacket = true;
    }
    
  } // end for each point
  
  ArLog::log(ArLog::Verbose, 
             "ArServerHandlerMap::writePointsToClient() totalCount = %i", 
             totalCount);
  
  // Send 0 points to indicate that all have been sent
  sendPacket.empty();
  sendPacket.byte4ToBuf(0);
  client->sendPacketTcp(&sendPacket);
  
} // end writePointsToClient

AREXPORT void ArServerHandlerMap::writeLinesToClient(
        int lineCount,
        std::vector<ArLineSegment> *lines,
        ArServerClient *client)
{
  ArNetPacket sendPacket;
  
  // Should never happen...
  if (lines == NULL) {
    // Send 0 points just so the client doesn't hang
    sendPacket.byte4ToBuf(0);
    client->sendPacketTcp(&sendPacket);
    return;
  }
  
  ArLog::log(ArLog::Verbose, 
             "ArServerHandlerMap::writeLinesToClient() pointCount = %i, vector size = %i", 
             lineCount, (int) lines->size());
  
  // Neither should this, but just in case...
  if (lineCount > (int) lines->size()) {
    lineCount = lines->size();
  }
  
  int maxInPacketCount = 1000; // Maximum number of points sent in a packet
  int currentCount = 0;         // Number put into the current packet 
  int remainingCount = lineCount; // Total number of points remaining to be sent
  bool isStartPacket = true;    // Whether a new packet is being started
  
  int totalCount = 0;
  
  for (std::vector<ArLineSegment>::iterator lineIt = lines->begin(); 
       lineIt != lines->end();
       lineIt++) 
  {
    
    // Start a new packet if the previous one was sent (or there is no 
    // previous one)
    if (isStartPacket) {
      
      isStartPacket = false;
      
      sendPacket.empty();
      currentCount = 0;
      
      // Leftover points...
      if (maxInPacketCount > remainingCount) {
        maxInPacketCount = remainingCount;
      }
      
      // The first item in the packet is the number of points contained
      sendPacket.byte4ToBuf(maxInPacketCount);
      
    } // end if starting a new packet
    
    // Add the current point to the packet (think that the test should 
    // always be true)
    if (currentCount < maxInPacketCount) {
      
      currentCount++;
      remainingCount--;
      
      sendPacket.byte4ToBuf(
              ArMath::roundInt((*lineIt).getX1()));
      sendPacket.byte4ToBuf(
              ArMath::roundInt((*lineIt).getY1()));
      sendPacket.byte4ToBuf(
              ArMath::roundInt((*lineIt).getX2()));
      sendPacket.byte4ToBuf(
              ArMath::roundInt((*lineIt).getY2()));
    }
    
    // If the packet is full, send it and then start a new one
    if (currentCount == maxInPacketCount) {
      
      totalCount += currentCount;
      
      client->sendPacketTcp(&sendPacket);
      //ArUtil::sleep(1);
      
      isStartPacket = true;
    }
    
  } // end for each point
  
  ArLog::log(ArLog::Verbose, 
             "ArServerHandlerMap::writePointsToClient() totalCount = %i", 
             totalCount);
  
  // Send 0 points to indicate that all have been sent
  sendPacket.empty();
  sendPacket.byte4ToBuf(0);
  client->sendPacketTcp(&sendPacket);
  
} // end writePointsToClient


AREXPORT void ArServerHandlerMap::serverGetMap(ArServerClient *client, 
                                        ArNetPacket *packet)
{
  ArLog::log(ArLog::Verbose, "Starting sending map to client");
  if (myMap == NULL)
  {
    writeMapToClient("", client);
    return;
  }

  myMap->lock();
  // This functor is used to send the entire map in text format.
  ArFunctor1<const char *> *functor = 
                new ArFunctor2C<ArServerHandlerMap, const char *, ArServerClient *>
                                        (this, 
                                         &ArServerHandlerMap::writeMapToClient,
                                         NULL,
                                         client);
  // Send the map 
  myMap->writeToFunctor(functor, "");

  delete functor;
  // Send an empty packet to indicate the end of the map.
  writeMapToClient("", client);

  // send an empty packet to say we're done
  ArNetPacket emptyPacket;
  client->sendPacketTcp(&emptyPacket);

  myMap->unlock();
  ArLog::log(ArLog::Verbose, "Finished sending map to client");

}

AREXPORT void ArServerHandlerMap::serverGetMapBinary(ArServerClient *client, 
                                                                                                         ArNetPacket *packet)
{
  ArLog::log(ArLog::Verbose, "Starting sending map (binary) to client");
  if (myMap == NULL)
  {
    writeMapToClient("", client);
    return;
  }

  myMap->lock();
  // This functor is used to send the map header and objects in text format.
  ArFunctor1<const char *> *textFunctor = 
                new ArFunctor2C<ArServerHandlerMap, const char *, ArServerClient *>
                                        (this, 
                                         &ArServerHandlerMap::writeMapToClient,
                                         NULL,
                                         client);

  // This functor is used to send the map data points in binary format (since
  // it is faster).
  ArFunctor2<int, std::vector<ArLineSegment> *> *linesFunctor =
          new ArFunctor3C<ArServerHandlerMap, int, std::vector<ArLineSegment> *, ArServerClient *>
                                   (this,
                                    &ArServerHandlerMap::writeLinesToClient,
                                    0,
                                    NULL,
                                    client);

  // This functor is used to send the map data points in binary format (since
  // it is faster).
  ArFunctor2<int, std::vector<ArPose> *> *pointsFunctor =
          new ArFunctor3C<ArServerHandlerMap, int, std::vector<ArPose> *, ArServerClient *>
                                   (this,
                                    &ArServerHandlerMap::writePointsToClient,
                                    0,
                                    NULL,
                                    client);

  // Send the map up to and including the DATA tag.
  myMap->writeObjectsToFunctor(textFunctor, "");

  // see if we want to send the lines and make sure we have lines
  if ((myDataToSend == LINES || myDataToSend == BOTH) && 
      myMap->getLines()->begin() != myMap->getLines()->end())
  {
    writeMapToClient("LINES", client);
    myMap->writeLinesToFunctor(linesFunctor);
  }

  // Always write the points (even if empty) because this signifies
  // the end of the map for the client.
  //if (myMap->getPoints()->begin() != myMap->getPoints()->end())
  //{
  writeMapToClient("DATA", client);
  // if we want to send points then send 'em
  if (myDataToSend == POINTS || myDataToSend == BOTH)
    myMap->writePointsToFunctor(pointsFunctor);
  // if not just say we're done
  else
    writeMapToClient("", client);
  //}

  // send an empty packet to say we're done
  ArNetPacket emptyPacket;
  client->sendPacketTcp(&emptyPacket);

  myMap->unlock();
  ArLog::log(ArLog::Verbose, "Finished sending map (binary) to client");

}


AREXPORT void ArServerHandlerMap::serverGetGoals(ArServerClient *client, 
                                                 ArNetPacket *packet)
{
  std::list<ArMapObject *>::iterator objIt;
  ArMapObject* obj;
  ArPose goal;  
  ArNetPacket sendPacket;

  for (objIt = myMap->getMapObjects()->begin(); 
       objIt != myMap->getMapObjects()->end(); 
       objIt++)
  {
    //
    // Get the forbidden lines and fill the occupancy grid there.
    //
    obj = (*objIt);
    if (obj == NULL)
      break;
    if (strcasecmp(obj->getType(), "GoalWithHeading") == 0 ||
        strcasecmp(obj->getType(), "Goal") == 0)
    {
      sendPacket.strToBuf(obj->getName());
    }
  }
  client->sendPacketTcp(&sendPacket);
}

AREXPORT void ArServerHandlerMap::mapChanged(void)
{
  ArNetPacket emptyPacket;
  strcpy(myMapFileName, myMap->getFileName());
  myServer->broadcastPacketTcp(&emptyPacket, "mapUpdated");
  myServer->broadcastPacketTcp(&emptyPacket, "goalsUpdated");
}

AREXPORT bool ArServerHandlerMap::processFile(void)
{
  struct stat mapFileStat;

  if (myMapFileName[0] == '\0')
  {
    ArLog::log(ArLog::Verbose, "ArServerHandlerMap::processFile: No map file");
    return true;
  }

  if (stat(myMapFileName, &mapFileStat) != 0)
  {
    ArLog::log(ArLog::Terse, "Cannot stat file");
    return false;
  }
 // see if we need to load the map file
  if (!myAlreadyLoaded || strcmp(myMapFileName, myLastMapFile) != 0 ||
      mapFileStat.st_mtime != myLastMapFileStat.st_mtime || 
      mapFileStat.st_ctime != myLastMapFileStat.st_ctime)
  {
    myAlreadyLoaded = true; 
    strcpy(myLastMapFile, myMapFileName);
    memcpy(&myLastMapFileStat, &mapFileStat, sizeof(mapFileStat));

    ArLog::log(ArLog::Terse, "Loading map %s", myMapFileName);
    if (!loadMap(myMapFileName))
    {
      ArLog::log(ArLog::Terse, "Failed loading map %s", myMapFileName);
      return false;
    }
    ArLog::log(ArLog::Terse, "Loaded map %s", myMapFileName);
    return true;
  }
  myAlreadyLoaded = true;
  ArLog::log(ArLog::Normal, "ArServerHandlerMapConfig: File did not need reloading");
  return true;
}

---