RobotFactory.cc

/*  
    (C) Copyright 2005, ActivMedia Robotics LLC <http://www.activmedia.com>
    (C) Copyright 2006-2010 MobileRobots, Inc. <http://www.mobilerobots.com>
    (C) Copyright 2011-2015 Adept MobileRobots <http://www.mobilerobots.com>
    (C) Copyright 2016-2017 Omron Adept Technologies

    This 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 software 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 software; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <set>
#include <errno.h>

#include "RobotFactory.hh"
#include "RobotInterface.hh"
#include "EmulatePioneer.hh"

#include "MobileSim.hh"
#include "Socket.hh"

#include "ArASyncTask.h"

#include "ListeningSocket.hh"

ArMutex RobotFactory::myClientSocketsMutex;


//RobotFactory::RobotFactory(const std::string& modelName, bool verbose, const char *listenAddress, const MobileSim::Options *userOpts) :
RobotFactory::RobotFactory(const std::string& modelName, const MobileSim::Options *userOpts, const char *listenAddress) :
  myModelName(modelName),
  myPort(8101),
  //myVerbose(verbose),
  myListenAddress(listenAddress),
  //mySRISimCompat(false),
  //mySRISimLaserCompat(true),
  //myLogPacketsReceived(false),
  acceptClientCB(this, &RobotFactory::acceptNewClient),
  //myWarnUnsupportedCommands(false)
  myUserOptions(userOpts)
{
  //ArLog::log(ArLog::Normal, "RobotFactory::ctor: modelName = %s, verbose = %d, listenAddress = %s, userOpts = %d\n", modelName.c_str(), (int)verbose, listenAddress, (int)userOpts);

  // For debugging only
  //myCRCumulateiveMSec = 0;
  //myCRNumCreated = 0;
  //myCreateRobotLastXMSec.clear();
  //myCreatedRobotLastXToTrack = 10;
}

ArSocket* RobotFactory::open(int port, const char *listenAddress)
{
  if(listenAddress)
    log("RobotFactory: opening port %d for new connections to address %s", port, listenAddress);
  else if(myListenAddress)
    log("RobotFactory: opening port %d for new connections to address %s", port, myListenAddress);
  else
    log("RobotFactory: opening port %d for new connections", port);

  // Launch ListeningSocket async task
  myListeningSocket = new ListeningSocket;
  ArSocket *facsock = myListeningSocket->init(port, this, listenAddress?listenAddress:myListenAddress);
  if(!facsock)
  {
    delete myListeningSocket;
    return NULL;
  }
  else
  {
    myListeningSocket->runAsync();
    return facsock;
  }


/* - Obsolete code, from when the listening socket was just one client in the RobotFactory's client list, made to wait while other MobileSim routines were running
  if(myListenSocket.open(port, ArSocket::TCP, listenAddress?listenAddress:myListenAddress)) // == true || mySocket.getError() == ArSocket::NoErr)
  {
    myListenSocket.setNonBlock();
    myListenSocket.setReuseAddress();
    // TODO: removing this temporarily so that testing new ListeningSocket doesn't conflict with this socket
    MobileSim::Sockets::addSocketCallback(&myListenSocket, &acceptClientCB, "RobotFactory listening socket (callback to accept clients and create EP objects)");
    print_debug("RobotFactory socket callback functor is 0x%x", &acceptClientCB);
    return &myListenSocket;
  }
  else  
  {
    fprintf(stderr, "Error %d opening socket on port %d for robot factory: %s\n", myListenSocket.getError(), port, myListenSocket.getErrorStr().c_str());
    log("RobotFactory: error opening socket:");
    log(myListenSocket.getErrorStr().c_str());
    return NULL;
  }
*/
}

RobotFactory::~RobotFactory()
{
}


void RobotFactory::acceptNewClient(unsigned int /*maxTime*/)
{
  ArSocket *clientSocket = new ArSocket;
  ArTime timer;

  if(!myListenSocket.accept(clientSocket))
  {
    log("RobotFactory: error accepting client:");
    log(myListenSocket.getErrorStr().c_str());
    delete clientSocket;
    return;
  }

  if(!clientSocket->isOpen())
  {
    log("RobotFactory: error accepting client: new client socket not open.");
    delete clientSocket;
    return;
  }

  log("Accepted client. Creating robot...");

  RobotInterface *ri = NULL;
  if(myUserOptions != NULL && myUserOptions->commercial)
    ri = createStubRobot(myModelName, clientSocket->getIPString()); // Create a stub robot (just a default 'position' model)
  else
    ri = createRobot(myModelName, clientSocket->getIPString());
  if(!ri)
  {
    log("Robot factory: Error creating new robot. Closing client socket.");
    clientSocket->close();
    delete clientSocket;
    return;
  }

  //print_debug("RobotFactory: new RobotInterface is 0x%x", ri);
  //log(("RobotFactory: created robot interface "));
  // Note, assumes that EmulatePioneer properly deletes client sockets
  // when told to do so (otherwise leaks memory)
  clientSocket->setNonBlock();
  EmulatePioneer *ep = new EmulatePioneer(ri, myModelName, clientSocket, /*deleteOnDisconnect=*/true, /*deleteClientSocketOnDisconnect=*/true, myUserOptions);
  ep->setSimulatorIdentification("MobileSim", MOBILESIM_VERSION);
  //ep->setCommandsToIgnore(myCommandsToIgnore);
  //ep->setVerbose(myVerbose);
  //ep->setSRISimCompat(mySRISimCompat, mySRISimLaserCompat);
  //ep->setLogPacketsReceived(myLogPacketsReceived);
  //ep->setWarnUnsupportedCommands(myWarnUnsupportedCommands);
} 

void RobotFactory::acceptNewClientFromListenerThread(ArSocket *clientSocket, unsigned int maxTime/*=0*/)
{
  myClientSocketsMutex.lock();
  //ArLog::log(ArLog::Normal, "RobotFactory::acceptNewClientFromListenerThread(): %s factory accepted new client. Adding to myNewClientSockets...", myModelName.c_str());
  myNewClientSockets.push(clientSocket);
  myClientSocketsMutex.unlock();
}

void RobotFactory::createNewRobotsFromClientsList()
{
  // Currently, this method creates one robot if there is at least one client socket
  //  in the pending queue.

  // Testing has shown this process takes ~1.5msec (for a full robot model, probably much less for modelInitByTCP's stub model)
  //   One robot per major update loop results in ~25 robots/sec created. If a higher
  //   creation rate is necessary for mass connections, this method should be looped
  //   with a max time, so more than one robot can be created per major update loop.

  while (true)  // This loop will iterate until the queue is empty. TODO: If this ever starts to take too long (such as during a mass connection event), a time-based exit condition should be added.
  {
    myClientSocketsMutex.lock();

    if (myNewClientSockets.empty())
    {
      //ArLog::log(ArLog::Normal, "RobotFactory::createNewRobotsFromClientsList(): %s factory's myNewClientSockets is empty. Returning...", myModelName.c_str());
      myClientSocketsMutex.unlock();
      return;
    }

    //ArLog::log(ArLog::Normal, "RobotFactory::createNewRobotsFromClientsList(): %s factory's myNewClientSockets has %d clients. Fetching one...", myModelName.c_str(), myNewClientSockets.size());
    ArSocket *clientSocket = myNewClientSockets.front();
    myNewClientSockets.pop();

    myClientSocketsMutex.unlock();

    //ArLog::log(ArLog::Normal, "RobotFactory::createNewRobotsFromClientsList(): %s factory. Creating one robot...", myModelName.c_str());
    RobotInterface *ri = NULL;
    if(myUserOptions != NULL && myUserOptions->commercial)
      ri = createStubRobot(myModelName, clientSocket->getIPString()); // Create a stub robot (just a default 'position' model)
    else
      ri = createRobot(myModelName, clientSocket->getIPString());
    if(!ri)
    {
      log("Robot factory: Error creating new robot. Closing client socket.");
      clientSocket->close();
      delete clientSocket;
      return;
    }

    //print_debug("RobotFactory: new RobotInterface is 0x%x", ri);
    //log(("RobotFactory: created robot interface "));
    // Note, assumes that EmulatePioneer properly deletes client sockets
    // when told to do so (otherwise leaks memory)
    clientSocket->setNonBlock();
    EmulatePioneer *ep = new EmulatePioneer(ri, myModelName, clientSocket, /*deleteOnDisconnect=*/true, /*deleteClientSocketOnDisconnect=*/true, myUserOptions);
    ep->setSimulatorIdentification("MobileSim", MOBILESIM_VERSION);
    //ep->setCommandsToIgnore(myCommandsToIgnore);
    //ep->setVerbose(myVerbose);
    //ep->setSRISimCompat(mySRISimCompat, mySRISimLaserCompat);
    //ep->setLogPacketsReceived(myLogPacketsReceived);
    //ep->setWarnUnsupportedCommands(myWarnUnsupportedCommands);
  }
}