ArSimpleConnector.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 "ariaOSDef.h"
#include "ArSimpleConnector.h"
#include "ArRobot.h"
#include "ArSick.h"
#include "ariaInternal.h"

AREXPORT ArSimpleConnector::ArSimpleConnector(int *argc, char **argv) :
  myParseArgsCB(this, &ArSimpleConnector::parseArgs),
  myLogOptionsCB(this, &ArSimpleConnector::logOptions)
{
  reset();
  myParser = new ArArgumentParser(argc, argv);
  myOwnParser = true;
  myParseArgsCB.setName("ArSimpleConnector");
  Aria::addParseArgsCB(&myParseArgsCB, 75);
  myLogOptionsCB.setName("ArSimpleConnector");
  Aria::addLogOptionsCB(&myLogOptionsCB, 90);
}

AREXPORT ArSimpleConnector::ArSimpleConnector(ArArgumentBuilder *builder) :
  myParseArgsCB(this, &ArSimpleConnector::parseArgs),
  myLogOptionsCB(this, &ArSimpleConnector::logOptions)
{
  reset();
  myParser = new ArArgumentParser(builder);
  myOwnParser = true;
  myParseArgsCB.setName("ArSimpleConnector");
  Aria::addParseArgsCB(&myParseArgsCB, 75);
  myLogOptionsCB.setName("ArSimpleConnector");
  Aria::addLogOptionsCB(&myLogOptionsCB, 90);
}

AREXPORT ArSimpleConnector::ArSimpleConnector(ArArgumentParser *parser) :
  myParseArgsCB(this, &ArSimpleConnector::parseArgs),
  myLogOptionsCB(this, &ArSimpleConnector::logOptions)
{
  reset();
  myParser = parser;
  myOwnParser = false;
  myParseArgsCB.setName("ArSimpleConnector");
  Aria::addParseArgsCB(&myParseArgsCB, 75);
  myLogOptionsCB.setName("ArSimpleConnector");
  Aria::addLogOptionsCB(&myLogOptionsCB, 90);
}

AREXPORT 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();
}

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



AREXPORT bool ArSimpleConnector::parseArgs(void)
{
  return parseArgs(myParser);
}

AREXPORT 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;
  }

}

AREXPORT 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) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myUnits,
                           "-laserUnits%s", buf) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myUnits,
                           "-lu%s", buf) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myBits,
                           "-laserReflectorBits%s", buf) ||
      !parser->checkParameterArgumentStringVar(NULL, &laserData->myBits,
                           "-lrb%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;
  }

  if (laserData->myUnits == NULL || laserData->myUnits[0] == '\0')
    laserData->mySickUnits = ArSick::UNITS_1MM;
  else if (strcasecmp(laserData->myUnits, "1mm") == 0)
    laserData->mySickUnits = ArSick::UNITS_1MM;
  else if (strcasecmp(laserData->myUnits, "1cm") == 0)
    laserData->mySickUnits = ArSick::UNITS_1CM;
  else if (strcasecmp(laserData->myUnits, "10cm") == 0)
    laserData->mySickUnits = ArSick::UNITS_10CM;
  else
  {
    ArLog::log(ArLog::Normal, 
           "Could not set laserUnits%s, it should be set to '1mm', '1cm', or '10cm'",
           buf);
    return false;
  }

  if (laserData->myBits == NULL || laserData->myBits[0] == '\0')
    laserData->mySickBits = ArSick::BITS_1REFLECTOR;
  else if (strcasecmp(laserData->myBits, "1ref") == 0)
    laserData->mySickBits = ArSick::BITS_1REFLECTOR;
  else if (strcasecmp(laserData->myBits, "2ref") == 0)
    laserData->mySickBits = ArSick::BITS_2REFLECTOR;
  else if (strcasecmp(laserData->myBits, "3ref") == 0)
    laserData->mySickBits = ArSick::BITS_3REFLECTOR;
  else
  {
    ArLog::log(ArLog::Normal, 
           "Could not set laserReflectorBits%s, it should be set to '1ref', '2ref', or '3ref'",
           buf);
    return false;
  }

  return true;
}

AREXPORT 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);
}

AREXPORT 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);
  ArLog::log(ArLog::Terse, "-laserUnits%s <1mm|1cm|10cm>", buf);
  ArLog::log(ArLog::Terse, "-lu%s <1mm|1cm|10cm>", buf);
  ArLog::log(ArLog::Terse, "-laserReflectorBits%s <1ref|2ref|3ref>", buf);
  ArLog::log(ArLog::Terse, "-lrb%s <1ref|2ref|3ref>", buf);

}




AREXPORT 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, port %d.\n", myRemoteHost, myRemoteRobotTcpPort);
      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;
}

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

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

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

AREXPORT 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, "ArSimpleConnector::setupLaser: Do not have laser #%d (parseArgs not called or beyond maximum number of lasers allowed?)", 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;
  }
  sick->setRangeInformation(laserData->mySickBits, laserData->mySickUnits);
  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;

}

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


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

AREXPORT 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();
}

---