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

ArCentralManager::ArCentralManager(ArServerBase *robotServer, 
                               ArServerBase *clientServer) :
  myNetSwitchCB(this, &ArCentralManager::netServerSwitch),
  myNetClientListCB(this, &ArCentralManager::netClientList),
  myAriaExitCB(this, &ArCentralManager::close)
{
  myRobotServer = robotServer;
  myClientServer = clientServer;
  
  myAriaExitCB.setName("ArCentralManager");
  Aria::addExitCallback(&myAriaExitCB, 25);
  
  myHeartbeatTimeout = 2;
  Aria::getConfig()->addParam(
          ArConfigArg("RobotTimeoutInMins", &myHeartbeatTimeout,
                      "The amount of time we can go without hearing a robot's heartbeat without disconnecting it.  A number less than 0 means that the robots will never timeout.  The time is in minutes but takes doubles (ie .5)", -1),
          "Central Server Settings", ArPriority::DETAILED);
  

  myRobotServer->addData("switch", "switches the direction of the connection, after this is requested it sends an empty packet denoting acceptance of the switch, then switches this to a client connection",
                         &myNetSwitchCB, "string: robotName", "empty packet", "RobotInfo", 
                         "RETURN_SINGLE");
  myClientServer->addData("clientList", "Lists the clients that are connected",
                          &myNetClientListCB, "none", 
                          "ubyte2: numClients; repeating for <numClients> [string: hostname (empty means this host); ubyte2: port; string: robot name; string: flags; string: robot ip address]",
                          "RobotInfo", "RETURN_SINGLE");
  myClientServer->addData("clientAdded", "Broadcast when a client is added",
                          NULL, "none", 
                          "string: hostname (empty means this host); ubyte2: port; string: robot name; string: flags; string: robot ip address",
                          "RobotInfo", "RETURN_SINGLE");
  myClientServer->addData("clientRemoved", "Broadcast when a client is removed",
                          NULL, "none", 
                          "string: hostname (empty means this host); ubyte2: port; string: robot name; string: flags; string: robot ip address",
                          "RobotInfo", "RETURN_SINGLE");

  myClientServer = clientServer;
  runAsync();
}

ArCentralManager::~ArCentralManager()
{
}

void ArCentralManager::close(void)
{
  std::list<ArCentralForwarder *>::iterator fIt;
  ArCentralForwarder *forwarder;

  ArLog::log(ArLog::Normal, "ArCentralManager closing");
  //myDataMutex.lock();
  while ((fIt = myForwarders.begin()) != myForwarders.end())
  {
    forwarder = (*fIt);
    std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
    for (it = myForwarderRemovedCBList.begin();
         it != myForwarderRemovedCBList.end();
         it++)
      (*it).second->invoke(forwarder);

    myForwarders.pop_front();
    delete forwarder;

  }
  //myDataMutex.unlock();
  ArLog::log(ArLog::Normal, "ArCentralManager closed");
}

void ArCentralManager::netServerSwitch(ArServerClient *client, ArNetPacket *packet)
{
  char robotName[512];
  std::string uniqueName;

  robotName[0] = '\0';

  //printf("robotName before '%s'\n", robotName);

  packet->bufToStr(robotName, sizeof(robotName));

  ArNetPacket sendPacket;
  client->sendPacketTcp(&sendPacket);

  ArSocket *clientSocket = new ArSocket;
  clientSocket->transfer(client->getTcpSocket());
  
  client->tcpCallback();
  client->forceDisconnect(true);

  
  // make the basis of the unique name
  if (robotName[0] != '\0')
    uniqueName = robotName;
  else
    uniqueName = client->getIPString();

  //printf("Starting with '%s'\n", uniqueName.c_str());

  myDataMutex.lock();
  // walk through and make the name unique
  bool nameIsUnique = false;
  std::list<ArCentralForwarder *>::iterator fIt;
  ArCentralForwarder *forwarder;
  while (!nameIsUnique)
  {
    nameIsUnique = true;
    for (fIt = myForwarders.begin(); 
         fIt != myForwarders.end() && nameIsUnique; 
         fIt++)
    {
      forwarder = (*fIt);
      if (strcasecmp(forwarder->getRobotName(), uniqueName.c_str()) == 0)
        nameIsUnique = false;
    }
    if (!nameIsUnique)
      uniqueName += "_";
  }
  myClientSockets.push_back(clientSocket);
  myClientNames.push_back(uniqueName);

  //printf("robotName after '%s'\n", robotName);
  // just print out the switch request name if its the same as it came in as
  if (strcmp(uniqueName.c_str(), robotName) == 0)
    ArLog::log(ArLog::Normal, "Got switch request from %s from %s", 
               client->getIPString(), uniqueName.c_str());
  // otherwise let them know what the unique name is
  if (strcmp(uniqueName.c_str(), robotName) != 0)
    ArLog::log(ArLog::Normal, 
               "Got switch request from %s from %s that started as %s", 
               client->getIPString(), uniqueName.c_str(), robotName);
  //printf("Ended with '%s'\n", uniqueName.c_str());
  myDataMutex.unlock();
}

void ArCentralManager::netClientList(ArServerClient *client, ArNetPacket *packet)
{
  ArNetPacket sendPacket;
  std::list<ArCentralForwarder *>::iterator fIt;
  ArCentralForwarder *forwarder;

  myDataMutex.lock();
  sendPacket.uByte2ToBuf(myForwarders.size());
  for (fIt = myForwarders.begin(); fIt != myForwarders.end(); fIt++)
  {
    forwarder = (*fIt);
    sendPacket.strToBuf("");
    sendPacket.uByte2ToBuf(forwarder->getPort());
    sendPacket.strToBuf(forwarder->getRobotName());
    sendPacket.strToBuf("");
    sendPacket.strToBuf(
            forwarder->getClient()->getTcpSocket()->getIPString());
  }
  myDataMutex.unlock();
  client->sendPacketTcp(&sendPacket);
}

void *ArCentralManager::runThread(void *arg)
{
  std::list<ArSocket *>::iterator sIt;
  std::list<std::string>::iterator nIt;
  std::list<ArCentralForwarder *>::iterator fIt;
  ArSocket *socket;
  std::string robotName;
  ArCentralForwarder *forwarder;

  threadStarted();
  setThreadName("serverSwitch");
  while (getRunning())
  {
    myDataMutex.lock();

    while ((sIt = myClientSockets.begin()) != myClientSockets.end() && 
           (nIt = myClientNames.begin()) != myClientNames.end())
    {
      socket = (*sIt);
      robotName = (*nIt);

      myClientSockets.pop_front();
      myClientNames.pop_front();

      ArLog::log(ArLog::Normal, 
                 "New forwarder for socket from %s with name %s", 
                 socket->getIPString(), robotName.c_str());

      forwarder = new ArCentralForwarder(myClientServer, socket, 
                                         robotName.c_str(), 
                                         myClientServer->getTcpPort() + 1, 
                                         &myUsedPorts);
      myForwarders.push_back(forwarder);
    }

    std::list<ArCentralForwarder *> removeList;
    for (fIt = myForwarders.begin(); fIt != myForwarders.end(); fIt++)
    {
      forwarder = (*fIt);
      bool connected = forwarder->isConnected();
      bool removed = false;
      if (!forwarder->callOnce(myHeartbeatTimeout))
      {
        ArLog::log(ArLog::Normal, "Will remove forwarder from %s", 
                   forwarder->getRobotName());
        removeList.push_back(forwarder);
        removed = true;
      }
      if (!connected && !removed && forwarder->isConnected())
      {
        ArLog::log(ArLog::Normal, "Adding forwarder %s", 
                   forwarder->getRobotName());
        myUsedPorts.insert(forwarder->getPort());
        
        std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
        for (it = myForwarderAddedCBList.begin();
             it != myForwarderAddedCBList.end();
             it++)
        {
          if ((*it).second->getName() == NULL || 
              (*it).second->getName()[0] == '\0')
            ArLog::log(ArLog::Normal, "Calling unnamed add functor at %d",
                       -(*it).first);
          else
            ArLog::log(ArLog::Normal, "Calling %s add functor at %d",
                       (*it).second->getName(), -(*it).first);
          (*it).second->invoke(forwarder);
        }
        ArLog::log(ArLog::Normal, "Added forwarder %s", 
                   forwarder->getRobotName());      
        ArNetPacket sendPacket;
        sendPacket.strToBuf("");
        sendPacket.uByte2ToBuf(forwarder->getPort());
        sendPacket.strToBuf(forwarder->getRobotName());
        sendPacket.strToBuf("");
        sendPacket.strToBuf(
                forwarder->getClient()->getTcpSocket()->getIPString());
        myClientServer->broadcastPacketTcp(&sendPacket, "clientAdded");
      }
    }

    while ((fIt = removeList.begin()) != removeList.end())
    {
      forwarder = (*fIt);

      ArLog::log(ArLog::Normal, "Removing forwarder %s", 
                 forwarder->getRobotName());
      std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
      for (it = myForwarderRemovedCBList.begin();
           it != myForwarderRemovedCBList.end();
           it++)
      {
        if ((*it).second->getName() == NULL || 
            (*it).second->getName()[0] == '\0')
          ArLog::log(ArLog::Normal, "Calling unnamed remove functor at %d",
                     -(*it).first);
        else
          ArLog::log(ArLog::Normal, "Calling %s remove functor at %d",
                     (*it).second->getName(), -(*it).first);
        (*it).second->invoke(forwarder);
      }

      ArLog::log(ArLog::Normal, "Called forwarder removed for %s", 
                 forwarder->getRobotName());
      ArNetPacket sendPacket;
      sendPacket.strToBuf("");
      sendPacket.uByte2ToBuf(forwarder->getPort());
      sendPacket.strToBuf(forwarder->getRobotName());
      sendPacket.strToBuf("");
      sendPacket.strToBuf(
              forwarder->getClient()->getTcpSocket()->getIPString());
      myClientServer->broadcastPacketTcp(&sendPacket, "clientRemoved");

      myUsedPorts.erase(forwarder->getPort());
      myForwarders.remove(forwarder);
      delete forwarder;
      removeList.pop_front();
      ArLog::log(ArLog::Normal, "Removed forwarder");

    }
    myDataMutex.unlock();

    ArUtil::sleep(100);
  }
  return NULL;
}


AREXPORT void ArCentralManager::addForwarderAddedCallback(
        ArFunctor1<ArCentralForwarder *> *functor, int priority)
{
  myCallbackMutex.lock();
  myForwarderAddedCBList.insert(
          std::pair<int, ArFunctor1<ArCentralForwarder *> *>(-priority, 
                                                                  functor));
  myCallbackMutex.unlock();
}

AREXPORT void ArCentralManager::remForwarderAddedCallback(
        ArFunctor1<ArCentralForwarder *> *functor)
{
  myCallbackMutex.lock();
  
  std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;

  for (it = myForwarderAddedCBList.begin(); 
       it != myForwarderAddedCBList.end(); 
       ++it)
  {
    if ((*it).second == functor)
    {
      myForwarderAddedCBList.erase(it);
      myCallbackMutex.unlock();
      remForwarderAddedCallback(functor);
      return;
    }
  }
  myCallbackMutex.unlock();
}

AREXPORT void ArCentralManager::addForwarderRemovedCallback(
        ArFunctor1<ArCentralForwarder *> *functor, int priority)
{
  myCallbackMutex.lock();
  myForwarderRemovedCBList.insert(
          std::pair<int, ArFunctor1<ArCentralForwarder *> *>(-priority, 
                                                                  functor));
  myCallbackMutex.unlock();
}

AREXPORT void ArCentralManager::remForwarderRemovedCallback(
        ArFunctor1<ArCentralForwarder *> *functor)
{
  myCallbackMutex.lock();
  std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;

  for (it = myForwarderRemovedCBList.begin(); 
       it != myForwarderRemovedCBList.end(); 
       ++it)
  {
    if ((*it).second == functor)
    {
      myForwarderRemovedCBList.erase(it);
      myCallbackMutex.unlock();
      remForwarderRemovedCallback(functor);
      return;
    }
  }
  myCallbackMutex.unlock();
}

---