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

AREXPORT bool ArServerMode::ourActiveModeLocked = false;
AREXPORT bool ArServerMode::ourActiveModeWillUnlockIfRequested = false;
AREXPORT ArServerMode *ArServerMode::ourActiveMode = NULL;
AREXPORT ArServerMode *ArServerMode::ourNextActiveMode = NULL;
AREXPORT std::list<ArServerMode *> ArServerMode::ourDefaultModes;
AREXPORT std::list<ArServerMode *> ArServerMode::ourRequestedActivateModes;
AREXPORT std::list<ArServerMode *> ArServerMode::ourModes;
bool ArServerMode::ourUserTaskAdded = false;
ArGlobalFunctor ArServerMode::ourUserTaskCB(ArServerMode::modeUserTask);
AREXPORT std::multimap<std::string, std::string> ArServerMode::ourModeDataMap;
AREXPORT ArMutex ArServerMode::ourModeDataMapMutex;
AREXPORT ArGlobalFunctor2<ArServerClient *, 
                          ArNetPacket *> ArServerMode::ourGetModeDataListCB(
                                  ArServerMode::getModeDataList);
AREXPORT ArGlobalFunctor2<ArServerClient *, 
                          ArNetPacket *> ArServerMode::ourGetModeInfoCB(
                                  ArServerMode::getModeInfo);
AREXPORT bool ArServerMode::ourLastActiveModeLocked = false;
AREXPORT ArServerMode *ArServerMode::ourLastActiveMode = NULL; 
AREXPORT bool ArServerMode::ourLastActiveModeWillUnlockIfRequested = false;


AREXPORT void ArServerMode::modeUserTask(void)
{
  if (ourActiveMode != NULL)
  {
    ourActiveMode->userTask();
  }
  else
  {
    std::list<ArServerMode *>::iterator it;
    for (it = ourDefaultModes.begin(); 
         it != ourDefaultModes.end() && ourActiveMode == NULL;
         it++)
    {
      ArLog::log(ArLog::Terse, "Checking default on %s mode", (*it)->getName());
      (*it)->checkDefault();
      if (ourActiveMode != NULL)
      {
        ArLog::log(ArLog::Normal, "Activated %s mode as default", (*it)->getName());
        return;
      }
    }
  }
}

AREXPORT ArServerMode::ArServerMode(ArRobot *robot, ArServerBase *server, 
                                    const char *name)
{
  myVerboseLogLevel = ArLog::Verbose;
  myRobot = robot;
  myServer = server;
  myName = name;
  myIsActive = false;
  myStatusSetThisCycle = false;
  ourModes.push_front(this);
  myHasSetActivityTime = false;
  if (!ourUserTaskAdded)
  {
    ourUserTaskAdded = true;
    myRobot->addUserTask("server mode", 50, &ourUserTaskCB);
    myServer->addData("getModeDataList", "Gets the list of commands associated with modes.", 
                      &ourGetModeDataListCB,
                      "none", 
                      "ubyte4: numData;  repeating numData times, string: mode; string: data", 
                      "RobotInfo", "RETURN_SINGLE");
    myServer->addData("getModeInfo", "Gets the current mode active and if its locked and will or won't unlock (for lists of which commands are with which mdoes use getModeDataList) commands associated with modes.", 
                      &ourGetModeInfoCB,
                      "none", 
                      "string: mode; uByte: locked (1 == locked, 0 == unlocked); uByte: willUnlockIfRequested (1 == will, 0 == won't)",
                      "RobotInfo", "RETURN_SINGLE");
  }
}

AREXPORT ArServerMode::~ArServerMode()
{

}

AREXPORT void ArServerMode::lockMode(bool willUnlockIfRequested)
{
  if (!myIsActive || ourActiveMode != this)
  {
    ArLog::log(myVerboseLogLevel, "ArServerMode::lockMode: mode %s could not lock because it is not active", getName());
    return;
  }
  ourActiveModeLocked = true;
  ourActiveModeWillUnlockIfRequested = willUnlockIfRequested;
  ArLog::log(myVerboseLogLevel, "Locked into %s mode, will unlock %s", 
             getName(), ArUtil::convertBool(willUnlockIfRequested));
  checkBroadcastModeInfoPacket();
}

AREXPORT void ArServerMode::unlockMode(void)
{
  if (!myIsActive || ourActiveMode != this)
  {
    ArLog::log(myVerboseLogLevel, "ArServerMode::unlockMode: mode %s could not unlock because it is not locked", getName());
    return;
  }
  ourActiveModeLocked = false;
  ourActiveModeWillUnlockIfRequested = false;
  ArLog::log(myVerboseLogLevel, "Unlocked from %s mode", getName());
  checkBroadcastModeInfoPacket();
}


AREXPORT bool ArServerMode::willUnlockIfRequested(void)
{
  if (ourActiveMode == NULL || !ourActiveModeLocked || 
      (ourActiveModeLocked && ourActiveModeWillUnlockIfRequested))
    return true;
  else
    return false;
}

       


AREXPORT bool ArServerMode::baseActivate(void)
{

  // if we're locked then return false so nothing else activates
  if (ourActiveMode != NULL && ourActiveMode != this && ourActiveModeLocked)
  {
    ArLog::log(myVerboseLogLevel, "Could not switch to %s mode because of lock.", 
               getName());
    // request our active mode to unlock
    ourActiveMode->requestUnlock();


    ourRequestedActivateModes.remove(this);
    ArLog::log(myVerboseLogLevel, "Mode %s wants to be activated.");
    ourRequestedActivateModes.push_front(this);
    return false;
  }
  ourNextActiveMode = this;
  if (ourActiveMode != NULL)
    ourActiveMode->deactivate();
  myIsActive = true;
  myActivityTimeMutex.lock();
  myActivityTime.setToNow(); 
  myActivityTimeMutex.unlock();

  /*
  if (myRobot != NULL)
  {
    myRobot->addUserTask(myName.c_str(), 50, &myUserTaskCB);
  }
  */
  ourActiveMode = this;
  ourNextActiveMode = NULL;
  if (myRobot != NULL)
  {
    myRobot->stop();
    myRobot->clearDirectMotion();
  }
  myActivateCallbacks.invoke();
  ArLog::log(myVerboseLogLevel, "Activated %s mode", getName());
  checkBroadcastModeInfoPacket();
  return true;
}

AREXPORT void ArServerMode::baseDeactivate(void)
{
  std::list<ArServerMode *>::iterator it;

  for (it = ourDefaultModes.begin(); 
       it != ourDefaultModes.end();
       it++)
  {
    ArLog::log(myVerboseLogLevel, "defaults are %s mode", (*it)->getName());
  }
  
  // if we're the active mode, we're deactivating, and we're locked, then unlock
  if (ourActiveMode != NULL && ourActiveMode == this && ourActiveModeLocked)
    unlockMode();
  /*
  if (myRobot != NULL)
    myRobot->remUserTask(&myUserTaskCB);
  */
  myIsActive = false;
  if (ourActiveMode == this)
  {
    ourActiveMode = NULL;
  }
  myDeactivateCallbacks.invoke();
  ArLog::log(myVerboseLogLevel, "Deactivated %s mode", getName());
  if (ourNextActiveMode == NULL)
  {
    // walk through until one of these is ready
    while ((it = ourRequestedActivateModes.begin()) != 
           ourRequestedActivateModes.end())
    {
      ArLog::log(myVerboseLogLevel,"Trying to activate %s", (*it)->getName());
      (*it)->activate();
      ourRequestedActivateModes.pop_front();
      if (ourActiveMode != NULL)
      {
        ArLog::log(myVerboseLogLevel, "and did, clearing requested activate modes");
        // now clear out the old modes so that we don't wind up
        // stacking too much
        ourRequestedActivateModes.clear();
        return;
      }
    }
    // now clear out the old modes so that we don't wind up stacking
    // too much (should be empty anyways here, just make sure
    ourRequestedActivateModes.clear();

    for (it = ourDefaultModes.begin(); 
         it != ourDefaultModes.end() && ourActiveMode == NULL;
         it++)
    {
      ArLog::log(ArLog::Terse, "Checking default on %s mode", (*it)->getName());
      (*it)->checkDefault();
      if (ourActiveMode != NULL)
      {
        //printf("and did\n");
        return;
      }
    }
  }
}

AREXPORT void ArServerMode::addAsDefaultMode(ArListPos::Pos pos)
{
  ArLog::log(ArLog::Normal, "Mode %s added as default mode", getName());
  if (pos == ArListPos::LAST)
    ourDefaultModes.push_back(this);
  else if (pos == ArListPos::FIRST)
    ourDefaultModes.push_front(this);
  else
  {
    ArLog::log(ArLog::Terse, "ArServerMode::addAsDefaultMode: bad list position.");
    ourDefaultModes.push_front(this);
  }
}

AREXPORT ArTime ArServerMode::getActivityTime(void) 
{ 
  ArTime ret;
  myActivityTimeMutex.lock();
  ret = myActivityTime;
  myActivityTimeMutex.unlock();
  return ret;
}

AREXPORT void ArServerMode::setActivityTimeToNow(void)     
{ 
  myActivityTimeMutex.lock();
  myHasSetActivityTime = true; 
  myActivityTime.setToNow(); 
  myActivityTimeMutex.unlock();
}

AREXPORT int ArServerMode::getActiveModeActivityTimeSecSince(void) 
{ 
  // chop this to an int so its easier to use, if you care about it
  // use the getActivityTime on the ourActiveMode
  if (ourActiveMode != NULL)
    return (int)ourActiveMode->getActivityTime().secSince();
  else
    return 0;
}

AREXPORT bool ArServerMode::addModeData(
        const char *name, const char *description,
        ArFunctor2<ArServerClient *, ArNetPacket *> *functor,
        const char *argumentDescription, const char *returnDescription,
        const char *commandGroup, const char *dataFlags)
{
  if (myServer->addData(name, description, functor, argumentDescription,
                        returnDescription, commandGroup, dataFlags))
  {
    ourModeDataMapMutex.lock();
    ourModeDataMap.insert(std::pair<std::string, std::string>(myName, name));
    ourModeDataMapMutex.unlock();
    return true;
  }
  else
  {
    ArLog::log(ArLog::Normal, "ArServerMode %s: Could not add mode data %s",
               myName.c_str(), name);
    return false;
  }
      
}

AREXPORT void ArServerMode::getModeDataList(ArServerClient *client, 
                                            ArNetPacket *packet)

{
  ourModeDataMapMutex.lock();
  ArNetPacket sending;
  sending.uByte4ToBuf(ourModeDataMap.size());
  std::multimap<std::string, std::string>::iterator it;
  for (it = ourModeDataMap.begin(); it != ourModeDataMap.end(); it++)
  {
    sending.strToBuf((*it).first.c_str());
    sending.strToBuf((*it).second.c_str());
  }
  ourModeDataMapMutex.unlock();
  client->sendPacketTcp(&sending);
}

AREXPORT void ArServerMode::getModeInfo(ArServerClient *client, 
                                        ArNetPacket *packet)
{
  ArNetPacket sending;
  buildModeInfoPacket(&sending);
  client->sendPacketTcp(&sending);
}

AREXPORT void ArServerMode::buildModeInfoPacket(ArNetPacket *sending)
{  
  if (ourActiveMode != NULL)
  {
    sending->strToBuf(ourActiveMode->getName());
    if (ourActiveModeLocked)
      sending->uByteToBuf(1);
    else
      sending->uByteToBuf(0);
    if (ourActiveModeWillUnlockIfRequested)
      sending->uByteToBuf(1);
    else
      sending->uByteToBuf(0);
  }
  else
  {
    sending->strToBuf("");
    sending->uByteToBuf(0);
    sending->uByteToBuf(0);
  }
  return;
}

AREXPORT void ArServerMode::checkBroadcastModeInfoPacket(void)
{
  if (ourActiveMode != ourLastActiveMode || 
      ourActiveModeLocked != ourLastActiveModeLocked ||
      ourActiveModeWillUnlockIfRequested != 
              ourLastActiveModeWillUnlockIfRequested)
  {
    ArNetPacket sending;
    buildModeInfoPacket(&sending);
    myServer->broadcastPacketTcp(&sending, "getModeInfo");
  }
  
  ourLastActiveMode = ourActiveMode;
  ourLastActiveModeLocked = ourActiveModeLocked;
  ourLastActiveModeWillUnlockIfRequested = ourActiveModeWillUnlockIfRequested;
}

AREXPORT ArServerMode* ArServerMode::getActiveMode(void) 
{ return ourActiveMode; }
  
AREXPORT std::list<ArServerMode *> *ArServerMode::getRequestedActivateModes(void)
{
  return &ourRequestedActivateModes;
}

---