Aria.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 "Aria.h"
#include "ArSocket.h"
#include "ArSignalHandler.h"
#include "ArKeyHandler.h"
#include "ArJoyHandler.h"
#ifndef ARINTERFACE
#include "ArModuleLoader.h"
#include "ArRobotJoyHandler.h"
#endif // ARINTERFACE

ArGlobalFunctor1<int> Aria::ourSignalHandlerCB(&Aria::signalHandlerCB);
ArMutex Aria::ourShuttingDownMutex;
bool Aria::ourShuttingDown=false;
bool Aria::ourExiting=false;
std::string Aria::ourDirectory="";
std::list<ArFunctor*> Aria::ourInitCBs;
std::list<ArFunctor*> Aria::ourUninitCBs;
ArKeyHandler *Aria::ourKeyHandler = NULL;
ArJoyHandler *Aria::ourJoyHandler = NULL;
bool Aria::ourInited = false;
bool Aria::ourRunning = false;
ArMutex Aria::ourExitCallbacksMutex;
std::multimap<int, ArFunctor *> Aria::ourExitCallbacks;
bool Aria::ourSigHandleExitNotShutdown = true;
std::multimap<int, ArRetFunctor<bool> *> Aria::ourParseArgCBs;
ArLog::LogLevel Aria::ourParseArgsLogLevel = ArLog::Verbose;
std::multimap<int, ArFunctor *> Aria::ourLogOptionsCBs;

#ifndef ARINTERFACE
std::list<ArRobot*> Aria::ourRobots;
ArConfig Aria::ourConfig;
ArRobotJoyHandler *Aria::ourRobotJoyHandler = NULL;
ArStringInfoGroup Aria::ourInfoGroup;
#endif // ARINTERFACE

AREXPORT void Aria::init(SigHandleMethod method, bool initSockets, 
             bool sigHandleExitNotShutdown)
{
  std::list<ArFunctor*>::iterator iter;
  std::string str;
  char buf[1024];

  if (ourInited == true)
    return;

  ourRunning = true;
#ifndef WIN32
  srand48(time(NULL));
#endif

  ArThread::init();

  char* overrideSigMethod = getenv("ARIA_SIGHANDLE_METHOD");
  if(overrideSigMethod)
  {
    ArLog::log(ArLog::Terse, "Overriding signal handler method with %s from ARIA_SIGHANDLE_METHOD environment variable.", overrideSigMethod);
    if(!strcmp(overrideSigMethod, "NONE"))
      method = SIGHANDLE_NONE;
    else if(!strcmp(overrideSigMethod, "SINGLE"))
      method = SIGHANDLE_SINGLE;
    else if(!strcmp(overrideSigMethod, "THREAD"))
      method = SIGHANDLE_THREAD;
  }

  if (method != SIGHANDLE_NONE)
  {
    ArSignalHandler::addHandlerCB(&ourSignalHandlerCB, ArListPos::LAST);
    ArSignalHandler::blockCommon();
    ArSignalHandler::handle(ArSignalHandler::SigHUP);
    ArSignalHandler::handle(ArSignalHandler::SigINT);
    ArSignalHandler::handle(ArSignalHandler::SigQUIT);
    ArSignalHandler::handle(ArSignalHandler::SigTERM);
    ArSignalHandler::handle(ArSignalHandler::SigPIPE);
    if (method == SIGHANDLE_SINGLE)
        ArSignalHandler::createHandlerNonThreaded();
    else if (method == SIGHANDLE_THREAD)
    {
      ArSignalHandler::blockCommonThisThread();
      ArSignalHandler::createHandlerThreaded();
    }
  }

  if (initSockets)
    ArSocket::init();

  if (ourDirectory.length() == 0)
  {
    if (getenv("ARIA") != NULL)
    {
      setDirectory(getenv("ARIA"));
    }
    else
    {
#ifndef WIN32
      ArUtil::getStringFromFile("/etc/Aria", buf, sizeof(buf));
      str = buf;
#else // WIN32
      if (ArUtil::findFirstStringInRegistry(
          "SOFTWARE\\MobileRobots\\Aria",
          "Install Directory", buf, 1024))
        str = buf;
      else
        if (ArUtil::findFirstStringInRegistry(
            "SOFTWARE\\ActivMedia Robotics\\Aria",
            "Install Directory", buf, 1024))
          str = buf;
        else
          str = "";
        
  #endif // WIN32
        if (str.length() > 0)
        {
    setDirectory(str.c_str());
        }
        else
        {
  #ifndef ARINTERFACE
    ArLog::log(ArLog::Terse, "NonCritical Error: ARIA could not find where it is located.");
  #else
    ArLog::log(ArLog::Verbose, "NonCritical Error: ARIA could not find where it is located.");
  #endif
        }
      }
    }
    ourSigHandleExitNotShutdown = sigHandleExitNotShutdown;

  ourInited = true;
  for (iter=ourInitCBs.begin(); iter !=  ourInitCBs.end(); ++iter)
    (*iter)->invoke();
  ArArgumentParser::addDefaultArgumentFile("/etc/Aria.args");
  ArArgumentParser::addDefaultArgumentEnv("ARIAARGS");
  
}

AREXPORT void Aria::uninit()
{
  std::list<ArFunctor*>::iterator iter;

  for (iter=ourUninitCBs.begin(); iter != ourUninitCBs.end(); ++iter)
    (*iter)->invoke();

#ifndef ARINTERFACE
  ArModuleLoader::closeAll();
#endif // ARINTERFACE
  ArSocket::shutdown();
}

AREXPORT void Aria::addInitCallBack(ArFunctor *cb, ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    ourInitCBs.push_front(cb);
  else
    ourInitCBs.push_back(cb);
}

AREXPORT void Aria::addUninitCallBack(ArFunctor *cb, ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    ourUninitCBs.push_front(cb);
  else
    ourUninitCBs.push_back(cb);
}

AREXPORT void Aria::shutdown()
{
  bool doExit=false;

  ourShuttingDownMutex.lock();
  ourRunning = false;
  if (ourShuttingDown)
    doExit=true;
  else
    ourShuttingDown=true;
  ourShuttingDownMutex.unlock();

  if (doExit)
    return;

  ArThread::stopAll();
  ArThread::joinAll();
  uninit();
}

AREXPORT void Aria::exit(int exitCode)
{
  bool doReturn = false;
  ourShuttingDownMutex.lock();
  ourRunning = false;
  if (ourExiting)
    doReturn=true;
  else
    ourExiting=true;
  ourShuttingDownMutex.unlock();

  if (doReturn)
    return;

  callExitCallbacks();
  ::exit(exitCode);
}

AREXPORT void Aria::callExitCallbacks(void)
{
  ourRunning = false;
  std::multimap<int, ArFunctor *>::reverse_iterator it;
 
  ourExitCallbacksMutex.lock();
  ArLog::log(ArLog::Verbose, "Aria::exit: Starting exit callbacks");
  for (it = ourExitCallbacks.rbegin(); it != ourExitCallbacks.rend(); it++)
  {
    ArLog::log(ArLog::Verbose, 
           "Aria::exit: Calling callback at position %d with name '%s'",
           (*it).first, (*it).second->getName());
    (*it).second->invoke();
  }
  ArLog::log(ArLog::Verbose, "Aria::exit: Finished exit callbacks");
  ourExitCallbacksMutex.unlock();
} 
 
AREXPORT void Aria::addExitCallback(ArFunctor *functor, int position)
{
  ourExitCallbacksMutex.lock();
  ArLog::log(ArLog::Verbose, 
    "Aria::addExitCallback: Adding callback at position %d with name '%s'",
         position, functor->getName());
  ourExitCallbacks.insert(std::pair<int, ArFunctor *>(position, functor));
  ourExitCallbacksMutex.unlock();
}

/*
AREXPORT void Aria::remExitCallback(ArFunctor *functor)
{
  std::multimap<int, ArFunctor *>::iterator it;
  
  ourExitCallbacksMutex.lock();
  for (it = ourExitCallbacks.begin(); it != ourExitCallbacks.end(); it++)
  {
    if ((*it).second == functor)
    {
      ourExitCallbacks.erase(it);
      ourExitCallbacksMutex.unlock();
      return remExitCallback(functor);
    }
  }
  ourExitCallbacksMutex.unlock();
}
*/

AREXPORT void Aria::exitOld(int exitCode)
{
  ourRunning = false;
  ArThread::cancelAll();
  uninit();
  ::exit(exitCode);
}

#ifndef ARINTERFACE
AREXPORT void Aria::addRobot(ArRobot *robot)
{
  ourRobots.push_back(robot);
}

AREXPORT void Aria::delRobot(ArRobot *robot)
{
  ourRobots.remove(robot);
}

AREXPORT ArRobot *Aria::findRobot(char *name)
{
  std::string rname;
  std::list<ArRobot *>::iterator it;
  if (name == NULL)
    return NULL;

  rname = name;
  for (it = ourRobots.begin(); it != ourRobots.end(); it++)
  {
    if ((*it)->getName() == rname)
      return (*it);
  }
  return NULL;

}

AREXPORT std::list<ArRobot*> * Aria::getRobotList()
{
  return(&ourRobots);
}

#endif // ARINTERFACE

AREXPORT void Aria::signalHandlerCB(int sig)
{

  // if we want to exit instead of shutdown then do that ( call never returns)
  if (ourSigHandleExitNotShutdown)
  {
    ArLog::log(ArLog::Normal, "Aria: Received signal '%s'. Exiting.",
           ArSignalHandler::nameSignal(sig));
    Aria::exit(0);
    // we shouldn't need this here, since the program should already
    // exited... but just in case
    ::exit(0);
  }


  ourShuttingDownMutex.lock();
  if (!ourRunning)
  {
    ourShuttingDownMutex.unlock();
    return;
  }
  ourShuttingDownMutex.unlock();


  ArLog::log(ArLog::Normal, "Aria: Received signal '%s'. Shutting down.",
         ArSignalHandler::nameSignal(sig));

#ifndef ARINTERFACE
  std::list<ArRobot*>::iterator iter;
  if ((sig == ArSignalHandler::SigINT) || (sig == ArSignalHandler::SigHUP) ||
      (sig == ArSignalHandler::SigTERM))
  {
    for (iter=ourRobots.begin(); iter != ourRobots.end(); ++iter)
      (*iter)->stopRunning();
  }
#endif //ARINTERFACE

  // I'm disregarding this advice below since I can't seem to get
  // anything else to work well and haven't seen problems from it

  // dont do an Aria::shutdown() here because we want the main()
  // function to do the ArThread::joinAll(). Otherwise variables on
  // the stack frame of main() may get destructed if main() happens to
  // exit before other threads.  And some of those variables may be
  // used by those threads.
  shutdown();
}

AREXPORT void Aria::setDirectory(const char *directory)
{
  int ind;
  if (directory != NULL)
  {
    ourDirectory = directory;
    ind = strlen(directory) - 1;
    if (ind < 0)
      ind = 0;
    if (directory[ind] != '/' && directory[ind] != '\\')
    {
#ifdef WIN32
      ourDirectory += "\\";
#else // win32
      ourDirectory += "/";
#endif // win32
    }
#ifndef ARINTERFACE
    ourConfig.setBaseDirectory(ourDirectory.c_str());
#endif // ARINTERFACE
  }
}

AREXPORT const char *Aria::getDirectory(void)
{
  return ourDirectory.c_str();
}

AREXPORT void Aria::setKeyHandler(ArKeyHandler *keyHandler)
{
  ourKeyHandler = keyHandler;
}

AREXPORT ArKeyHandler *Aria::getKeyHandler(void)
{
  return ourKeyHandler;
}

AREXPORT void Aria::setJoyHandler(ArJoyHandler *joyHandler)
{
  ourJoyHandler = joyHandler;
}

AREXPORT ArJoyHandler *Aria::getJoyHandler(void)
{
  return ourJoyHandler;
}

#ifndef ARINTERFACE

AREXPORT void Aria::setRobotJoyHandler(ArRobotJoyHandler *robotJoyHandler)
{
  ourRobotJoyHandler = robotJoyHandler;
}

AREXPORT ArRobotJoyHandler *Aria::getRobotJoyHandler(void)
{
  return ourRobotJoyHandler;
}

AREXPORT ArConfig *Aria::getConfig(void)
{
  return &ourConfig;
}

AREXPORT ArStringInfoGroup *Aria::getInfoGroup(void)
{
  return &ourInfoGroup;
}

#endif // ARINTERFACE

AREXPORT bool Aria::getRunning(void)
{
  return ourRunning;
}

AREXPORT bool Aria::parseArgs(void)
{
  std::multimap<int, ArRetFunctor<bool> *>::reverse_iterator it;
  ArRetFunctor<bool> *callback;

  ArLog::log(ourParseArgsLogLevel, "Aria: Parsing arguments");
  for (it = ourParseArgCBs.rbegin(); it != ourParseArgCBs.rend(); it++)
  {
    callback = (*it).second;
    if (callback->getName() != NULL && callback->getName()[0] != '\0')
      ArLog::log(ourParseArgsLogLevel, 
         "Aria: Calling parse arg functor '%s' (%d)", 
         callback->getName(), (*it).first);
    else
      ArLog::log(ourParseArgsLogLevel, 
         "Aria: Calling unnamed parse arg functor (%d)", 
         (*it).first);

    if (!callback->invokeR())
    {
      return false;
    }
  }
  return true;
}

AREXPORT void Aria::logOptions(void)
{
  std::multimap<int, ArFunctor *>::reverse_iterator it;

  for (it = ourLogOptionsCBs.rbegin(); it != ourLogOptionsCBs.rend(); it++)
  {
    (*it).second->invoke();
    ArLog::log(ArLog::Terse, "");
    ArLog::log(ArLog::Terse, "");
  }
}

AREXPORT void Aria::addParseArgsCB(ArRetFunctor<bool> *functor, 
                      int position)
{
  ourParseArgCBs.insert(std::pair<int, ArRetFunctor<bool> *>(position, 
                                 functor));
}

AREXPORT void Aria::setParseArgLogLevel(ArLog::LogLevel level)
{
  ourParseArgsLogLevel = level;
}

AREXPORT void Aria::addLogOptionsCB(ArFunctor *functor, int position)
{
  ourLogOptionsCBs.insert(std::pair<int, ArFunctor *>(position, functor));
}

---