ArServerHandlerMapping.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 "ArServerHandlerMapping.h"

AREXPORT ArServerHandlerMapping::ArServerHandlerMapping(
        ArServerBase *server, ArRobot *robot, ArSick *sick,
        const char *baseDirectory, const char *tempDirectory,
        bool useReflectorValues) :
  myMappingStartCB(this, &ArServerHandlerMapping::serverMappingStart),
  myMappingEndCB(this, &ArServerHandlerMapping::serverMappingEnd),
  myMappingStatusCB(this, &ArServerHandlerMapping::serverMappingStatus),
  myPacketHandlerCB(this, &ArServerHandlerMapping::packetHandler),
  myLoopStartCB(this, &ArServerHandlerMapping::simpleLoopStart),
  myLoopEndCB(this, &ArServerHandlerMapping::simpleLoopEnd)
{
  myServer = server;
  myRobot = robot;
  mySick = sick;
  myUseReflectorValues = useReflectorValues;

  if (baseDirectory != NULL && strlen(baseDirectory) > 0)
    myBaseDirectory = baseDirectory;
  else
    myBaseDirectory = "";
  if (tempDirectory != NULL && strlen(tempDirectory) > 0)
    myTempDirectory = tempDirectory;
  else
    myTempDirectory = myBaseDirectory;

  if (myServer != NULL)
  {
    myServer->addData("mappingStart", "Starts making a map",
                      &myMappingStartCB, "string: name of map file",
                      "byte: 0 (started mapping), 1 (already mapping), 2 (could not start map)",
                      "Mapping", "RETURN_SINGLE");
    myServer->addData("mappingEnd", "Stops making a map",
                      &myMappingEndCB, "none",
                      "byte: 0 (ended mapping), 1 (no mapping to end), 2 (could not move temporary file to permanent file)", 
                      "Mapping", "RETURN_SINGLE");
    myServer->addData("mappingStatus", "Gets the status of mapping",
                      &myMappingStatusCB, "none",
              "string: mapName if mapping, empty string if not mapping", 
                      "RobotInfo", "RETURN_SINGLE");
    myServer->addData("mappingStatusBroadcast", "Gets the status of mapping, also sent when mapping starts or ends (this is a new/better mappingStatus)",
                      &myMappingStatusCB, "none",
              "string: mapName if mapping, empty string if not mapping", 
                      "RobotInfo", "RETURN_SINGLE");
  }
  mySickLogger = NULL;
  myMapName = "";
  myFileName = "";
  myHandlerCommands = NULL;
  
  myPacketHandlerCB.setName("ArServerHandlerMapping");
  myRobot->addPacketHandler(&myPacketHandlerCB);
}

AREXPORT ArServerHandlerMapping::~ArServerHandlerMapping()
{
  delete mySickLogger;
  mySickLogger = NULL;
}

AREXPORT void ArServerHandlerMapping::serverMappingStart(
        ArServerClient *client, ArNetPacket *packet)
{
  ArNetPacket sendPacket;
  char buf[512];
  packet->bufToStr(buf, sizeof(buf));

  // see if we're already mapping
  if (mySickLogger != NULL)
  {
    ArLog::log(ArLog::Normal, "MappingStart: Map already being made");
    sendPacket.byteToBuf(1);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    return;
  }

  // call our mapping started callbacks
  std::list<ArFunctor *>::iterator fit;
  for (fit = myMappingStartCallbacks.begin(); 
       fit != myMappingStartCallbacks.end(); 
       fit++)
    (*fit)->invoke();

  myRobot->lock();
  // lower case everything (to avoid case conflicts)
  ArUtil::lower(buf, buf, sizeof(buf));
  myMapName = buf;

  myFileName = myMapName;
  if (strstr(myMapName.c_str(), ".2d") == NULL)
    myFileName += ".2d";

  mySickLogger = new ArSickLogger(myRobot, mySick, 300, 25, myFileName.c_str(),
                                  true, Aria::getJoyHandler(), 
                                  myTempDirectory.c_str(), 
                                  myUseReflectorValues,
                                  Aria::getRobotJoyHandler());
  if (mySickLogger == NULL)
  {
    ArLog::log(ArLog::Normal, "MappingStart: mySickLogger == NULL");
    sendPacket.byteToBuf(2);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    myMapName = "";
    myFileName = "";
    myRobot->unlock();
    return;
  }
  if (!mySickLogger->wasFileOpenedSuccessfully())
  {
    ArLog::log(ArLog::Normal, "MappingStart: Cannot open map file %s", 
               myFileName.c_str());
    sendPacket.byteToBuf(2);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    myMapName = "";
    myFileName = "";
    delete mySickLogger;
    mySickLogger = NULL;
    myRobot->unlock();
    return;
  }

  // toss our strings for the start on there
  std::list<std::string>::iterator it;
  
  for (it = myStringsForStartOfLog.begin(); 
       it != myStringsForStartOfLog.end(); 
       it++)
    mySickLogger->addInfoToLogPlain((*it).c_str());
  myRobot->unlock();
  
  
  ArLog::log(ArLog::Normal, "MappingStart: Map %s started", 
             myFileName.c_str());
  sendPacket.byteToBuf(0);
  if (client != NULL)
    client->sendPacketTcp(&sendPacket);
  
  ArNetPacket broadcastPacket;
  broadcastPacket.strToBuf(myFileName.c_str());
  myServer->broadcastPacketTcp(&broadcastPacket, "mappingStatusBroadcast");
}


AREXPORT void ArServerHandlerMapping::serverMappingEnd(
        ArServerClient *client, ArNetPacket *packet)
{
  std::list<ArFunctor *>::iterator fit;

  ArNetPacket sendPacket;
  if (mySickLogger == NULL)
  {
    ArLog::log(ArLog::Normal, "MappingEnd: No map being made");
    sendPacket.byteToBuf(1);
    if (client != NULL)
      client->sendPacketTcp(&sendPacket);
    return;
  }

  myRobot->lock();
  delete mySickLogger;
  mySickLogger = NULL;

  // now, if our temp directory and base directory are different we
  // need to move it and put the result in the packet, otherwise put
  // in we're okay
  if (ArUtil::strcasecmp(myBaseDirectory, myTempDirectory) != 0)
  {
#ifndef WIN32
    char *mvName = "mv";
#else
    char *mvName = "move";
#endif
    char systemBuf[6400];
    char fromBuf[1024];
    char toBuf[1024];

    if (myTempDirectory.size() > 0)
      snprintf(fromBuf, sizeof(fromBuf), "%s%s", 
               myTempDirectory.c_str(), myFileName.c_str());
    else
      snprintf(fromBuf, sizeof(fromBuf), "%s", myFileName.c_str());
    ArUtil::fixSlashes(fromBuf, sizeof(fromBuf));

    if (myTempDirectory.size() > 0)
      snprintf(toBuf, sizeof(toBuf), "%s%s", 
               myBaseDirectory.c_str(), myFileName.c_str());
    else
      snprintf(toBuf, sizeof(toBuf), "%s", myFileName.c_str());
    ArUtil::fixSlashes(toBuf, sizeof(toBuf));

    sprintf(systemBuf, "%s \"%s\" \"%s\"", mvName, fromBuf, toBuf);
    ArLog::log(ArLog::Verbose, "Moving with '%s'", systemBuf);
    // call our pre move callbacks
    for (fit = myPreMoveCallbacks.begin(); 
         fit != myPreMoveCallbacks.end(); 
         fit++)
      (*fit)->invoke();

    // move file
    int ret;
    if ((ret = system(systemBuf)) == 0)
    {
      ArLog::log(ArLog::Verbose, "ArServerHandlerMapping: Moved file %s (with %s)", 
                 myFileName.c_str(), systemBuf);
      sendPacket.byteToBuf(0);
    }
    else
    {
      ArLog::log(ArLog::Normal, 
                 "ArServerHandlerMapping: Couldn't move file for %s (ret of '%s' is %d) removing file", 
                 myFileName.c_str(), systemBuf, ret);
      unlink(fromBuf);
      sendPacket.uByte2ToBuf(2);
    }

    // call our post move callbacks
    for (fit = myPostMoveCallbacks.begin(); 
         fit != myPostMoveCallbacks.end(); 
         fit++)
      (*fit)->invoke();

  }
  else
  {
    // just put in things are okay, it'll get sent below
    sendPacket.byteToBuf(0);
  }


  ArLog::log(ArLog::Normal, "MappingEnd: Stopped mapping %s", 
             myFileName.c_str());
  myMapName = "";
  myFileName = "";

  myRobot->unlock();
  if (client != NULL)
    client->sendPacketTcp(&sendPacket);

  // call our mapping started callbacks
  for (fit = myMappingEndCallbacks.begin(); 
       fit != myMappingEndCallbacks.end(); 
       fit++)
    (*fit)->invoke();

  ArNetPacket broadcastPacket;
  broadcastPacket.strToBuf(myFileName.c_str());
  myServer->broadcastPacketTcp(&broadcastPacket, "mappingStatusBroadcast");
}


AREXPORT void ArServerHandlerMapping::serverMappingStatus(
        ArServerClient *client, ArNetPacket *packet)
{
  ArNetPacket sendPacket;
  sendPacket.strToBuf(myMapName.c_str());
  client->sendPacketTcp(&sendPacket);
}

AREXPORT void ArServerHandlerMapping::addStringForStartOfLogs(
        const char *str, ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myStringsForStartOfLog.push_front(str);
  else if (position == ArListPos::LAST)
    myStringsForStartOfLog.push_back(str);
  else 
    ArLog::log(ArLog::Terse, "ArServerHandlerMapping::addStringForStartOfLogs: Invalid position.");
}

AREXPORT void ArServerHandlerMapping::remStringForStartOfLogs(
        char *str)
{
  myStringsForStartOfLog.remove(str);
}

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

AREXPORT void ArServerHandlerMapping::remMappingStartCallback(
        ArFunctor *functor)
{
  myMappingStartCallbacks.remove(functor);
}

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

AREXPORT void ArServerHandlerMapping::remMappingEndCallback(
        ArFunctor *functor)
{
  myMappingEndCallbacks.remove(functor);
}

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

AREXPORT void ArServerHandlerMapping::remPreMoveCallback(
        ArFunctor *functor)
{
  myPreMoveCallbacks.remove(functor);
}

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

AREXPORT void ArServerHandlerMapping::remPostMoveCallback(
        ArFunctor *functor)
{
  myPostMoveCallbacks.remove(functor);
}

AREXPORT void ArServerHandlerMapping::addSimpleCommands(
        ArServerHandlerCommands *handlerCommands)
{
  myHandlerCommands = handlerCommands;
  myHandlerCommands->addStringCommand(
          "mappingLoopStart", 
          "If mapping is happening it starts a new loop with the tag of the given string", 
          &myLoopStartCB);

  myHandlerCommands->addStringCommand(
          "mappingLoopEnd", 
          "If mapping is happening it ends a loop with the tag of the given string", 
          &myLoopEndCB);
}

AREXPORT void ArServerHandlerMapping::simpleLoopStart(ArArgumentBuilder *arg)
{
  if (mySickLogger != NULL) 
    mySickLogger->addTagToLog("loop start %s", arg->getFullString());
}

AREXPORT void ArServerHandlerMapping::simpleLoopEnd(ArArgumentBuilder *arg)
{
  if (mySickLogger != NULL) 
    mySickLogger->addTagToLog("loop end %s", arg->getFullString());
}

AREXPORT bool ArServerHandlerMapping::packetHandler(ArRobotPacket *packet)
{
  if (packet->getID() != 0x97)
    return false;

  myRobot->unlock();
  char argument = packet->bufToByte();
  if (argument == 1)
  {
    // see if we're already mapping
    if (mySickLogger != NULL)
    {
      ArLog::log(ArLog::Normal, 
                 "ArServerHandlerMapping::packetHandler: Start scan requested when already mapping");
    }
    else
    {
      // build a name
      time_t now;
      struct tm nowStruct;
      now = time(NULL);
      char buf[1024];

          if (ArUtil::localtime(&now, &nowStruct))
      {
        sprintf(buf, "%02d%02d%02d_%02d%02d%02d", 
                (nowStruct.tm_year%100), nowStruct.tm_mon+1, nowStruct.tm_mday, 
                nowStruct.tm_hour, nowStruct.tm_min, nowStruct.tm_sec);
      }
      else
      {
        ArLog::log(ArLog::Normal, 
                   "ArServerHandlerMapping::packetHandler: Could not make good packet name (error getting time), so just naming it \"lcd\"");
        sprintf(buf, "lcd");
      }

      ArLog::log(ArLog::Normal, "Starting scan '%s' from LCD", buf);
      ArNetPacket fakePacket;
      fakePacket.strToBuf(buf);
      fakePacket.finalizePacket();
      serverMappingStart(NULL, &fakePacket);
    }
  }
  else if (argument == 0)
  {
    if (mySickLogger == NULL)
    {
      ArLog::log(ArLog::Normal, 
                 "ArServerHandlerMapping::packetHandler: Stop scan requested when not mapping");
    }
    else
    {
      ArLog::log(ArLog::Normal, "Stopping scan from LCD");
      serverMappingEnd(NULL, NULL);
    }
  }
  else
  {
    ArLog::log(ArLog::Normal, "ArServerHandlerMapping::packetHandler: Unknown scan argument %d", argument);
  }
  myRobot->lock();
  return true;
}

---