ArSimpleConnector.cpp

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     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 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArSimpleConnector.h"
#include "ArRobot.h"
#include "ArSick.h"

ArSimpleConnector::ArSimpleConnector(int *argc, char **argv) 
{
  reset();
  myParser = new ArArgumentParser(argc, argv);
  myOwnParser = true;
}

ArSimpleConnector::ArSimpleConnector(ArArgumentBuilder *builder) 
{
  reset();
  myParser = new ArArgumentParser(builder);
  myOwnParser = true;
}

ArSimpleConnector::ArSimpleConnector(ArArgumentParser *parser)
{
  reset();
  myParser = parser;
  myOwnParser = false;
}

ArSimpleConnector::~ArSimpleConnector(void)
{

}

void ArSimpleConnector::reset(void)
{
  myRobot = NULL;
  myRemoteHost = NULL;
  myRobotPort = NULL;
  myRemoteRobotTcpPort = 8101;
  myRobotBaud = 9600;
  myRemoteIsSim = false;
  setMaxNumLasers();
  ArUtil::deleteSet(myLasers.begin(), myLasers.end());
  myLasers.clear();
}

void ArSimpleConnector::setMaxNumLasers(int maxNumLasers) 
{
  if (maxNumLasers > 0)
    myMaxNumLasers = maxNumLasers;
  else
    myMaxNumLasers = 0;
  ArUtil::deleteSet(myLasers.begin(), myLasers.end());
  myLasers.clear();
}



bool ArSimpleConnector::parseArgs(void)
{
  return parseArgs(myParser);
}
bool ArSimpleConnector::parseArgs(ArArgumentParser *parser)
{
  int i;
  if (myMaxNumLasers > 0)
  {
    for (i = 1; i <= myMaxNumLasers; i++)
      myLasers.push_front(new LaserData(i));
  }

  if (parser->checkArgument("-remoteIsSim") ||
      parser->checkArgument("-ris"))
    myRemoteIsSim = true;

  if (!parser->checkParameterArgumentString("-remoteHost", 
                                             &myRemoteHost) ||
      !parser->checkParameterArgumentString("-rh", 
                                             &myRemoteHost) ||
      !parser->checkParameterArgumentString("-robotPort",
                                             &myRobotPort) ||
      !parser->checkParameterArgumentString("-rp",
                                             &myRobotPort) ||
      !parser->checkParameterArgumentInteger("-remoteRobotTcpPort",
                                              &myRemoteRobotTcpPort) ||
      !parser->checkParameterArgumentInteger("-rrtp",
                                              &myRemoteRobotTcpPort) ||
      !parser->checkParameterArgumentInteger("-robotBaud",
                                              &myRobotBaud) ||
      !parser->checkParameterArgumentInteger("-rb",
                                             &myRobotBaud))
    return false;
  else
  {
    for (i = 1; i <= myMaxNumLasers; i++)
      if (!parseLaserArgs(parser, i))
        return false;
    return true;
  }

}

bool ArSimpleConnector::parseLaserArgs(ArArgumentParser *parser, 
                                                int laserNumber)
{
  char buf[512];
  std::list<LaserData *>::iterator it;
  LaserData *laserData = NULL;
    
  for (it = myLasers.begin(); it != myLasers.end(); it++)
  {
    if ((*it)->myNumber == laserNumber)
    {
      laserData = (*it);
      break;
    }
  }
  if (laserData == NULL)
  {
    ArLog::log(ArLog::Terse, "Do not have laser %d", laserNumber);
    return false;
  }

  if (laserData->myNumber == 1)
    buf[0] = '\0';
  else
    sprintf(buf, "%d", laserData->myNumber);

  if (parser->checkArgumentVar("-connectLaser%s", buf) || 
      parser->checkArgumentVar("-cl%s", buf))
  {
    laserData->myConnect = true;
  }

  if (!parser->checkParameterArgumentStringVar(NULL, &laserData->myPort, 
                                               "-laserPort%s", buf) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myPort,
                                            "-lp%s", buf) ||
      !parser->checkParameterArgumentIntegerVar(NULL,
              &laserData->myRemoteTcpPort, 
              "-remoteLaserTcpPort%s", buf) ||
      !parser->checkParameterArgumentIntegerVar(NULL,
              &laserData->myRemoteTcpPort,
              "-rltp%s", buf) ||
      !parser->checkParameterArgumentBoolVar(&laserData->myFlippedReallySet,
                                             &laserData->myFlipped,
                                             "-laserFlipped%s", buf) ||
      !parser->checkParameterArgumentBoolVar(&laserData->myFlippedReallySet,
                                             &laserData->myFlipped,
                                             "-lf%s", buf) ||
      !parser->checkParameterArgumentBoolVar(
              &laserData->myPowerControlledReallySet,
              &laserData->myPowerControlled,
              "-laserPowerControlled%s", buf) ||
      !parser->checkParameterArgumentBoolVar(
              &laserData->myPowerControlledReallySet,
              &laserData->myPowerControlled,
                                             "-lpc%s", buf) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myDegrees,
                                               "-laserDegrees%s", buf) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myDegrees,
                                               "-ld%s", buf) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myIncrement,
                                               "-laserIncrement%s", buf) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myIncrement,
                                               "-li%s", buf))
  {
    return false;
  }
  if (laserData->myDegrees == NULL || laserData->myDegrees[0] == '\0')
    laserData->mySickDegrees = ArSick::DEGREES180;
  else if (strcasecmp(laserData->myDegrees, "180") == 0)
    laserData->mySickDegrees = ArSick::DEGREES180;
  else if (strcasecmp(laserData->myDegrees, "100") == 0)
    laserData->mySickDegrees = ArSick::DEGREES100;
  else
  {
    ArLog::log(ArLog::Normal, 
               "Could not set laserDegrees%s, it should be set to 180 or 100",
               buf);
    return false;
  }
  
  if (laserData->myIncrement == NULL || laserData->myIncrement[0] == '\0')
    laserData->mySickIncrement = ArSick::INCREMENT_ONE;
  else if (strcasecmp(laserData->myIncrement, "one") == 0)
    laserData->mySickIncrement = ArSick::INCREMENT_ONE;
  else if (strcasecmp(laserData->myIncrement, "half") == 0)
    laserData->mySickIncrement = ArSick::INCREMENT_HALF;
  else
  {
    ArLog::log(ArLog::Normal, 
        "Could not set laserIncrement%s, it should be set to 'one' or 'half'",
               buf);
    return false;
  }

  return true;
}

void ArSimpleConnector::logOptions(void) const
{
  ArLog::log(ArLog::Terse, "Options for ArSimpleConnector (see docs for more details):");
  ArLog::log(ArLog::Terse, "");
  ArLog::log(ArLog::Terse, "Robot options:");
  ArLog::log(ArLog::Terse, "-remoteHost <remoteHostNameOrIP>");
  ArLog::log(ArLog::Terse, "-rh <remoteHostNameOrIP>");
  ArLog::log(ArLog::Terse, "-robotPort <robotSerialPort>");
  ArLog::log(ArLog::Terse, "-rp <robotSerialPort>");
  ArLog::log(ArLog::Terse, "-robotBaud <baud>");
  ArLog::log(ArLog::Terse, "-rb <baud>");
  ArLog::log(ArLog::Terse, "-remoteRobotTcpPort <remoteRobotTcpPort>");
  ArLog::log(ArLog::Terse, "-rrtp <remoteRobotTcpPort>");
  ArLog::log(ArLog::Terse, "-remoteIsSim");
  ArLog::log(ArLog::Terse, "-ris");
  
  for (int i = 1; i <= myMaxNumLasers; i++)
    logLaserOptions(i);
}

void ArSimpleConnector::logLaserOptions(
        unsigned int laserNumber) const
{
  char buf[512];
  
  if (laserNumber == 1)
    buf[0] = '\0';
  else
    sprintf(buf, "%d", laserNumber);

  ArLog::log(ArLog::Terse, "");
  ArLog::log(ArLog::Terse, "Laser%s options:", buf);
  ArLog::log(ArLog::Terse, "-connectLaser%s", buf);
  ArLog::log(ArLog::Terse, "-cl%s", buf);
  ArLog::log(ArLog::Terse, "-laserPort%s <laserSerialPort>", buf);
  ArLog::log(ArLog::Terse, "-lp%s <laserSerialPort>", buf);
  ArLog::log(ArLog::Terse, "-remoteLaserTcpPort%s <remoteLaserTcpPort>", buf);
  ArLog::log(ArLog::Terse, "-rltp%s <remoteLaserTcpPort>", buf);  
  ArLog::log(ArLog::Terse, "-laserFlipped%s <true|false>", buf);
  ArLog::log(ArLog::Terse, "-lf%s <true|false>", buf);
  ArLog::log(ArLog::Terse, "-laserPowerControlled%s <true|false>", buf);
  ArLog::log(ArLog::Terse, "-lpc%s <true|false>", buf);
  ArLog::log(ArLog::Terse, "-laserDegrees%s <180|100>", buf);
  ArLog::log(ArLog::Terse, "-ld%s <180|100>", buf);
  ArLog::log(ArLog::Terse, "-laserIncrement%s <one|half>", buf);
  ArLog::log(ArLog::Terse, "-li%s <one|half>", buf);

}




bool ArSimpleConnector::setupRobot(ArRobot *robot)
{
  myRobot = robot;
  // First we see if we can open the tcp connection, if we can we'll
  // assume we're connecting to the sim, and just go on...  if we
  // can't open the tcp it means the sim isn't there, so just try the
  // robot

  // see if we're doing remote host or not
  if (myRemoteHost != NULL)
    myRobotTcpConn.setPort(myRemoteHost, myRemoteRobotTcpPort);
  else
    myRobotTcpConn.setPort("localhost", myRemoteRobotTcpPort);

  // see if we can get to the simulator  (true is success)
  if (myRobotTcpConn.openSimple())
  {
    robot->setDeviceConnection(&myRobotTcpConn);
    // we could get to the sim, so set the robots device connection to the sim
    if (myRemoteHost != NULL)
    {
      ArLog::log(ArLog::Normal, "Connected to remote host %s through tcp.\n", 
                 myRemoteHost);
      if (myRemoteIsSim)
        myUsingSim = true;
      else
        myUsingSim = false;
    }
    else
    {
      ArLog::log(ArLog::Normal, "Connecting to simulator through tcp.\n");
      myUsingSim = true;
    }
  }
  else
  {
    // if we were trying for a remote host and it failed, just exit
    if (myRemoteHost != NULL)
    {
      ArLog::log(ArLog::Terse, "Could not connect robot to remote host %s.\n", myRemoteHost);
      return false;
    }
    // we couldn't get to the sim, so set the port on the serial
    // connection and then set the serial connection as the robots
    // device

    myRobotSerConn.setPort(myRobotPort);
    myRobotSerConn.setBaud(myRobotBaud);
    ArLog::log(ArLog::Normal,
               "Could not connect to simulator, connecting to robot through serial port %s.", 
               myRobotSerConn.getPort());
    robot->setDeviceConnection(&myRobotSerConn);
    myUsingSim = false;
  }
  return true;
}

bool ArSimpleConnector::connectRobot(ArRobot *robot)
{
  if (!setupRobot(robot))
    return false;
  else
    return robot->blockingConnect();
}

bool ArSimpleConnector::setupLaser(ArSick *sick)
{
  return setupLaserArbitrary(sick, 1);
}

bool ArSimpleConnector::setupSecondLaser(ArSick *sick)
{
  return setupLaserArbitrary(sick, 2);
}

bool ArSimpleConnector::setupLaserArbitrary(ArSick *sick, 
                                                    int laserNumber)
{
  std::list<LaserData *>::iterator it;
  LaserData *laserData = NULL;
  const ArRobotParams *params;
    
  for (it = myLasers.begin(); it != myLasers.end(); it++)
  {
    if ((*it)->myNumber == laserNumber)
    {
      laserData = (*it);
      break;
    }
  }
  if (laserData == NULL)
  {
    ArLog::log(ArLog::Terse, "Do not have laser %d", laserNumber);
    return false;
  }
  
  if (laserData->myNumber == 1 && myRobot != NULL && 
      myRobot->isConnected())
  {
    params = myRobot->getRobotParams();
    if (!laserData->myFlippedReallySet)
      laserData->myFlipped = params->getLaserFlipped();
    if (!laserData->myPowerControlledReallySet)
      laserData->myPowerControlled = params->getLaserPowerControlled();
    if (laserData->myPort == NULL || laserData->myPort[0] == '\0')
    {
      if (strcmp(params->getLaserPort(), "COM1") == 0)
        laserData->myPort = ArUtil::COM1;
      else if (strcmp(params->getLaserPort(), "COM2") == 0)
        laserData->myPort = ArUtil::COM2;
      else if (strcmp(params->getLaserPort(), "COM3") == 0)
        laserData->myPort = ArUtil::COM3;
      else if (strcmp(params->getLaserPort(), "COM4") == 0)
        laserData->myPort = ArUtil::COM4;
      else
      {
        if (params->getLaserPort() != NULL)
          laserData->myPort = params->getLaserPort();
        else
          laserData->myPort = ArUtil::COM3;
        ArLog::log(ArLog::Normal,
                   "Could not find LaserPort from robot parameters, using %s", 
                   laserData->myPort);

      }
    }
    
  }

  if (laserData->myPort == NULL || strlen(laserData->myPort) == 0)
  {
    ArLog::log(ArLog::Normal, "There is no port defined for laser %d", 
               laserData->myNumber);
    return false;
  }
  if (!myUsingSim && myRemoteHost != NULL && strlen(myRemoteHost) > 0)
    sick->configure(myUsingSim, laserData->myPowerControlled,
                    laserData->myFlipped,
                    ArSick::BAUD9600, laserData->mySickDegrees, 
                    laserData->mySickIncrement);
  else if (myUsingSim && laserData->myNumber != 1)
  {
    ArLog::log(ArLog::Normal, "Cannot use the simulator with multiple lasers");
    return false;
  }
  else
    sick->configure(myUsingSim, laserData->myPowerControlled,
                    laserData->myFlipped,
                    ArSick::BAUD38400, laserData->mySickDegrees, 
                    laserData->mySickIncrement);

  // set the port, if we need to
  if (!myUsingSim)
  { 
    if (myRemoteHost != NULL && strlen(myRemoteHost) > 0)
    { 
      laserData->myTcpConn.setPort(myRemoteHost, 
                                   laserData->myRemoteTcpPort);
      sick->setDeviceConnection(&laserData->myTcpConn);
      if (!laserData->myTcpConn.openSimple())
      { 
        ArLog::log(ArLog::Terse, "Could not connect laser to remote host %s.\n", myRemoteHost);
        return false; 
      } 
    } 
    else 
    { 
      laserData->mySerConn.setPort(laserData->myPort);
      sick->setDeviceConnection(&laserData->mySerConn); 
    } 
  }
  return true;

}

bool ArSimpleConnector::connectLaser(ArSick *sick)
{
  return connectLaserArbitrary(sick, 1);
}


bool ArSimpleConnector::connectSecondLaser(ArSick *sick)
{
  return connectLaserArbitrary(sick, 2);
}

bool ArSimpleConnector::connectLaserArbitrary(ArSick *sick,
                                                      int laserNumber)
{
  std::list<LaserData *>::iterator it;
  LaserData *laserData = NULL;
  
  for (it = myLasers.begin(); it != myLasers.end(); it++)
  {
    if ((*it)->myNumber == laserNumber)
    {
      laserData = (*it);
      break;
    }
  }
  if (laserData == NULL)
  {
    ArLog::log(ArLog::Terse, "Do not have laser %d", laserNumber);
    return false;
  }

  sick->lockDevice();
  // set up the laser regardless
  setupLaserArbitrary(sick, laserNumber);
  sick->unlockDevice();
  // see if we want to connect
  if (!laserData->myConnect)
    return true;
  else
    return sick->blockingConnect();
}

---