ArRobot.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 <time.h>
#include <ctype.h>

#include "ArRobot.h"
#include "ArLog.h"
#include "ArDeviceConnection.h"
#include "ArTcpConnection.h"
#include "ArSerialConnection.h"
#include "ArLogFileConnection.h"
#include "ariaUtil.h"
#include "ArSocket.h"
#include "ArCommands.h"
#include "ArRobotTypes.h"
#include "ArSignalHandler.h"
#include "ArPriorityResolver.h"
#include "ArAction.h"
#include "ArRangeDevice.h"
#include "ArRobotConfigPacketReader.h"
#include "ariaInternal.h"

ArRobot::ArRobot(const char *name, bool obsolete, 
                          bool doSigHandle, bool normalInit,
                          bool addAriaExitCallback) :
  myMotorPacketCB(this, &ArRobot::processMotorPacket),
  myEncoderPacketCB(this, &ArRobot::processEncoderPacket),
  myIOPacketCB(this, &ArRobot::processIOPacket),
  myPacketHandlerCB(this, &ArRobot::packetHandler),
  myActionHandlerCB(this, &ArRobot::actionHandler),
  myStateReflectorCB(this, &ArRobot::stateReflector),
  myRobotLockerCB(this, &ArRobot::robotLocker),
  myRobotUnlockerCB(this, &ArRobot::robotUnlocker),
  myKeyHandlerExitCB(this, &ArRobot::keyHandlerExit),
  myGetCycleWarningTimeCB(this, &ArRobot::getCycleWarningTime),
  myGetNoTimeWarningThisCycleCB(this, &ArRobot::getNoTimeWarningThisCycle),
  myBatteryAverager(20),
  myRealBatteryAverager(20),
  myAriaExitCB(this, &ArRobot::ariaExitCallback)
{
  setName(name);
  myAriaExitCB.setName("ArRobotExit");
  myNoTimeWarningThisCycle = false;
  myGlobalPose.setPose(0, 0, 0);

  myParams = new ArRobotGeneric("");

  myPtz = NULL;
  myKeyHandler = NULL;
  myKeyHandlerCB = NULL;

  myConn = NULL;

  myOwnTheResolver = false;

  myBlockingConnectRun = false;
  myAsyncConnectFlag = false;

  myLogMovementSent = false;
  myLogMovementReceived = false;
  myLogVelocitiesReceived = false;

  myPacketsSentTracking = false;
  myPacketsReceivedTracking = false;
  myPacketsReceivedTrackingCount = false;
  myPacketsReceivedTrackingStarted.setToNow();

  myCycleTime = 100;
  myCycleWarningTime = 250;
  myConnectionCycleMultiplier = 2;
  myTimeoutTime = 8000;
  myStabilizingTime = 0;
  myCounter = 1;
  myResolver = NULL;
  myNumSonar = 0;

  myRequestedIOPackets = false;
  myRequestedEncoderPackets = false;
  myEncoderCorrectionCB = NULL;

  myCycleChained = true;

  myMoveDoneDist = 40;
  myHeadingDoneDiff = 3;

  myOrigRobotConfig = NULL;
  reset();
  if (normalInit)
    init();
  
  mySyncLoop.setRobot(this);

  if (doSigHandle)
    Aria::addRobot(this);
  if (addAriaExitCallback)
  {
    Aria::addExitCallback(&myAriaExitCB, 0);
    myAddedAriaExitCB = true;
  }
  else
  {
    myAddedAriaExitCB = false;
  }
}



ArRobot::~ArRobot()
{
  ArResolver::ActionMap::iterator it;

  stopRunning();
  delete mySyncTaskRoot;
  ArUtil::deleteSetPairs(mySonars.begin(), mySonars.end());
  Aria::delRobot(this);

  if (myKeyHandlerCB != NULL)
    delete myKeyHandlerCB;

  for (it = myActions.begin(); it != myActions.end(); ++it)
  {
    (*it).second->setRobot(NULL);
  }
}


void ArRobot::init(void)
{
  setUpPacketHandlers();
  setUpSyncList();
  myOwnTheResolver = true;
  myResolver = new ArPriorityResolver;
}

void ArRobot::run(bool stopRunIfNotConnected)
{
  if (mySyncLoop.getRunning())
  {
    ArLog::log(ArLog::Terse, 
               "The robot is already running, cannot run it again.");
    return;
  }
  mySyncLoop.setRunning(true);
  mySyncLoop.stopRunIfNotConnected(stopRunIfNotConnected);
  mySyncLoop.runInThisThread();
}

bool ArRobot::isRunning(void) const
{
  return mySyncLoop.getRunning();
}

void ArRobot::runAsync(bool stopRunIfNotConnected)
{
  if (mySyncLoop.getRunning())
  {
    ArLog::log(ArLog::Terse, 
               "The robot is already running, cannot run it again.");
    return;
  }
  mySyncLoop.stopRunIfNotConnected(stopRunIfNotConnected);
  // Joinable, but do NOT lower priority. The robot thread is the most
  // important one around.
  mySyncLoop.create(true, false);
}

void ArRobot::stopRunning(bool doDisconnect)
{
  if (myKeyHandler != NULL)
    myKeyHandler->restore();
  mySyncLoop.stopRunning();
  myBlockingConnectRun = false;
  if (doDisconnect && (isConnected() || myIsStabilizing))
  {
    waitForRunExit();
    disconnect();
  }
  wakeAllWaitingThreads();
}

void ArRobot::setUpSyncList(void)
{
  mySyncTaskRoot = new ArSyncTask("SyncTasks");
  mySyncTaskRoot->setWarningTimeCB(&myGetCycleWarningTimeCB);
  mySyncTaskRoot->setNoTimeWarningCB(&myGetNoTimeWarningThisCycleCB);
  mySyncTaskRoot->addNewLeaf("Packet Handler", 85, &myPacketHandlerCB);
  mySyncTaskRoot->addNewLeaf("Robot Locker", 70, &myRobotLockerCB);
  mySyncTaskRoot->addNewBranch("Sensor Interp", 65);
  mySyncTaskRoot->addNewLeaf("Action Handler", 55, &myActionHandlerCB);
  mySyncTaskRoot->addNewLeaf("State Reflector", 45, &myStateReflectorCB);
  mySyncTaskRoot->addNewBranch("User Tasks", 25);
  mySyncTaskRoot->addNewLeaf("Robot Unlocker", 20, &myRobotUnlockerCB);
}


void ArRobot::setUpPacketHandlers(void)
{
  addPacketHandler(&myMotorPacketCB, ArListPos::FIRST);
  addPacketHandler(&myEncoderPacketCB, ArListPos::LAST);
  addPacketHandler(&myIOPacketCB, ArListPos::LAST);
}

void ArRobot::reset(void)
{
  myInterpolation.reset();
  myEncoderInterpolation.reset();
  if (myOrigRobotConfig != NULL)
    delete myOrigRobotConfig;
  myOrigRobotConfig = new ArRobotConfigPacketReader(this, true);
  myFirstEncoderPose = true;
  //myEncoderPose.setPose(0, 0, 0);
  //myEncoderPoseTaken.setToNow();
  //myRawEncoderPose.setPose(0, 0, 0);
  //myGlobalPose.setPose(0, 0, 0);
  //myEncoderTransform.setTransform(myEncoderPose, myGlobalPose);
  myTransVelMax = 0;
  myTransAccel = 0;
  myTransDecel = 0;
  myRotVelMax = 0;
  myRotAccel = 0;
  myRotDecel = 0;

  myLeftVel = 0;
  myRightVel = 0;
  myBatteryVoltage = 13;
  myBatteryAverager.clear();
  myBatteryAverager.add(myBatteryVoltage);
  myStallValue = 0;
  myControl = 0;
  myFlags = 0;
  myCompass = 0;
  myAnalogPortSelected = 0;
  myAnalog = 0;
  myDigIn = 0;
  myDigOut = 0;
  myIOAnalogSize = 0;
  myIODigInSize = 0;
  myIODigOutSize = 0;
  myLastIOPacketReceivedTime.setSec(0);
  myLastIOPacketReceivedTime.setMSec(0);
  myVel = 0;
  myRotVel = 0;
  myChargeState = CHARGING_UNKNOWN;
  myLastX = 0;
  myLastY = 0;
  myLastTh = 0;
  mySentPulse = false;
  myIsConnected = false;
  myIsStabilizing = false;

  myTransVal = 0;
  myTransVal2 = 0;
  myTransType = TRANS_NONE;
  myLastTransVal = 0;
  myLastTransVal2 = 0;
  myLastTransType = TRANS_NONE;
  myLastTransSent.setToNow();
  myActionTransSet = false;
  myLastActionRotStopped = false;
  myLastActionRotHeading = false;

  myRotVal = 0;
  myLastRotVal = 0;
  myRotType = ROT_NONE;
  myLastRotType = ROT_NONE;
  myLastRotSent.setToNow();
  myActionRotSet = false;

  myLastPulseSent.setToNow();

  myDirectPrecedenceTime = 0;
  myStateReflectionRefreshTime = 500;

  myActionDesired.reset();

  myMotorPacCurrentCount = 0;
  myMotorPacCount = 0;
  myTimeLastMotorPacket = 0;

  mySonarPacCurrentCount = 0;
  mySonarPacCount = 0;
  myTimeLastSonarPacket = 0;

  myLeftEncoder = 0;
  myRightEncoder = 0;

  myWarnedAboutExtraSonar = false;
}

void ArRobot::setTransVelMax(double vel)
{
  if (vel > myAbsoluteMaxTransVel)
  {
    ArLog::log(ArLog::Terse, "ArRobot: transVelMax of %g is over the absolute max of %g, ignoring", vel, myAbsoluteMaxTransVel);
    return;
  }
  myTransVelMax = vel;
}

void ArRobot::setTransAccel(double acc)
{
  myTransAccel = acc;
}

void ArRobot::setTransDecel(double decel)
{
  myTransDecel = ArMath::fabs(decel);
}

void ArRobot::setRotVelMax(double vel)
{
  if (vel > myAbsoluteMaxRotVel)
  {
    ArLog::log(ArLog::Terse, "ArRobot: rotVelMax of %g is over the absolute max of %g, ignoring", vel, myAbsoluteMaxRotVel);
    return;
  }
  myRotVelMax = vel;
}

void ArRobot::setRotAccel(double acc)
{
  myRotAccel = acc;
}

void ArRobot::setRotDecel(double decel)
{
  myRotDecel = ArMath::fabs(decel);
}

double ArRobot::getTransVelMax(void) const
{
  return myTransVelMax;
}

double ArRobot::getTransAccel(void) const
{
  return myTransAccel;
}

double ArRobot::getTransDecel(void) const
{
  return myTransDecel;
}

double ArRobot::getRotVelMax(void) const
{
  return myRotVelMax;
}

double ArRobot::getRotAccel(void) const
{
  return myRotAccel;
}

double ArRobot::getRotDecel(void) const
{
  return myRotDecel;
}

void ArRobot::setDeviceConnection(ArDeviceConnection *connection)
{
  myConn = connection;
  mySender.setDeviceConnection(myConn);
  myReceiver.setDeviceConnection(myConn);
}

ArDeviceConnection *ArRobot::getDeviceConnection(void) const
{
  return myConn;
}

void ArRobot::setConnectionTimeoutTime(int mSecs)
{
  if (mSecs > 0)
    myTimeoutTime = mSecs;
  else
    myTimeoutTime = 0;
}

int ArRobot::getConnectionTimeoutTime(void) const
{
  return myTimeoutTime;
}

ArTime ArRobot::getLastPacketTime(void) const
{
  return myLastPacketReceivedTime;
}
bool ArRobot::asyncConnect(void)
{
  if (!mySyncLoop.getRunning() || isConnected())
    return false;
  
  myAsyncConnectFlag = true;
  myBlockingConnectRun = false;
  myAsyncConnectState = -1;
  return true;
}

bool ArRobot::blockingConnect(void)
{
  int ret = 0;

  lock();
  if (myIsConnected)
    disconnect();
  myBlockingConnectRun = true;
  myAsyncConnectState = -1;
  while ((ret = asyncConnectHandler(true)) == 0 && myBlockingConnectRun)
    ArUtil::sleep(ArMath::roundInt(getCycleTime() * .80));
  unlock();
  if (ret == 1)
    return true;
  else
    return false;
}

int ArRobot::asyncConnectHandler(bool tryHarderToConnect)
{
  int ret = 0;
  ArRobotPacket *packet;
  ArRobotPacket *tempPacket = NULL;
  char robotSubType[255];
  int i;
  int len;
  std::string str;
  ArTime endTime;
  int timeToWait;
  ArSerialConnection *serConn;

  endTime.setToNow();
  endTime.addMSec(getCycleTime()*myConnectionCycleMultiplier);
  
  myNoTimeWarningThisCycle = true;

  // if this is -1, then we're doing initialization stuff, then returning
  if (myAsyncConnectState == -1)
  {
    myAsyncConnectSentChangeBaud = false;
    myAsyncConnectNoPacketCount = 0;
    myAsyncConnectTimesTried = 0;
    reset();
    if (myConn == NULL)
    {
      ArLog::log(ArLog::Terse, "Cannot connect, no connection has been set.");
      failedConnect();
      return 2;
    }
    
    if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN) 
    {
      if (!myConn->openSimple())
      {
        /*str = myConn->getStatusMessage(myConn->getStatus());
          ArLog::log(ArLog::Terse, "Trying to connect to robot but connection not opened, it is %s", str.c_str());*/
        ArLog::log(ArLog::Terse, 
                   "Could not connect, because open on the device connection failed.");
        failedConnect();
        return 2;
      }
    }

    // check for log file connection: just return success
    if (dynamic_cast<ArLogFileConnection *>(myConn))
    {
      ArLogFileConnection *con = dynamic_cast<ArLogFileConnection *>(myConn);
      myRobotName = con->myName;
      myRobotType = con->myType;
      myRobotSubType = con->mySubtype;
      if (con->havePose)        // we know where to move
        moveTo(con->myPose);
      setCycleChained(false);   // don't process packets all at once
      madeConnection();
      finishedConnection();
      return 1;
    }


    if (dynamic_cast<ArSerialConnection *>(myConn))
    {
      myConn->write("WMS2\15", strlen("WMS2\15"));
    }
    // here we're flushing out all previously received packets
    while ( endTime.mSecTo() > 0 && myReceiver.receivePacket(0) != NULL);

    myAsyncConnectState = 0;
    return 0;
  }

  if (myAsyncConnectState >= 3)
  {
    bool handled;
    std::list<ArRetFunctor1<bool, ArRobotPacket *> *>::iterator it;
    while ((packet = myReceiver.receivePacket(0)) != NULL)
    {
      //printf("0x%x\n", packet->getID());
      for (handled = false, it = myPacketHandlerList.begin(); 
           it != myPacketHandlerList.end() && handled == false; 
           it++)
      {
        if ((*it) != NULL && (*it)->invokeR(packet)) 
          handled = true;
        else
          packet->resetRead();
      }
    }
  }

  if (myAsyncConnectState == 3)
  {
    //if (!myOrigRobotConfig->hasPacketBeenRequested())
    if (!myOrigRobotConfig->hasPacketArrived())
    {
      myOrigRobotConfig->requestPacket();
    }
    // if we've gotten our config packet or if we've timed out then
    // set our vel and acc/decel params and skip to the next part
    if (myOrigRobotConfig->hasPacketArrived() || 
        myAsyncStartedConnection.mSecSince() > 1000)
    {
      bool gotConfig;
      // if we have data from the robot use that
      if (myOrigRobotConfig->hasPacketArrived())
      {
        gotConfig = true;
        setAbsoluteMaxTransVel(myOrigRobotConfig->getTransVelTop());
        setAbsoluteMaxRotVel(myOrigRobotConfig->getRotVelTop());
        setTransVelMax(myOrigRobotConfig->getTransVelMax());
        setTransAccel(myOrigRobotConfig->getTransAccel());
        setTransDecel(myOrigRobotConfig->getTransDecel());
        setRotVelMax(myOrigRobotConfig->getRotVelMax());
        setRotAccel(myOrigRobotConfig->getRotAccel());
        setRotDecel(myOrigRobotConfig->getRotDecel());
      }
      // if our absolute maximums weren't set then set them
      else
      {
        gotConfig = false;
        setAbsoluteMaxTransVel(myParams->getAbsoluteMaxVelocity());
        setAbsoluteMaxRotVel(myParams->getAbsoluteMaxRotVelocity());
      }
      // okay we got in that data, now put over it any of the things
      // that we got from the robot parameter file... if we don't have
      // max vels from above or the parameters then use the absolutes
      // which we do have for everything
      if (ArMath::fabs(myParams->getTransVelMax()) > 1)
        setTransVelMax(myParams->getTransVelMax());
      else if (!gotConfig)
        setTransVelMax(myParams->getAbsoluteMaxVelocity());
      if (ArMath::fabs(myParams->getRotVelMax()) > 1)
        setRotVelMax(myParams->getRotVelMax());
      else if (!gotConfig)
        setRotVelMax(myParams->getAbsoluteMaxRotVelocity());
      if (ArMath::fabs(myParams->getTransAccel()) > 1)
        setTransAccel(myParams->getTransAccel());
      if (ArMath::fabs(myParams->getTransDecel()) > 1)
        setTransDecel(myParams->getTransDecel());
      if (ArMath::fabs(myParams->getRotAccel()) > 1)
        setRotAccel(myParams->getRotAccel());
      if (ArMath::fabs(myParams->getRotDecel()) > 1)
        setRotDecel(myParams->getRotDecel());
      myAsyncConnectState = 4;
    }
    else
      return 0;
  }
  // we want to see if we should switch baud
  if (myAsyncConnectState == 4)
  {
    serConn = dynamic_cast<ArSerialConnection *>(myConn);
    // if we didn't get a config packet or we can't change the baud or
    // we aren't using a serial port then don't switch the baud
    if (!myOrigRobotConfig->hasPacketArrived() || 
        !myOrigRobotConfig->getResetBaud() || 
        serConn == NULL ||
        myParams->getSwitchToBaudRate() == 0)
    {
      // if we're using a serial connection store our baud rate
      if (serConn != NULL)
        myAsyncConnectStartBaud = serConn->getBaud();
      myAsyncConnectState = 5;
    }
    // if we did get a packet and can change baud send the command
    else if (!myAsyncConnectSentChangeBaud)
    {
      // if we're using a serial connection store our baud rate
      if (serConn != NULL)
        myAsyncConnectStartBaud = serConn->getBaud();

      int baudNum = -1;
      
      // first suck up all the packets we have now
      while ((packet = myReceiver.receivePacket(0)) != NULL);
      if (myParams->getSwitchToBaudRate() == 9600)
        baudNum = 0;
      else if (myParams->getSwitchToBaudRate() == 19200)
        baudNum = 1;
      else if (myParams->getSwitchToBaudRate() == 38400)
        baudNum = 2;
      else if (myParams->getSwitchToBaudRate() == 57600)
        baudNum = 3;
      else if (myParams->getSwitchToBaudRate() == 115200)
        baudNum = 4;
      else
      {
        ArLog::log(ArLog::Normal, "Warning: SwitchToBaud is set to %d baud, ignoring.",
                   myParams->getSwitchToBaudRate());
        ArLog::log(ArLog::Normal, "\tGood bauds are 9600 19200 38400 56800 116200.");
        myAsyncConnectState = 5;
        baudNum = -1;
        return 0;
      }
      if (baudNum != -1)
      {
        // now switch it over
        comInt(ArCommands::HOSTBAUD, baudNum);
        ArUtil::sleep(10);
        myAsyncConnectSentChangeBaud = true;
        myAsyncConnectStartedChangeBaud.setToNow();
        serConn->setBaud(myParams->getSwitchToBaudRate());
        //serConn->setBaud(19200);
        ArUtil::sleep(10);
        com(0);
        return 0;
      }
    }
    // if we did send the command then wait and see if we get any packets back
    else
    {
      packet = myReceiver.receivePacket(100);
      com(0);
      // if we got any packet we're good
      if (packet != NULL)
      {
        myAsyncConnectState = 5;
      }
      // if we didn't get it and its been 500 ms then fail
      else if (myAsyncConnectStartedChangeBaud.mSecSince() > 900)
      {
        ArLog::log(ArLog::Verbose, "Controller did not switch to baud, reset to %d baud.", myAsyncConnectStartBaud);
        serConn->setBaud(myAsyncConnectStartBaud);
        myAsyncConnectState = 5;
      }
      else
        return 0;
    }    
  }

  if (myAsyncConnectState == 5)
  {
    if (!myIsStabilizing)
      startStabilization();
    if (myStabilizingTime == 0 || 
        myStartedStabilizing.mSecSince() > myStabilizingTime)
    {
      finishedConnection();
      return 1;
    }
    else
      return 0;
  }

  // here we've gone beyond the initialization, so read set a time limit,
  // read in one packet, then if its a bad packet type, read in all the 
  // packets there are to read... if its a good packet, continue with sequence
  myAsyncConnectTimesTried++;
  ArLog::log(ArLog::Normal, "Syncing %d", myAsyncConnectState);
  mySender.com(myAsyncConnectState);
  //  packet = myReceiver.receivePacket(endTime.mSecTo());
  packet = myReceiver.receivePacket(1000);
  
  if (packet != NULL) 
  {
    ret = packet->getID();
    //printf("Got a packet %d\n", ret);
        
    if (ret == 50)
    {
      ArLog::log(ArLog::Normal, "Attempting to close previous connection.");
      comInt(ArCommands::CLOSE,1);
      while (endTime.mSecTo() > 0)
      {
        timeToWait = endTime.mSecTo();
        if (timeToWait < 0)
          timeToWait = 0;
        tempPacket = myReceiver.receivePacket(timeToWait);
        if (tempPacket != NULL)
          ArLog::log(ArLog::Verbose, "Got in another packet!");
        if (tempPacket != NULL && tempPacket->getID() == 50)
          comInt(ArCommands::CLOSE,1);
      }
      myAsyncConnectState = 0;
    } 
    else if (ret == 255)
    {
      ArLog::log(ArLog::Normal, "Attempting to correct syncCount");
      /*
      while (endTime.mSecTo() > 0)
      {
        timeToWait = endTime.mSecTo();
        if (timeToWait < 0)
          timeToWait = 0;
        tempPacket = myReceiver.receivePacket(timeToWait);
        if (tempPacket != NULL)
          ArLog::log(ArLog::Verbose, "Got in another packet!");
      }
      */
      while ((tempPacket = myReceiver.receivePacket(0)) != NULL);

      if (tempPacket != NULL && tempPacket->getID() == 0)
        myAsyncConnectState = 1;
      else
        myAsyncConnectState = 0;
      return 0;
    } 
    else if  (ret != myAsyncConnectState++)
    {
      myAsyncConnectState = 0;
    }
  }
  else 
  {
    ArLog::log(ArLog::Normal, "No packet.");
    myAsyncConnectNoPacketCount++;
    if ((myAsyncConnectNoPacketCount > 5
         && myAsyncConnectNoPacketCount >= myAsyncConnectTimesTried)
        || (myAsyncConnectNoPacketCount > 10))
    {
      ArLog::log(ArLog::Terse, "Could not connect, no robot responding.");
      failedConnect();
      return 2;
    }

      /* This code to connect the radio modems should never really be needed 
         so is being removed for now
         if (myAsyncConnectNoPacketCount >= 4)
         {
         
         if (tryHarderToConnect && dynamic_cast<ArSerialConnection *>(myConn))
         {
         ArLog::log(ArLog::Normal, 
         "Radio modem may not connected, trying to connect it.");
         ArUtil::sleep(1000);
         myConn->write("|||\15", strlen("!!!\15"));
         ArUtil::sleep(60);
         myConn->write("WMN\15", strlen("WMN\15"));
         ArUtil::sleep(60);
         myConn->write("WMS2\15", strlen("WMS2\15"));
         ArUtil::sleep(1000);
         } 
         else if (dynamic_cast<ArSerialConnection *>(myConn))
         {
         ArLog::log(ArLog::Normal,
         "Radio modem may not connected, trying to connect it.");
         myConn->write("WMS2\15", strlen("WMS2\15"));
         ArUtil::sleep(60);
         }
         }
      */
    
    // If we get no response first we dump close commands at the thing
    // in different bauds (if its a serial connection)
    if (myAsyncConnectNoPacketCount == 2 && 
        myAsyncConnectNoPacketCount >= myAsyncConnectTimesTried &&
        (serConn = dynamic_cast<ArSerialConnection *>(myConn)) != NULL)
    {
      int origBaud;
      ArLog::log(ArLog::Normal, "Trying to close possible old connection");
      origBaud = serConn->getBaud();
      serConn->setBaud(9600);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(38400);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(115200);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(19200);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(57600);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(origBaud);
    }

    if (myAsyncConnectNoPacketCount > 3)
    {
      ArLog::log(ArLog::Normal,
                 " Robot may be connected but not open, trying to dislodge.");
      mySender.comInt(ArCommands::OPEN,1);
    }
    
    myAsyncConnectState = 0;
  }
  // if we've connected and have a packet get the connection
  // information from it
  if (myAsyncConnectState == 3 && packet != NULL) 
  {
    char nameBuf[512];
    ArLog::log(ArLog::Terse, "Connected to robot.");
    packet->bufToStr(nameBuf, 512);
    myRobotName = nameBuf;
    ArLog::log(ArLog::Normal, "Name: %s", myRobotName.c_str());
    packet->bufToStr(nameBuf, 512);
    myRobotType = nameBuf;
    ArLog::log(ArLog::Normal, "Type: %s", myRobotType.c_str());
    packet->bufToStr(nameBuf, 512);
    myRobotSubType = nameBuf;
    strcpy(robotSubType, myRobotSubType.c_str());
    len = strlen(robotSubType);
    for (i = 0; i < len; i++)
      robotSubType[i] = tolower(robotSubType[i]);
    myRobotSubType = robotSubType;
    ArLog::log(ArLog::Normal, "Subtype: %s", myRobotSubType.c_str());
    ArUtil::sleep(getCycleTime());
    mySender.comInt(ArCommands::OPEN,1);
    if (!madeConnection())
    {
      mySender.comInt(ArCommands::CLOSE,1);
      failedConnect();
      return 2;
    }

    // now we return off so we can handle the rest of connecting, if
    // you're just using this for your own connections you can skip
    // this part because its really connected now
    myAsyncStartedConnection.setToNow();
    return asyncConnectHandler(tryHarderToConnect);
  }
  if (myAsyncConnectTimesTried > 50)
  {
    failedConnect();
    return 2;
  }
  return 0;
}

bool ArRobot::loadParamFile(const char *file)
{
  if (myParams != NULL)
    delete myParams;

  myParams = new ArRobotGeneric("");
  if (!myParams->parseFile(file, false, true))
  {
    ArLog::log(ArLog::Normal, "ArRobot::loadParamFile: Could not find file '%s' to load.", file);
    return false;
  }
  processParamFile();
  ArLog::log(ArLog::Normal, "Loaded robot parameters from %s.", file);
  return true;
}

void ArRobot::processParamFile(void)
{
  for (int i = 0; i < myParams->getNumSonar(); ++i)
  {
    //printf("sonar %d %d %d %d\n", i, myParams->getSonarX(i),
    //myParams->getSonarY(i), myParams->getSonarTh(i));
    if (mySonars.find(i) == mySonars.end())
    {
      mySonars[i] = new ArSensorReading(myParams->getSonarX(i),
                                        myParams->getSonarY(i), 
                                        myParams->getSonarTh(i));
      mySonars[i]->setIgnoreThisReading(true);
    }
    else
    {
      mySonars[i]->resetSensorPosition(myParams->getSonarX(i),
                                      myParams->getSonarY(i), 
                                      myParams->getSonarTh(i));
      mySonars[i]->setIgnoreThisReading(true);
    }
    if ((i + 1) > myNumSonar)
      myNumSonar = i + 1;
  }
  //myRobotType = myParams->getClassName();
  //myRobotSubType = myParams->getSubClassName();
  myAbsoluteMaxTransVel = myParams->getAbsoluteMaxVelocity();
  myAbsoluteMaxRotVel = myParams->getAbsoluteMaxRotVelocity();
}


bool ArRobot::madeConnection(void)
{
  std::string subTypeParamName;
  std::string paramFileName;
  bool loadedSubTypeParam;
  bool loadedNameParam;
  bool hadDefault = true;
  
  if (myParams != NULL)
    delete myParams;
  
  // Find the robot parameters to load and get them into the structure we have
  if (ArUtil::strcmp(myRobotSubType, "p2dx") == 0)
    myParams = new ArRobotP2DX;
  else if (ArUtil::strcmp(myRobotSubType, "p2ce") == 0)
    myParams = new ArRobotP2CE;
  else if (ArUtil::strcmp(myRobotSubType, "p2de") == 0)
    myParams = new ArRobotP2DXe;
  else if (ArUtil::strcmp(myRobotSubType, "p2df") == 0)
    myParams = new ArRobotP2DF;
  else if (ArUtil::strcmp(myRobotSubType, "p2d8") == 0)
    myParams = new ArRobotP2D8;
  else if (ArUtil::strcmp(myRobotSubType, "amigo") == 0)
    myParams = new ArRobotAmigo;
  else if (ArUtil::strcmp(myRobotSubType, "amigo-sh") == 0)
    myParams = new ArRobotAmigoSh;
  else if (ArUtil::strcmp(myRobotSubType, "p2at") == 0)
    myParams = new ArRobotP2AT;
  else if (ArUtil::strcmp(myRobotSubType, "p2at8") == 0)
    myParams = new ArRobotP2AT8;
  else if (ArUtil::strcmp(myRobotSubType, "p2it") == 0)
    myParams = new ArRobotP2IT;
  else if (ArUtil::strcmp(myRobotSubType, "p2pb") == 0)
    myParams = new ArRobotP2PB;
  else if (ArUtil::strcmp(myRobotSubType, "p2pp") == 0)
    myParams = new ArRobotP2PP;
  else if (ArUtil::strcmp(myRobotSubType, "p3at") == 0)
    myParams = new ArRobotP3AT;
  else if (ArUtil::strcmp(myRobotSubType, "p3dx") == 0)
    myParams = new ArRobotP3DX;
  else if (ArUtil::strcmp(myRobotSubType, "perfpb") == 0)
    myParams = new ArRobotPerfPB;
  else if (ArUtil::strcmp(myRobotSubType, "pion1m") == 0)
    myParams = new ArRobotPion1M;
  else if (ArUtil::strcmp(myRobotSubType, "pion1x") == 0)
    myParams = new ArRobotPion1X;
  else if (ArUtil::strcmp(myRobotSubType, "psos1m") == 0)
    myParams = new ArRobotPsos1M;
  else if (ArUtil::strcmp(myRobotSubType, "psos43m") == 0)
    myParams = new ArRobotPsos43M;
  else if (ArUtil::strcmp(myRobotSubType, "psos1x") == 0)
    myParams = new ArRobotPsos1X;
  else if (ArUtil::strcmp(myRobotSubType, "pionat") == 0)
    myParams = new ArRobotPionAT;
  else if (ArUtil::strcmp(myRobotSubType, "mappr") == 0)
    myParams = new ArRobotMapper;
  else if (ArUtil::strcmp(myRobotSubType, "powerbot") == 0)
    myParams = new ArRobotPowerBot;
  else if (ArUtil::strcmp(myRobotSubType, "p2d8+") == 0)
    myParams = new ArRobotP2D8Plus;
  else if (ArUtil::strcmp(myRobotSubType, "p2at8+") == 0)
    myParams = new ArRobotP2AT8Plus;
  else if (ArUtil::strcmp(myRobotSubType, "perfpb+") == 0)
    myParams = new ArRobotPerfPBPlus;
  else if (ArUtil::strcmp(myRobotSubType, "p3dx-sh") == 0)
    myParams = new ArRobotP3DXSH;
  else if (ArUtil::strcmp(myRobotSubType, "p3at-sh") == 0)
    myParams = new ArRobotP3ATSH;
  else if (ArUtil::strcmp(myRobotSubType, "p3atiw-sh") == 0)
    myParams = new ArRobotP3ATIWSH;
  else if (ArUtil::strcmp(myRobotSubType, "patrolbot-sh") == 0)
    myParams = new ArRobotPatrolBotSH;
  else if (ArUtil::strcmp(myRobotSubType, "peoplebot-sh") == 0)
    myParams = new ArRobotPeopleBotSH;
  else if (ArUtil::strcmp(myRobotSubType, "powerbot-sh") == 0)
    myParams = new ArRobotPowerBotSH;
  else if (ArUtil::strcmp(myRobotSubType, "wheelchair-sh") == 0)
    myParams = new ArRobotWheelchairSH;
  else
  {
    hadDefault = false;
    myParams = new ArRobotGeneric(myRobotName.c_str());
  }
  
  // load up the param file for the subtype
  paramFileName = Aria::getDirectory();
  paramFileName += "params/";
  paramFileName += myRobotSubType;
  paramFileName += ".p";
  if ((loadedSubTypeParam = myParams->parseFile(paramFileName.c_str(), true, true)))
      ArLog::log(ArLog::Normal, 
                 "Loaded robot parameters from %s.p", 
                 myRobotSubType.c_str());
  /* If the above line was replaced with this one line
     paramFile->load(); 
     then the sonartest (and lots of other stuff probably) would break
  */
  // then the one for the particular name, if we can
  paramFileName = Aria::getDirectory();
  paramFileName += "params/";
  paramFileName += myRobotName;
  paramFileName += ".p";
  if ((loadedNameParam = myParams->parseFile(paramFileName.c_str(),
                                             true, true)))
  {
    if (loadedSubTypeParam)
      ArLog::log(ArLog::Normal, 
 "Loaded robot parameters from %s.p on top of %s.p robot parameters", 
                 myRobotName.c_str(), myRobotSubType.c_str());
    else
      ArLog::log(ArLog::Normal, "Loaded robot parameters from %s.p", 
                 myRobotName.c_str());
  }
   
  if (!loadedSubTypeParam && !loadedNameParam)
  {
    if (hadDefault)
      ArLog::log(ArLog::Normal, "Using default parameters for a %s robot", 
                 myRobotSubType.c_str());
    else
    {
      ArLog::log(ArLog::Terse, "Error: Have no parameters for this robot, bad configuration or out of date Aria");
      return false;
    }
  }

  processParamFile();

  if (myParams->getRequestIOPackets() || myRequestedIOPackets)
    comInt(ArCommands::IOREQUEST, 2);

  if(myParams->getRequestEncoderPackets() || myRequestedEncoderPackets)
    comInt(ArCommands::ENCODER, 2);

  return true;
}

void ArRobot::requestEncoderPackets(void)
{
  comInt(ArCommands::ENCODER, 2);
  myRequestedEncoderPackets = true;
}

void ArRobot::requestIOPackets(void)
{
  comInt(ArCommands::IOREQUEST, 2);
  myRequestedIOPackets = true;
}

void ArRobot::stopEncoderPackets(void)
{
  comInt(ArCommands::ENCODER, 0);
  myRequestedEncoderPackets = false;
}

void ArRobot::stopIOPackets(void)
{
  comInt(ArCommands::IOREQUEST, 0);
  myRequestedIOPackets = false;
}

void ArRobot::startStabilization(void)
{
  std::list<ArFunctor *>::iterator it;
  myIsStabilizing = true;
  myStartedStabilizing.setToNow();

  for (it = myStabilizingCBList.begin(); 
       it != myStabilizingCBList.end(); 
       it++)
    (*it)->invoke();

}

void ArRobot::finishedConnection(void)
{
  std::list<ArFunctor *>::iterator it;

  myIsStabilizing = false;
  myIsConnected = true;
  myAsyncConnectFlag = false;
  myBlockingConnectRun = false;
  
  for (it = myConnectCBList.begin(); it != myConnectCBList.end(); it++)
    (*it)->invoke();
  myLastPacketReceivedTime.setToNow();

  wakeAllConnWaitingThreads();
}

void ArRobot::failedConnect(void)
{
  std::list<ArFunctor *>::iterator it;  
  
  myAsyncConnectFlag = false;
  myBlockingConnectRun = false;
  ArLog::log(ArLog::Terse, "Failed to connect to robot.");
  myIsConnected = false;
  for (it = myFailedConnectCBList.begin(); 
       it != myFailedConnectCBList.end(); 
       it++)
    (*it)->invoke();

  if (myConn != NULL)
    myConn->close();
  wakeAllConnOrFailWaitingThreads();
}

bool ArRobot::disconnect(void)
{
  std::list<ArFunctor *>::iterator it;  
  bool ret;
  ArSerialConnection *serConn;

  if (!myIsConnected && !myIsStabilizing)
    return true;
  
  ArLog::log(ArLog::Terse, "Disconnecting from robot.");
  myNoTimeWarningThisCycle = true;
  myIsConnected = false;
  myIsStabilizing = false;
  if (myIsConnected)
  {
    for (it = myDisconnectNormallyCBList.begin(); 
         it != myDisconnectNormallyCBList.end(); 
         it++)
      (*it)->invoke();
  }
  ret = mySender.comInt(ArCommands::CLOSE, 1);
  ArUtil::sleep(1000);
  if (ret == true)
  {
    if (myConn != NULL)
    {
      ret = myConn->close();
      if ((serConn = dynamic_cast<ArSerialConnection *>(myConn)) != NULL)
        serConn->setBaud(myAsyncConnectStartBaud);
    } 
    else
      ret = false;
  } 
  else if (myConn != NULL)
    myConn->close();
    
  return ret;
}

void ArRobot::dropConnection(void)
{
  std::list<ArFunctor *>::iterator it;  

  if (!myIsConnected)
    return;

  ArLog::log(ArLog::Terse, "Lost connection to the robot because of error.");
  myIsConnected = false;
  for (it = myDisconnectOnErrorCBList.begin(); 
       it != myDisconnectOnErrorCBList.end(); 
       it++)
    (*it)->invoke();

  if (myConn != NULL)
    myConn->close();
  return;
}

void ArRobot::cancelConnection(void)
{
  //std::list<ArFunctor *>::iterator it;  

  ArLog::log(ArLog::Verbose, "Cancelled connection to the robot because of command.");
  myIsConnected = false;
  myNoTimeWarningThisCycle = true;
  myIsStabilizing = false;
  return;
}

void ArRobot::stop(void)
{
  comInt(ArCommands::VEL, 0);
  comInt(ArCommands::RVEL, 0);
  setVel(0);
  setRotVel(0);
  myLastActionTransVal = 0;
  myLastActionRotStopped = true;
  myLastActionRotHeading = false;
}

void ArRobot::setVel(double velocity)
{
  myTransType = TRANS_VEL;
  myTransVal = velocity;
  myTransVal2 = 0;
  myTransSetTime.setToNow();
}

void ArRobot::setVel2(double leftVelocity, double rightVelocity)
{
  myTransType = TRANS_VEL2;
  myTransVal = leftVelocity;
  myTransVal2 = rightVelocity;
  myRotType = ROT_IGNORE;
  myRotVal = 0;
  myTransSetTime.setToNow();
}

void ArRobot::move(double distance)
{
  myTransType = TRANS_DIST_NEW;
  myTransDistStart = getPose();
  myTransVal = distance;
  myTransVal2 = 0;
  myTransSetTime.setToNow();
}

bool ArRobot::isMoveDone(double delta)
{
  if (fabs(delta) < 0.001)
    delta = myMoveDoneDist;
  if (myTransType != TRANS_DIST && myTransType != TRANS_DIST_NEW)
    return true;                // no distance command operative
  if (myTransDistStart.findDistanceTo(getPose()) <
      fabs(myTransVal) - delta)
    return false;
  return true;
}


bool ArRobot::isHeadingDone(double delta) const
{
  if (fabs(delta) < 0.001)
    delta = myHeadingDoneDiff;
  if (myRotType != ROT_HEADING)
    return true;                // no heading command operative
  if(fabs(ArMath::subAngle(getTh(), myRotVal)) > delta)
    return false;
  return true;
}



void ArRobot::setHeading(double heading)
{
  myRotVal = heading;
  myRotType = ROT_HEADING;
  myRotSetTime.setToNow();
  if (myTransType == TRANS_VEL2)
  {
    myTransType = TRANS_IGNORE;
    myTransVal = 0;
    myTransVal2 = 0;
  }
}

void ArRobot::setRotVel(double velocity)
{
  myRotVal = velocity;
  myRotType = ROT_VEL;
  myRotSetTime.setToNow();
  if (myTransType == TRANS_VEL2)
  {
    myTransType = TRANS_IGNORE;
    myTransVal = 0;
    myTransVal2 = 0;
  }
}

void ArRobot::setDeltaHeading(double deltaHeading)
{
  myRotVal = ArMath::addAngle(getTh(), deltaHeading);
  myRotType = ROT_HEADING;
  myRotSetTime.setToNow();
  if (myTransType == TRANS_VEL2)
  {
    myTransType = TRANS_IGNORE;
    myTransVal = 0;
    myTransVal2 = 0;
  }
}

bool ArRobot::setAbsoluteMaxTransVel(double maxVel)
{
  if (maxVel < 0)
    return false;
  myAbsoluteMaxTransVel = maxVel;
  if (getTransVelMax() > myAbsoluteMaxTransVel)
    setTransVelMax(myAbsoluteMaxTransVel);
  return true;
}

bool ArRobot::setAbsoluteMaxRotVel(double maxVel)
{
  if (maxVel < 0)
    return false;
  myAbsoluteMaxRotVel = maxVel;
  if (getRotVelMax() > myAbsoluteMaxRotVel)
    setRotVelMax(myAbsoluteMaxRotVel);
  return true;
}

int ArRobot::getIOAnalog(int num) const
{
  if (num <= getIOAnalogSize())
    return myIOAnalog[num];
  else
    return 0;
}

double ArRobot::getIOAnalogVoltage(int num) const
{
  if (num <= getIOAnalogSize())
  {
    return (myIOAnalog[num] & 0xfff) * .0048828;
  }
  else
    return 0;
}

unsigned char ArRobot::getIODigIn(int num) const
{
  if (num <= getIODigInSize())
    return myIODigIn[num];
  else
    return (unsigned char) 0;
}

unsigned char  ArRobot::getIODigOut(int num) const
{
  if (num <= getIODigOutSize())
    return myIODigOut[num];
  else
    return (unsigned char) 0;
}

const ArRobotParams *ArRobot::getRobotParams(void) const
{
  return myParams;
}

const ArRobotConfigPacketReader *ArRobot::getOrigRobotConfig(void) const
{
  return myOrigRobotConfig;
}
  
void ArRobot::addPacketHandler(
        ArRetFunctor1<bool, ArRobotPacket *> *functor, 
        ArListPos::Pos position) 
{
  if (position == ArListPos::FIRST)
    myPacketHandlerList.push_front(functor);
  else if (position == ArListPos::LAST)
    myPacketHandlerList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, "ArRobot::addPacketHandler: Invalid position.");

}

void ArRobot::remPacketHandler(
        ArRetFunctor1<bool, ArRobotPacket *> *functor)
{
  myPacketHandlerList.remove(functor);
}

void ArRobot::addConnectCB(ArFunctor *functor, 
                                    ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myConnectCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myConnectCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
               "ArRobot::addConnectCallback: Invalid position.");
}

void ArRobot::remConnectCB(ArFunctor *functor)
{
  myConnectCBList.remove(functor);
}


void ArRobot::addFailedConnectCB(ArFunctor *functor, 
                                          ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myFailedConnectCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myFailedConnectCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
               "ArRobot::addFailedCallback: Invalid position.");
}

void ArRobot::remFailedConnectCB(ArFunctor *functor)
{
  myFailedConnectCBList.remove(functor);
}

void ArRobot::addDisconnectNormallyCB(ArFunctor *functor, 
                                               ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myDisconnectNormallyCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myDisconnectNormallyCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
               "ArRobot::addDisconnectNormallyCB: Invalid position.");
}

void ArRobot::remDisconnectNormallyCB(ArFunctor *functor)
{
  myDisconnectNormallyCBList.remove(functor);
}

void ArRobot::addDisconnectOnErrorCB(ArFunctor *functor, 
                                              ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myDisconnectOnErrorCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myDisconnectOnErrorCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
               "ArRobot::addDisconnectOnErrorCB: Invalid position");
}

void ArRobot::remDisconnectOnErrorCB(ArFunctor *functor)
{
  myDisconnectOnErrorCBList.remove(functor);
}

void ArRobot::addRunExitCB(ArFunctor *functor,
                                    ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myRunExitCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myRunExitCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, "ArRobot::addRunExitCB: Invalid position");
}

void ArRobot::remRunExitCB(ArFunctor *functor)
{
  myRunExitCBList.remove(functor);
}

void ArRobot::addStabilizingCB(ArFunctor *functor, 
                                       ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myStabilizingCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myStabilizingCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
               "ArRobot::addConnectCallback: Invalid position.");
}

void ArRobot::remStabilizingCB(ArFunctor *functor)
{
  myStabilizingCBList.remove(functor);
}


std::list<ArFunctor *> * ArRobot::getRunExitListCopy()
{
  return(new std::list<ArFunctor *>(myRunExitCBList));
}

ArRobot::WaitState ArRobot::waitForConnect(unsigned int msecs)
{
  int ret;

  if (isConnected())
    return(WAIT_CONNECTED);

  if (msecs == 0)
    ret=myConnectCond.wait();
  else
    ret=myConnectCond.timedWait(msecs);

  if (ret == ArCondition::STATUS_WAIT_INTR)
    return(WAIT_INTR);
  else if (ret == ArCondition::STATUS_WAIT_TIMEDOUT)
    return(WAIT_TIMEDOUT);
  else if (ret == 0)
    return(WAIT_CONNECTED);
  else
    return(WAIT_FAIL);
}

ArRobot::WaitState
ArRobot::waitForConnectOrConnFail(unsigned int msecs)
{
  int ret;

  if (isConnected())
    return(WAIT_CONNECTED);

  if (msecs == 0)
    ret=myConnOrFailCond.wait();
  else
    ret=myConnOrFailCond.timedWait(msecs);

  if (ret == ArCondition::STATUS_WAIT_INTR)
    return(WAIT_INTR);
  else if (ret == ArCondition::STATUS_WAIT_TIMEDOUT)
    return(WAIT_TIMEDOUT);
  else if (ret == 0)
  {
    if (isConnected())
      return(WAIT_CONNECTED);
    else
      return(WAIT_FAILED_CONN);
  }
  else
    return(WAIT_FAIL);
}

ArRobot::WaitState ArRobot::waitForRunExit(unsigned int msecs)
{
  int ret;

  if (!isRunning())
    return(WAIT_RUN_EXIT);

  if (msecs == 0)
    ret=myRunExitCond.wait();
  else
    ret=myRunExitCond.timedWait(msecs);

  if (ret == ArCondition::STATUS_WAIT_INTR)
    return(WAIT_INTR);
  else if (ret == ArCondition::STATUS_WAIT_TIMEDOUT)
    return(WAIT_TIMEDOUT);
  else if (ret == 0)
    return(WAIT_RUN_EXIT);
  else
    return(WAIT_FAIL);
}

void ArRobot::wakeAllWaitingThreads()
{
  wakeAllConnWaitingThreads();
  wakeAllRunExitWaitingThreads();
}

void ArRobot::wakeAllConnWaitingThreads()
{
  myConnectCond.broadcast();
  myConnOrFailCond.broadcast();
}

void ArRobot::wakeAllConnOrFailWaitingThreads()
{
  myConnOrFailCond.broadcast();
}

void ArRobot::wakeAllRunExitWaitingThreads()
{
  myRunExitCond.broadcast();
}

ArSyncTask *ArRobot::getSyncTaskRoot(void)
{
  return mySyncTaskRoot;
}

bool ArRobot::addUserTask(const char *name, int position, 
                                      ArFunctor *functor, 
                                      ArTaskState::State *state)
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return false;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return false;
  
  proc->addNewLeaf(name, position, functor, state);
  return true;  
}

void ArRobot::remUserTask(const char *name)
{
  ArSyncTask *proc;
  ArSyncTask *userProc;

  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return;
  
  userProc = proc->findNonRecursive(name);
  if (userProc == NULL)
    return;

  
  delete userProc;

}

void ArRobot::remUserTask(ArFunctor *functor)
{
  ArSyncTask *proc;
  ArSyncTask *userProc;

  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return;
  
  
  userProc = proc->findNonRecursive(functor);
  if (userProc == NULL)
    return;

  
  delete userProc;

}

bool ArRobot::addSensorInterpTask(const char *name, int position, 
                                              ArFunctor *functor,
                                              ArTaskState::State *state)
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return false;

  proc = mySyncTaskRoot->findNonRecursive("Sensor Interp");
  if (proc == NULL)
    return false;
  
  proc->addNewLeaf(name, position, functor, state);
  return true;  
}

void ArRobot::remSensorInterpTask(const char *name)
{
  ArSyncTask *proc;
  ArSyncTask *sensorInterpProc;

  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("Sensor Interp");
  if (proc == NULL)
    return;
  
  sensorInterpProc = proc->findNonRecursive(name);
  if (sensorInterpProc == NULL)
    return;

  
  delete sensorInterpProc;

}

void ArRobot::remSensorInterpTask(ArFunctor *functor)
{
  ArSyncTask *proc;
  ArSyncTask *sensorInterpProc;

  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("Sensor Interp");
  if (proc == NULL)
    return;
  
  
  sensorInterpProc = proc->findNonRecursive(functor);
  if (sensorInterpProc == NULL)
    return;

  
  delete sensorInterpProc;

}

void ArRobot::logUserTasks(void) const
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return;

  proc->log();
}

void ArRobot::logAllTasks(void) const
{
  if (mySyncTaskRoot != NULL)
    mySyncTaskRoot->log();
}

ArSyncTask *ArRobot::findUserTask(const char *name)
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return NULL;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return NULL;

  return proc->find(name);
}

ArSyncTask *ArRobot::findUserTask(ArFunctor *functor)
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return NULL;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return NULL;

  return proc->find(functor);
}

ArSyncTask *ArRobot::findTask(const char *name)
{
  if (mySyncTaskRoot != NULL)
    return mySyncTaskRoot->find(name);
  else
    return NULL;

}

ArSyncTask *ArRobot::findTask(ArFunctor *functor)
{
  if (mySyncTaskRoot != NULL)
    return mySyncTaskRoot->find(functor);
  else
    return NULL;

}

bool ArRobot::addAction(ArAction *action, int priority)
{
  if (action == NULL)
  {
    ArLog::log(ArLog::Terse, 
      "ArRobot::addAction: an attempt was made to add a NULL action pointer");
    return false;
  }
  
  action->setRobot(this);
  myActions.insert(std::pair<int, ArAction *>(priority, action));
  return true;
}

bool ArRobot::remAction(const char *actionName)
{
  ArResolver::ActionMap::iterator it;
  ArAction *act;

  for (it = myActions.begin(); it != myActions.end(); ++it)
  {
    act = (*it).second;
    if (strcmp(actionName, act->getName()) == 0)
      break;
  }
  if (it != myActions.end())
  {
    myActions.erase(it);
    return true;
  }
  return false;

}

bool ArRobot::remAction(ArAction *action)
{
  ArResolver::ActionMap::iterator it;
  ArAction *act;

  for (it = myActions.begin(); it != myActions.end(); ++it)
  {
    act = (*it).second;
    if (act == action)
      break;
  }
  if (it != myActions.end())
  {
    myActions.erase(it);
    return true;
  }
  return false;

}


ArAction *ArRobot::findAction(const char *actionName)
{
  ArResolver::ActionMap::reverse_iterator it;
  ArAction *act;
  
  for (it = myActions.rbegin(); it != myActions.rend(); ++it)
  {
    act = (*it).second;
    if (strcmp(actionName, act->getName()) == 0)
      return act;
  }
  return NULL;
}

ArResolver::ActionMap *ArRobot::getActionMap(void)
{
  return &myActions;
}

void ArRobot::deactivateActions(void)
{
  ArResolver::ActionMap *am;
  ArResolver::ActionMap::iterator amit;
  
  am = getActionMap();
  if (am == NULL)
  {
    ArLog::log(ArLog::Terse, 
            "ArRobot::deactivateActions: NULL action map... failed.");
    return;
  }
  for (amit = am->begin(); amit != am->end(); amit++)
    (*amit).second->deactivate();
  

}


void ArRobot::logActions(void) const
{
  ArResolver::ActionMap::const_reverse_iterator it;
  int lastPriority;
  bool first = true;

  ArLog::log(ArLog::Terse, "The action list (%d of them):\n", myActions.size());
  for (it = myActions.rbegin(); it != myActions.rend(); ++it)
  {
    if (first || lastPriority != (*it).first) 
    {
      ArLog::log(ArLog::Terse, "Priority %d:", (*it).first);
      first = false;
      lastPriority = (*it).first;
    }
    (*it).second->log(false);
  }
    
}

ArResolver *ArRobot::getResolver(void)
{
  return myResolver;
}

void ArRobot::setResolver(ArResolver *resolver)
{
  if (myOwnTheResolver)
  {
    delete myResolver;
    myResolver = NULL;
  }

  myResolver = resolver;
}

void ArRobot::stateReflector(void)
{
  short transVal;
  short transVal2;
  short maxVel;
  short maxNegVel;
  double maxTransVel;
  double maxNegTransVel;
  double maxRotVel;
  double transAccel;
  double transDecel;
  short rotVal;
  double rotAccel;
  double rotDecel;
  bool rotStopped = false;
  bool rotHeading = false;
  double encTh;
  double rawTh;


  if (!myIsConnected)
    return;

  // if this is true actions can't go
  if ((myTransType != TRANS_NONE && myDirectPrecedenceTime == 0) ||
      (myTransType != TRANS_NONE && myDirectPrecedenceTime != 0 && 
       myTransSetTime.mSecSince() < myDirectPrecedenceTime))
  {
    myActionTransSet = false;
    transVal = ArMath::roundShort(myTransVal);
    transVal2 = 0;

    if (hasSettableVelMaxes() && 
        ArMath::fabs(myLastSentTransVelMax - myTransVelMax) >= 1)
    {
      comInt(ArCommands::SETV,
             ArMath::roundShort(myTransVelMax));
      myLastSentTransVelMax = myTransVelMax;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "Non-action trans max vel of %d", 
                   ArMath::roundShort(myTransVelMax));
    }

    if (hasSettableAccsDecs() && ArMath::fabs(myTransAccel) > 1 && 
        ArMath::fabs(myLastSentTransAccel - myTransAccel) >= 1)
    {
      comInt(ArCommands::SETA,
             ArMath::roundShort(myTransAccel));
      myLastSentTransAccel = myTransAccel;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "Non-action trans accel of %d", 
                   ArMath::roundShort(myTransAccel));
    }

    if (hasSettableAccsDecs() && ArMath::fabs(myTransDecel) > 1 &&
        ArMath::fabs(myLastSentTransDecel - myTransDecel) >= 1)
    {
      comInt(ArCommands::SETA,
             -ArMath::roundShort(myTransDecel));
      myLastSentTransDecel = myTransDecel;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "Non-action trans decel of %d", 
                   -ArMath::roundShort(myTransDecel));
    }

    if (myTransType == TRANS_VEL)
    {
      maxVel = ArMath::roundShort(myTransVelMax);
      if (transVal > maxVel)
        transVal = maxVel;
      if (transVal < -maxVel)
        transVal = -maxVel;
      if (myLastTransVal != transVal || myLastTransType != myTransType ||
          (myLastTransSent.mSecSince() >= myStateReflectionRefreshTime))
      {
        comInt(ArCommands::VEL, ArMath::roundShort(transVal));
        myLastTransSent.setToNow();
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "Non-action trans vel of %d", 
                     ArMath::roundShort(transVal));
        //printf("Sent command vel!\n");
      }
    }
    else if (myTransType == TRANS_VEL2)
    {
      if (ArMath::roundShort(myTransVal/myParams->getVel2Divisor()) > 128)
        transVal = 128;
      else if (ArMath::roundShort(myTransVal/myParams->getVel2Divisor()) < -128)
        transVal = -128;
      else 
        transVal = ArMath::roundShort(myTransVal/myParams->getVel2Divisor());
      if (ArMath::roundShort(myTransVal2/myParams->getVel2Divisor()) > 128)
        transVal2 = 128;
      else if (ArMath::roundShort(myTransVal2/myParams->getVel2Divisor()) < -128)
        transVal2 = -128;
      else 
        transVal2 = ArMath::roundShort(myTransVal2/myParams->getVel2Divisor());
      if (myLastTransVal != transVal || myLastTransVal2 != transVal2 || 
          myLastTransType != myTransType ||
          (myLastTransSent.mSecSince() >= myStateReflectionRefreshTime))
      {
        com2Bytes(ArCommands::VEL2, transVal, transVal2);
        myLastTransSent.setToNow();
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "Non-action vel2 of %d %d", 
                     transVal, transVal2);
        //printf("Sent command vel2!\n");
      }
    }
    else if (myTransType == TRANS_DIST_NEW || myTransType == TRANS_DIST)
    {
      // if the robot doesn't have its own distance command
      if (!myParams->hasMoveCommand())
      {
        double distGone;
        double distToGo;
        double vel;

        myTransType = TRANS_DIST;
        distGone = myTransDistStart.findDistanceTo(getPose());
        distToGo = fabs(fabs(myTransVal) - distGone);
        if (distGone > fabs(myTransVal) || 
            (distToGo < 10 && fabs(getVel()) < 30))
        {
          comInt(ArCommands::VEL, 0);
          myTransType = TRANS_VEL;
          myTransVal = 0;
        }
        vel = sqrt(distToGo * 200 * 2);
        if (vel > getTransVelMax())
          vel = getTransVelMax();
        if (myTransVal < 0)
          vel *= -1;
        comInt(ArCommands::VEL, ArMath::roundShort(vel));
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "Non-action move-helper of %d", 
                     ArMath::roundShort(vel));
      }
      else if (myParams->hasMoveCommand() && myTransType == TRANS_DIST_NEW) 
      {
        comInt(ArCommands::MOVE, transVal);
        myLastTransSent.setToNow();
        myTransType = TRANS_DIST;
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "Non-action move of %d", 
                     transVal);
      }
      else if (myTransType == TRANS_DIST && 
          (myLastTransSent.mSecSince() >= myStateReflectionRefreshTime))
      {
        com(0);
        myLastPulseSent.setToNow();
        myLastTransSent.setToNow();
        //printf("Sent pulse for dist!\n");
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "Non-action pulse for dist");
      }
      //printf("Sent command move!\n");
    }
    else if (myTransType == TRANS_IGNORE)
    {
      //printf("No trans command sent\n");
    }
    else
      ArLog::log(ArLog::Terse, 
                 "ArRobot::stateReflector: Invalid translational type %d.",
                 myTransType);
    myLastTransVal = transVal;
    myLastTransVal2 = transVal2;
    myLastTransType = myTransType;
  }
  else // if actions can go
  {
    if (hasSettableVelMaxes() && 
        ArMath::fabs(myLastSentTransVelMax - myTransVelMax) >= 1)
    {
      comInt(ArCommands::SETV,
             ArMath::roundShort(myTransVelMax));
      myLastSentTransVelMax = myTransVelMax;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "Action-but-robot trans max vel of %d", 
                   ArMath::roundShort(myTransVelMax));
    }
    
    // first we'll handle all of the accel decel things
    if (myActionDesired.getTransAccelStrength() >= 
        ArActionDesired::MIN_STRENGTH)
    {
      transAccel = ArMath::roundShort(myActionDesired.getTransAccel());
      if (hasSettableAccsDecs() && ArMath::fabs(transAccel) > 1 &&
          ArMath::fabs(myLastSentTransAccel - transAccel) >= 1)
      {
        comInt(ArCommands::SETA,
               ArMath::roundShort(transAccel));
        myLastSentTransAccel = transAccel;
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "Action trans accel of %d", 
                     ArMath::roundShort(transAccel));
      }
    }
    else if (hasSettableAccsDecs() && ArMath::fabs(myTransAccel) > 1 && 
        ArMath::fabs(myLastSentTransAccel - myTransAccel) >= 1)
    {
      comInt(ArCommands::SETA,
             ArMath::roundShort(myTransAccel));
      myLastSentTransAccel = myTransAccel;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "Action-but-robot trans accel of %d", 
                   ArMath::roundShort(myTransAccel));
    }

    if (myActionDesired.getTransDecelStrength() >=
        ArActionDesired::MIN_STRENGTH)
    {
      transDecel = ArMath::roundShort(myActionDesired.getTransDecel());
      if (hasSettableAccsDecs() && ArMath::fabs(transDecel) > 1 &&
          ArMath::fabs(myLastSentTransDecel - transDecel) >= 1)
      {
        comInt(ArCommands::SETA,
               -ArMath::roundShort(transDecel));
        myLastSentTransDecel = transDecel;
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "Action trans decel of %d", 
                     -ArMath::roundShort(transDecel));
      }
    }
    else if (hasSettableAccsDecs() && ArMath::fabs(myTransDecel) > 1 &&
        ArMath::fabs(myLastSentTransDecel - myTransDecel) >= 1)
    {
      comInt(ArCommands::SETA,
             -ArMath::roundShort(myTransDecel));
      myLastSentTransDecel = myTransDecel;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "Action-but-robot trans decel of %d", 
                   -ArMath::roundShort(myTransDecel));
    }

    if (myActionDesired.getMaxVelStrength() >= ArActionDesired::MIN_STRENGTH)
    {
      maxTransVel = myActionDesired.getMaxVel();
      if (maxTransVel > myTransVelMax)
        maxTransVel = myTransVelMax;
    }
    else
      maxTransVel = myTransVelMax;

    if (myActionDesired.getMaxNegVelStrength() >= 
        ArActionDesired::MIN_STRENGTH)
    {
      maxNegTransVel = -ArMath::fabs(myActionDesired.getMaxNegVel());
      if (maxNegTransVel < -myTransVelMax)
        maxNegTransVel = -myTransVelMax;
    }
    else
      maxNegTransVel = -myTransVelMax;
    
    if (myActionDesired.getVelStrength() >= ArActionDesired::MIN_STRENGTH)
    {
      transVal = ArMath::roundShort(myActionDesired.getVel());
      myActionTransSet = true;
    }
    else
    {
      transVal = myLastActionTransVal;
    }

    maxVel = ArMath::roundShort(maxTransVel);
    maxNegVel = ArMath::roundShort(maxNegTransVel);
    if (transVal > maxVel)
      transVal = maxVel;
    if (transVal < maxNegVel)
      transVal = maxNegVel;

    if (myActionTransSet && 
        (myLastTransSent.mSecSince() >= myStateReflectionRefreshTime ||
         transVal != myLastActionTransVal))                          
    {
      comInt(ArCommands::VEL, ArMath::roundShort(transVal));
      myLastTransSent.setToNow();
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "Action trans vel of %d", 
                   ArMath::roundShort(transVal));      
    }
    myLastActionTransVal = transVal;
  }

  // if this is true actions can't go
  if ((myRotType != ROT_NONE && myDirectPrecedenceTime == 0) ||
      (myRotType != ROT_NONE && myDirectPrecedenceTime != 0 && 
       myRotSetTime.mSecSince() < myDirectPrecedenceTime))
  {
    if (hasSettableVelMaxes() && 
        ArMath::fabs(myLastSentRotVelMax - myRotVelMax) >= 1)
    {
      comInt(ArCommands::SETRV,
             ArMath::roundShort(myRotVelMax));
      myLastSentRotVelMax = myRotVelMax;
    }
    if (hasSettableAccsDecs() && ArMath::fabs(myRotAccel) > 1 &&
        ArMath::fabs(myLastSentRotAccel - myRotAccel) >= 1)
    {
      comInt(ArCommands::SETRA,
             ArMath::roundShort(myRotAccel));
      myLastSentRotAccel = myRotAccel;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "%25sNon-action rot accel of %d", "",
                   ArMath::roundShort(myRotAccel));      
    }
    if (hasSettableAccsDecs() && ArMath::fabs(myRotDecel) > 1 &&
        ArMath::fabs(myLastSentRotDecel - myRotDecel) >= 1)
    {
      comInt(ArCommands::SETRA,
             -ArMath::roundShort(myRotDecel));
      myLastSentRotDecel = myRotDecel;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "%25sNon-action rot decel of %d", "",
                   -ArMath::roundShort(myRotDecel));      
    }

    myActionRotSet = false;
    rotVal = ArMath::roundShort(myRotVal);
    if (myRotType == ROT_HEADING)
    {
      encTh = ArMath::subAngle(myRotVal, myEncoderTransform.getTh());
      rawTh = ArMath::addAngle(encTh, 
                               ArMath::subAngle(myRawEncoderPose.getTh(),
                                                myEncoderPose.getTh()));
      rotVal = ArMath::roundShort(rawTh);

      // if we were using a different heading type, a different heading
      // our heading doesn't match what we want it to be, or its been a while
      // since we sent the heading, send it again
      if (myLastRotVal != rotVal || myLastRotType != myRotType ||
          fabs(ArMath::subAngle(rotVal, getTh())) > 1 ||
          (myLastRotSent.mSecSince() >= myStateReflectionRefreshTime))
      {
        comInt(ArCommands::HEAD, rotVal);
        myLastRotSent.setToNow();
        //printf("sent command, heading\n");
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, 
             "%25sNon-action rot heading of %d (encoder %d, raw %d)",
                     "",
                     ArMath::roundShort(myRotVal),
                     ArMath::roundShort(encTh),
                     ArMath::roundShort(rotVal));
      }
    }
    else if (myRotType == ROT_VEL)
    {
      if (myLastRotVal != rotVal || myLastRotType != myRotType ||
          (myLastRotSent.mSecSince() >= myStateReflectionRefreshTime))
      {
        comInt(ArCommands::RVEL, rotVal);
        myLastRotSent.setToNow();
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "%25sNon-action rot vel of %d", "",
                     rotVal);      
        //printf("sent command, rot vel\n");
      }
    }
    else if (myRotType == ROT_IGNORE)
    {
      //printf("Not sending any command, rot is set to ignore");
    }
    else
      ArLog::log(ArLog::Terse, 
                 "ArRobot::stateReflector: Invalid rotation type %d.",
                 myRotType);
    myLastRotVal = rotVal;
    myLastRotType = myRotType;
  }
  else // if the action can fire
  {
    // first we'll handle all of the accel decel things
    if (myActionDesired.getMaxRotVelStrength() >=
        ArActionDesired::MIN_STRENGTH)
    {
      maxRotVel = myActionDesired.getMaxRotVel();
      if (maxRotVel > myAbsoluteMaxRotVel)
        maxRotVel = myAbsoluteMaxRotVel;
      maxRotVel = ArMath::roundShort(maxRotVel);
      if (ArMath::fabs(myLastSentRotVelMax - maxRotVel) >= 1)
      {
        myLastSentRotVelMax = maxRotVel;
        comInt(ArCommands::SETRV, 
               ArMath::roundShort(maxRotVel));
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "%25sAction rot vel max of %d", "",
                     ArMath::roundShort(maxRotVel));
      }
    }
    else if (hasSettableVelMaxes() && 
             ArMath::fabs(myLastSentRotVelMax - myRotVelMax) >= 1)
    {
      comInt(ArCommands::SETRV,
             ArMath::roundShort(myRotVelMax));
      myLastSentRotVelMax = myRotVelMax;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, 
                   "%25sAction-but-robot rot vel max of %d", 
                   "",  ArMath::roundShort(myRotVelMax));      
    }

    if (myActionDesired.getRotAccelStrength() >= ArActionDesired::MIN_STRENGTH)
    {
      rotAccel = ArMath::roundShort(myActionDesired.getRotAccel());
      if (ArMath::fabs(myLastSentRotAccel - rotAccel) >= 1)
      {
        comInt(ArCommands::SETRA,
               ArMath::roundShort(rotAccel));
        myLastSentRotAccel = rotAccel;
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "%25sAction rot accel of %d", "",
                     ArMath::roundShort(rotAccel));
      }
    }
    else if (hasSettableAccsDecs() && ArMath::fabs(myRotAccel) > 1 &&
        ArMath::fabs(myLastSentRotAccel - myRotAccel) >= 1)
    {
      comInt(ArCommands::SETRA,
             ArMath::roundShort(myRotAccel));
      myLastSentRotAccel = myRotAccel;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "%25sAction-but-robot rot accel of %d", 
                   "", ArMath::roundShort(myRotAccel));      
    }

    if (myActionDesired.getRotDecelStrength() >= ArActionDesired::MIN_STRENGTH)
    {
      rotDecel = ArMath::roundShort(myActionDesired.getRotDecel());
      if (ArMath::fabs(myLastSentRotDecel - rotDecel) >= 1)
      {
        comInt(ArCommands::SETRA,
               -ArMath::roundShort(rotDecel));
        myLastSentRotDecel = rotDecel;
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "%25sAction rot decel of %d", "",
                     -ArMath::roundShort(rotDecel));
      }
    }
    else if (hasSettableAccsDecs() && ArMath::fabs(myRotDecel) > 1 &&
        ArMath::fabs(myLastSentRotDecel - myRotDecel) >= 1)
    {
      comInt(ArCommands::SETRA,
             -ArMath::roundShort(myRotDecel));
      myLastSentRotDecel = myRotDecel;
      if (myLogMovementSent)
        ArLog::log(ArLog::Normal, "%25sAction-but-robot rot decel of %d", 
                   "", -ArMath::roundShort(myRotDecel));      
    }



    if (myActionDesired.getDeltaHeadingStrength() >=
                                             ArActionDesired::MIN_STRENGTH)
    {
      if (ArMath::roundShort(myActionDesired.getDeltaHeading()) == 0)
      {
        rotStopped = true;
        rotVal = 0;
        rotHeading = false;
      }
      else
      {
        //printf("delta %.0f\n", myActionDesired.getDeltaHeading());
        //encTh = ArMath::subAngle(myRotVal, myEncoderTransform.getTh());
        encTh = ArMath::subAngle(
                ArMath::addAngle(myActionDesired.getDeltaHeading(), 
                                 getTh()),
                myEncoderTransform.getTh());
        //printf("final th %.0f\n", th);
        rawTh = ArMath::addAngle(encTh, 
                                 ArMath::subAngle(myRawEncoderPose.getTh(),
                                                  myEncoderPose.getTh()));
        rotVal = ArMath::roundShort(rawTh);
        rotStopped = false;
        rotHeading = true;
      }
      myActionRotSet = true;
    } 
    else if (myActionDesired.getRotVelStrength() >=
                                             ArActionDesired::MIN_STRENGTH)
    {
      if (ArMath::roundShort(myActionDesired.getRotVel()) == 0)
      {
        rotStopped = true;
        rotVal = 0;
        rotHeading = false;
      }
      else
      {
        rotStopped = false;
        rotVal = ArMath::roundShort(myActionDesired.getRotVel());
        rotHeading = false;
      }
      myActionRotSet = true;
    }
    else
    {
      rotStopped = myLastActionRotStopped;
      rotVal = myLastActionRotVal;
      rotHeading = myLastActionRotHeading;
    }

    if (myActionRotSet && 
        (myLastRotSent.mSecSince() > myStateReflectionRefreshTime ||
         rotStopped != myLastActionRotStopped || 
         rotVal != myLastActionRotVal || 
         rotHeading != myLastActionRotHeading))
    {
      if (rotStopped)
      {
        comInt(ArCommands::RVEL, 0);
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, 
                     "%25sAction rot vel of 0 (rotStopped)",
                     "");
      }
      else if (rotHeading)
      {
        comInt(ArCommands::HEAD, rotVal);
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, 
                     "%25sAction rot heading of %d (encoder %d, raw %d)",
                     "",
                     ArMath::roundShort(ArMath::addAngle(
                             myActionDesired.getDeltaHeading(), 
                             getTh())),
                     ArMath::roundShort(encTh),
                     ArMath::roundShort(rotVal));
      }
      else
      {
        comInt(ArCommands::RVEL, rotVal);
        if (myLogMovementSent)
          ArLog::log(ArLog::Normal, "%25sAction rot vel of %d", "", 
                     rotVal);
      }
      myLastRotSent.setToNow();
    }               
    
    myLastActionRotVal = rotVal;
    myLastActionRotStopped = rotStopped;
    myLastActionRotHeading = rotHeading;
  }

  if (myLastRotSent.mSecSince() > myStateReflectionRefreshTime &&
      myLastTransSent.mSecSince() > myStateReflectionRefreshTime &&
      myLastPulseSent.mSecSince() > myStateReflectionRefreshTime)
  {
    com(ArCommands::PULSE);
    myLastPulseSent.setToNow();
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Pulse"); 

  }
}

bool ArRobot::handlePacket(ArRobotPacket *packet)
{
  std::list<ArRetFunctor1<bool, ArRobotPacket *> *>::iterator it;
  bool handled;

  lock();
  //printf("ms since last packet %ld this type 0x%x\n", myLastPacketReceivedTime.mSecSince(packet->getTimeReceived()), packet->getID());
  myLastPacketReceivedTime = packet->getTimeReceived();
  if (packet->getID() == 0xff) 
  {
    ArLog::log(ArLog::Terse, "Losing connection because robot was reset.");
    dropConnection();
    unlock();
    return false;
  }

  for (handled = false, it = myPacketHandlerList.begin(); 
       it != myPacketHandlerList.end() && handled == false; 
       it++)
  {
    if ((*it) != NULL && (*it)->invokeR(packet)) 
      handled = true;
    else
      packet->resetRead();
  }
  if (!handled)
    ArLog::log(ArLog::Normal, 
               "No packet handler wanted packet with ID: 0x%x", 
               packet->getID());
  unlock();
  return handled;
}


/* @note You must first start the encoder packet stream by calling
 * requestEncoderPackets() before this function will return encoder values.
 */
long int ArRobot::getLeftEncoder()
{
  return myLeftEncoder;
}

/* @note You must first start the encoder packet stream by calling
 * requestEncoderPackets() before this function will return encoder values.
 */
long int ArRobot::getRightEncoder()
{
  return myRightEncoder;
}


void ArRobot::robotLocker(void)
{
  lock();
}

void ArRobot::robotUnlocker(void)
{
  unlock();
}

void ArRobot::packetHandler(void)
{
  ArRobotPacket *packet;
  int timeToWait;
  ArTime start;
  bool sipHandled = false;

  if (myAsyncConnectFlag)
  {
    lock();
    asyncConnectHandler(false);
    unlock();
    return;
  }

  if (!isConnected())
    return;

  start.setToNow();
  /*
    The basic idea is that if we're chained to the sip we run through
    and see if we have any packets available now (like if we got
    backed up), we only check this for half the cycle time
    though... if we know the cycle time of the robot (from config)
    then we go for half that, if we don't know the cycle time of the
    robot (from config) then we go for half of whatever our cycle time
    is set to

    if we don't have any packets waiting then we chill and wait for
    it, if we got one, just get on with it
  **/
  packet = NULL;
  // read all the packets that are available
  while ((packet = myReceiver.receivePacket(0)) != NULL)
  {
    if (myPacketsReceivedTracking)
    {
      ArLog::log(ArLog::Normal, 
                 "Rcvd: prePacket (%ld) 0x%x at %ld (%ld)", 
                 myPacketsReceivedTrackingCount, 
                 packet->getID(), start.mSecSince(), 
                 myPacketsReceivedTrackingStarted.mSecSince());
      myPacketsReceivedTrackingCount++;
    }

    handlePacket(packet);
    if ((packet->getID() & 0xf0) == 0x30)
      sipHandled = true;
    packet = NULL;

    // if we've taken too long here then break
    if ((getOrigRobotConfig()->hasPacketArrived() &&
         start.mSecSince() > getOrigRobotConfig()->getSipCycleTime() / 2) ||
        (!getOrigRobotConfig()->hasPacketArrived() &&
         (unsigned int) start.mSecSince() > myCycleTime / 2))
    {
      break;
    }
  }

  if (isCycleChained())
    timeToWait = getCycleTime() * 2 - start.mSecSince();
  
  // if we didn't get a sip and we're chained to the sip, wait for the sip
  while (isCycleChained() && !sipHandled && isRunning() && 
         (packet = myReceiver.receivePacket(timeToWait)) != NULL)
  {
    if (myPacketsReceivedTracking)
    {
      ArLog::log(ArLog::Normal, "Rcvd: Packet (%ld) 0x%x at %ld (%ld)", 
                 myPacketsReceivedTrackingCount, 
                 packet->getID(), start.mSecSince(), 
                 myPacketsReceivedTrackingStarted.mSecSince());
      myPacketsReceivedTrackingCount++;
    }

    handlePacket(packet);
    if ((packet->getID() & 0xf0) == 0x30)
      break;
    timeToWait = getCycleTime() * 2 - start.mSecSince();
    if (timeToWait < 0)
      timeToWait = 0;
  }

  if (myTimeoutTime > 0 && 
      ((-myLastPacketReceivedTime.mSecTo()) > myTimeoutTime))
  {
    ArLog::log(ArLog::Terse, 
               "Losing connection because nothing received from robot in %d milliseconds.", 
               myTimeoutTime);
    dropConnection();
  }

  if (myPacketsReceivedTracking)
    ArLog::log(ArLog::Normal, "Rcvd: time taken %ld", start.mSecSince());

}

void ArRobot::actionHandler(void)
{
  ArActionDesired *actDesired;

  if (myResolver == NULL || myActions.size() == 0 || !isConnected())
    return;
  
  actDesired = myResolver->resolve(&myActions, this);
  
  myActionDesired.reset();

  if (actDesired == NULL)
    return;

  myActionDesired.merge(actDesired);  
}

void ArRobot::setCycleWarningTime(unsigned int ms)
{
  myCycleWarningTime = ms;
  // we don't have to send it down because the functor gets it each cycle
}

unsigned int ArRobot::getCycleWarningTime(void) const
{
  return myCycleWarningTime;
}

unsigned int ArRobot::getCycleWarningTime(void)
{
  return myCycleWarningTime;
}

void ArRobot::setCycleTime(unsigned int ms)
{
  myCycleTime = ms;
}

void ArRobot::setStabilizingTime(int mSecs)
{
  if (mSecs > 0)
    myStabilizingTime = mSecs;
  else
    myStabilizingTime = 0;
}

int ArRobot::getStabilizingTime(void) const
{
  return myStabilizingTime;
}

unsigned int ArRobot::getCycleTime(void) const
{
  return myCycleTime;
}



void ArRobot::setConnectionCycleMultiplier(unsigned int multiplier)
{
  myConnectionCycleMultiplier = multiplier;
}

unsigned int ArRobot::getConnectionCycleMultiplier(void) const
{
  return myConnectionCycleMultiplier;
}


void ArRobot::loopOnce(void)
{
  if (mySyncTaskRoot != NULL)
    mySyncTaskRoot->run();
  
  incCounter();
}


// DigIn IR logic is reverse.  0 means broken, 1 means not broken

bool ArRobot::isLeftTableSensingIRTriggered(void) const
{
  if (myParams->haveTableSensingIR())
  {
    if (myParams->haveNewTableSensingIR() && myIODigInSize > 3)
      return !(getIODigIn(3) & ArUtil::BIT1);
    else
      return !(getDigIn() & ArUtil::BIT0);
  }
  return 0;
}

bool ArRobot::isRightTableSensingIRTriggered(void) const
{
  if (myParams->haveTableSensingIR())
  {
    if (myParams->haveNewTableSensingIR() && myIODigInSize > 3) 
      return !(getIODigIn(3) & ArUtil::BIT0);
    else
      return !(getDigIn() & ArUtil::BIT1);
  }
  return 0;
}

bool ArRobot::isLeftBreakBeamTriggered(void) const
{
  if (myParams->haveTableSensingIR())
  {
    if (myParams->haveNewTableSensingIR() && myIODigInSize > 3) 
      return !(getIODigIn(3) & ArUtil::BIT2);
    else
      return !(getDigIn() & ArUtil::BIT3);
  }
  return 0;
}

bool ArRobot::isRightBreakBeamTriggered(void) const
{
  if (myParams->haveTableSensingIR())
  {
    if (myParams->haveNewTableSensingIR() && myIODigInSize > 3) 
      return !(getIODigIn(3) & ArUtil::BIT3);
    else
      return !(getDigIn() & ArUtil::BIT2);
  }
  return 0;
}

int ArRobot::getMotorPacCount(void) const
{
  if (myTimeLastMotorPacket == time(NULL))
    return myMotorPacCount;
  if (myTimeLastMotorPacket == time(NULL) - 1)
    return myMotorPacCurrentCount;
  return 0;
}

int ArRobot::getSonarPacCount(void) const
{
  if (myTimeLastSonarPacket == time(NULL))
    return mySonarPacCount;
  if (myTimeLastSonarPacket == time(NULL) - 1)
    return mySonarPacCurrentCount;
  return 0;
}


bool ArRobot::processMotorPacket(ArRobotPacket *packet)
{
  int x, y, th, qx, qy, qth; 
  double deltaX, deltaY, deltaTh;

  int numReadings;
  int sonarNum;
  int sonarRange;

  if (packet->getID() != 0x32 && packet->getID() != 0x33) 
    return false;

  // upkeep the counting variable
  if (myTimeLastMotorPacket != time(NULL)) 
  {
    myTimeLastMotorPacket = time(NULL);
    myMotorPacCount = myMotorPacCurrentCount;
    myMotorPacCurrentCount = 0;
  }
  myMotorPacCurrentCount++;

  x = (packet->bufToUByte2() & 0x7fff);
  y = (packet->bufToUByte2() & 0x7fff);
  th = packet->bufToByte2();

  if (myFirstEncoderPose)
  {
    qx = 0;
    qy = 0;
    qth = 0;
    myFirstEncoderPose = false;
    myRawEncoderPose.setPose(
            myParams->getDistConvFactor() * x,
            myParams->getDistConvFactor() * y, 
            ArMath::radToDeg(myParams->getAngleConvFactor() * (double)th));
    myEncoderPose = myRawEncoderPose;
    myEncoderTransform.setTransform(myEncoderPose, myGlobalPose);
  }
  else 
  {
    qx = x - myLastX;
    qy = y - myLastY;
    qth = th - myLastTh;
  }


  //ArLog::log(ArLog::Terse, "Damnit qx %d qy %d,  x %d y %d,  lastx %d lasty %d", qx, qy, x, y, myLastX, myLastY);  
  myLastX = x;
  myLastY = y;
  myLastTh = th;

  if (qx > 0x1000) 
    qx -= 0x8000;
  if (qx < -0x1000)
    qx += 0x8000;

  if (qy > 0x1000) 
    qy -= 0x8000;
  if (qy < -0x1000)
    qy += 0x8000;
  
  deltaX = myParams->getDistConvFactor() * (double)qx;
  deltaY = myParams->getDistConvFactor() * (double)qy;
  deltaTh = ArMath::radToDeg(myParams->getAngleConvFactor() * (double)qth);
  //encoderTh = ArMath::radToDeg(myParams->getAngleConvFactor() * (double)(th));


  // encoder stuff was here



  myLeftVel = myParams->getVelConvFactor() * packet->bufToByte2();
  myRightVel = myParams->getVelConvFactor() * packet->bufToByte2();
  myVel = (myLeftVel + myRightVel)/2.0;
  myRotVel = ArMath::radToDeg((myRightVel - myLeftVel) / 2.0 * 
                                    myParams->getDiffConvFactor());

  myBatteryVoltage = packet->bufToUByte() * .1;
  myBatteryAverager.add(myBatteryVoltage);
  myStallValue = packet->bufToByte2();
  
  //ArLog::log("x %.1f y %.1f th %.1f vel %.1f voltage %.1f", myX, myY, myTh, 
  //myVel, myBatteryVoltage);

  myControl = ArMath::fixAngle(ArMath::radToDeg(myParams->getAngleConvFactor() *
                                  (packet->bufToByte2() - th)));
  myFlags = packet->bufToUByte2();
  myCompass = 2*packet->bufToUByte();
  
  for (numReadings = packet->bufToByte(); numReadings > 0; numReadings--)
  {
    sonarNum = packet->bufToByte();
    sonarRange = ArMath::roundInt(
            (double)packet->bufToUByte2() * myParams->getRangeConvFactor());
    processNewSonar(sonarNum, sonarRange, packet->getTimeReceived());
  }
  
  if (packet->getDataLength() - packet->getDataReadLength() > 0)
  {
    myAnalogPortSelected = packet->bufToUByte2();
    myAnalog = packet->bufToByte();
    myDigIn = packet->bufToByte();
    myDigOut = packet->bufToByte();
  }

  if (packet->getDataLength() - packet->getDataReadLength() > 0)
    myRealBatteryVoltage = packet->bufToUByte2() * .1;
  else
    myRealBatteryVoltage = myBatteryVoltage;

  myRealBatteryAverager.add(myRealBatteryVoltage);

  if (packet->getDataLength() - packet->getDataReadLength() > 0)
    myChargeState = (ChargeState) packet->bufToUByte();
  else
    myChargeState = CHARGING_UNKNOWN;

  /*
    Okay how this works is like so.  

    We keep around the raw encoder position, because we must use this
    to find differences between last position and this position.
    
    We find the difference in x and y positions (deltaX and deltaY)
    and keep these around for later use, but we also add these to our
    raw encoder readings for X and Y.  We also find the change in
    angle (deltaTh), which is used for inertial corrections, and added
    to the raw encoder heading to find which the current raw encoder
    heading.


    From here there are two paths:

    Path 1) Have a callback.  If we have a callback it means that we
    have an inertial nav device of some kind.  If this is the case,
    then we pass the callback the delta between last position and
    current position, along with the time of the current position,
    then the callback gives us back a new delta theta (deltaTh).  We
    then need to rotate the deltaX and deltaY into our corrected
    encoder space.  We do this by making a transform that takes the
    raw encoder heading and transforms it to what our new heading is
    (adding deltaTh to our current encoder th), and then applying that
    transform, taking the results as our new deltaX and deltaY.

    Path 2) We have no callback, we just use the heading that came
    back from the robot as our delta theta (deltaTh);

    From here the two paths unify again.  deltaX and deltaY are added
    to the encoder pose (this is the corrected encoder pose), and the
    encoder heading is set to the newTh.

    Note that this leaves a difference between rawEncoder heading and
    our heading, which is fine, BUT if you are sending heading
    commands to the robot you need to compenstate for the difference
    between these. 
    
    Note above that we return deltaTh instead of just heading so that
    we can turn inertial on and off without losing track of where
    we're at... since we're just adding in deltas from the heading it
    doesn't matter how we switch around the callback.
    
  **/

  myRawEncoderPose.setX(myRawEncoderPose.getX() + deltaX);
  myRawEncoderPose.setY(myRawEncoderPose.getY() + deltaY);
  myRawEncoderPose.setTh(myRawEncoderPose.getTh() + deltaTh);

  // check if there is a correction callback, if there is get the new
  // heading out of it instead of using the raw encoder heading
  if (myEncoderCorrectionCB != NULL)
  {
    ArPoseWithTime deltaPose(deltaX, deltaY, deltaTh,
                             packet->getTimeReceived());
    deltaTh = myEncoderCorrectionCB->invokeR(deltaPose);   
    ArTransform trans(ArPose(0, 0, myRawEncoderPose.getTh()),
                      ArPose(0, 0,
                             ArMath::addAngle(myEncoderPose.getTh(), 
                                              deltaTh)));

    ArPose rotatedDelta = trans.doTransform(ArPose(deltaX, deltaY, 0));

    deltaX = rotatedDelta.getX();
    deltaY = rotatedDelta.getY();
  }

  myEncoderPose.setTime(packet->getTimeReceived());
  myEncoderPose.setX(myEncoderPose.getX() + deltaX);
  myEncoderPose.setY(myEncoderPose.getY() + deltaY);
  myEncoderPose.setTh(ArMath::addAngle(myEncoderPose.getTh(), deltaTh));

  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);

  if (myLogMovementReceived)
    ArLog::log(ArLog::Normal, 
               "Global (%5.0f %5.0f %5.0f) Encoder (%5.0f %5.0f %5.0f) Raw (%5.0f %5.0f %5.0f)",
               myGlobalPose.getX(), myGlobalPose.getY(),
               myGlobalPose.getTh(),
               myEncoderPose.getX(), myEncoderPose.getY(),
               myEncoderPose.getTh(),
               myRawEncoderPose.getX(), myRawEncoderPose.getY(), 
               myRawEncoderPose.getTh());

  if (myLogVelocitiesReceived)
    ArLog::log(ArLog::Normal, 
               "     TransVel: %5.0f RotVel: %5.0f LeftVel: %5.0f RightVel: %5.0f Heading %5.0f",
               getVel(), getRotVel(), getLeftVel(), getRightVel(), 
               getTh());

  //ArLog::log(ArLog::Terse, "(%.0f %.0f) (%.0f %.0f)", deltaX, deltaY, myGlobalPose.getX(),         myGlobalPose.getY());
  
  myInterpolation.addReading(packet->getTimeReceived(), myGlobalPose);
  myEncoderInterpolation.addReading(packet->getTimeReceived(), myEncoderPose);

  return true;
}

void ArRobot::processNewSonar(char number, int range, 
                                       ArTime timeReceived)
{
  std::map<int, ArSensorReading *>::iterator it;
  ArSensorReading *sonar;
  ArTransform encoderTrans;
  ArPose encoderPose;

  if ((it = mySonars.find(number)) != mySonars.end())
  {
    sonar = (*it).second;
    sonar->newData(range, getPose(), getEncoderPose(), getToGlobalTransform(), 
                   getCounter(), timeReceived); 
    if (myTimeLastSonarPacket != time(NULL)) 
    {
      myTimeLastSonarPacket = time(NULL);
      mySonarPacCount = mySonarPacCurrentCount;
      mySonarPacCurrentCount = 0;
    }
    mySonarPacCurrentCount++;
  }
  else if (!myWarnedAboutExtraSonar)
  {
    ArLog::log(ArLog::Normal, "Robot gave back extra sonar reading!  Either the parameter file for the robot or the firmware needs updating.");
    myWarnedAboutExtraSonar = true;
  }
}



bool ArRobot::processEncoderPacket(ArRobotPacket *packet)
{
  if (packet->getID() != 0x90)
    return false;
  myLeftEncoder = packet->bufToByte4();
  myRightEncoder = packet->bufToByte4();
  return true;
}

bool ArRobot::processIOPacket(ArRobotPacket *packet)
{
  int i, num;

  if (packet->getID() != 0xf0)
    return false;

  myLastIOPacketReceivedTime = packet->getTimeReceived();

  // number of DigIn bytes
  num = packet->bufToUByte();
  for (i = 0; i < num; ++i)
    myIODigIn[i] = packet->bufToUByte();
  myIODigInSize = num;

  // number of DigOut bytes
  num = packet->bufToUByte();
  for (i = 0; i < num; ++i)
    myIODigOut[i] = packet->bufToUByte();
  myIODigOutSize = num;

  // number of A/D bytes
  num = packet->bufToUByte();
  for (i = 0; i < num; ++i)
    myIOAnalog[i] = packet->bufToUByte2();
  myIOAnalogSize = num;

  return true;
}

int ArRobot::getSonarRange(int num) const
{
  std::map<int, ArSensorReading *>::const_iterator it;
  
  if ((it = mySonars.find(num)) != mySonars.end())
    return (*it).second->getRange();
  else
    return -1;
}

bool ArRobot::isSonarNew(int num) const
{
  std::map<int, ArSensorReading *>::const_iterator it;
  
  if ((it = mySonars.find(num)) != mySonars.end())
    return (*it).second->isNew(getCounter());
  else
    return false;
}

ArSensorReading *ArRobot::getSonarReading(int num) const
{
  std::map<int, ArSensorReading *>::const_iterator it;
  
  if ((it = mySonars.find(num)) != mySonars.end())
    return (*it).second;
  else
    return NULL;
}


bool ArRobot::com(unsigned char command)
{
  if (myPacketsSentTracking)
    ArLog::log(ArLog::Normal, "Sent: com(%d)", command);
  return mySender.com(command);
}

bool ArRobot::comInt(unsigned char command, short int argument)
{
  if (myPacketsSentTracking)
    ArLog::log(ArLog::Normal, "Sent: comInt(%d, %d)", command, argument);
  return mySender.comInt(command, argument);
}

bool ArRobot::com2Bytes(unsigned char command, char high, char low)
{
  if (myPacketsSentTracking)
    ArLog::log(ArLog::Normal, "Sent: com2Bytes(%d, %d, %d)", command, 
               high, low);
  return mySender.com2Bytes(command, high, low);
}

bool ArRobot::comStr(unsigned char command, const char *argument)
{
  if (myPacketsSentTracking)
    ArLog::log(ArLog::Normal, "Sent: comStr(%d, '%s')", command, 
               argument);
  return mySender.comStr(command, argument);
}

bool ArRobot::comStrN(unsigned char command, const char *str, 
                               int size)
{
  if (myPacketsSentTracking)
  {
    char strBuf[512];
    strncpy(strBuf, str, size);
    strBuf[size] = '\0';
    ArLog::log(ArLog::Normal, "Sent: comStrN(%d, '%s') (size %d)",
               command, strBuf, size);
  }
  return mySender.comStrN(command, str, size);
}

int ArRobot::getClosestSonarRange(double startAngle, double endAngle) const
{
  int num;
  num = getClosestSonarNumber(startAngle, endAngle);
  if (num == -1)
    return -1;
  else 
    return getSonarRange(num);
}

int ArRobot::getClosestSonarNumber(double startAngle, double endAngle) const
{
  int i;
  ArSensorReading *sonar;
  int closestReading;
  int closestSonar;
  bool noReadings = true;

  for (i = 0; i < getNumSonar(); i++) 
  {
    sonar = getSonarReading(i);
    if (sonar == NULL)
    {
      ArLog::log(ArLog::Terse, "Have an empty sonar at number %d, there should be %d sonar.", i, getNumSonar());
      continue;
    }
    if (ArMath::angleBetween(sonar->getSensorTh(), startAngle, endAngle))
    {
      if (noReadings)
      {
        closestReading = sonar->getRange();
        closestSonar = i;
        noReadings = false;
      }
      else if (sonar->getRange() < closestReading)
      {
        closestReading = sonar->getRange();
        closestSonar = i;
      }
    }
  }

  if (noReadings)
    return -1;
  else
    return closestSonar;
}

void ArRobot::addRangeDevice(ArRangeDevice *device)
{
  device->setRobot(this);
  myRangeDeviceList.push_front(device);
}

void ArRobot::remRangeDevice(const char *name)
{
  std::list<ArRangeDevice *>::iterator it;
  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    if (strcmp(name, (*it)->getName()) == 0)
    {
      myRangeDeviceList.erase(it);
      return;
    }
  }
}

void ArRobot::remRangeDevice(ArRangeDevice *device)
{
  std::list<ArRangeDevice *>::iterator it;
  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    if ((*it) == device)
    {
      myRangeDeviceList.erase(it);
      return;
    }
  }
}

ArRangeDevice *ArRobot::findRangeDevice(const char *name)
{
  std::list<ArRangeDevice *>::iterator it;
  ArRangeDevice *device;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    if (strcmp(name, device->getName()) == 0)
    {
      return device;
    }
  }
  return NULL;
}

const ArRangeDevice *ArRobot::findRangeDevice(const char *name) const
{
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    if (strcmp(name, device->getName()) == 0)
    {
      return device;
    }
  }
  return NULL;
}

std::list<ArRangeDevice *> *ArRobot::getRangeDeviceList(void)
{
  return &myRangeDeviceList;
}

bool ArRobot::hasRangeDevice(ArRangeDevice *device) const
{
  std::list<ArRangeDevice *>::const_iterator it;
  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    if ((*it) == device)
      return true;
  }
  return false;
}

double ArRobot::checkRangeDevicesCurrentPolar(double startAngle,
                                                       double endAngle,
                                                       double *angle) const
{
  double closest, closeAngle, tempDist, tempAngle;
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;
  bool foundOne = false;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    device->lockDevice();
    if (!foundOne || 
        (tempDist = device->currentReadingPolar(startAngle, endAngle,
                                                &tempAngle)) < closest)
    {
      if (!foundOne)
      {
        closest = device->currentReadingPolar(startAngle, endAngle, 
                                              &closeAngle);
      }
      else
      {
        closest = tempDist;
        closeAngle = tempAngle;
      }
      foundOne = true;
    }
    device->unlockDevice();
  }
  if (!foundOne)
    return -1;
  if (angle != NULL)
    *angle = closeAngle;
  return closest;
  
}

double ArRobot::checkRangeDevicesCumulativePolar(double startAngle,
                                                       double endAngle,
                                                       double *angle) const
{
  double closest, closeAngle, tempDist, tempAngle;
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;
  bool foundOne = false;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    device->lockDevice();
    if (!foundOne || 
        (tempDist = device->cumulativeReadingPolar(startAngle, endAngle,
                                                 &tempAngle)) < closest)
    {
      if (!foundOne)
      {
        closest = device->cumulativeReadingPolar(startAngle, endAngle, 
                                              &closeAngle);
      }
      else
      {
        closest = tempDist;
        closeAngle = tempAngle;
      }
      foundOne = true;
    }
    device->unlockDevice();
  }
  if (!foundOne)
    return -1;
  if (angle != NULL)
    *angle = closeAngle;
  return closest;
  
}

double ArRobot::checkRangeDevicesCurrentBox(double x1, double y1,
                                                     double x2, double y2,
                                                     ArPose *readingPos) const
{

  double closest, tempDist;
  ArPose closestPos, tempPos;
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;
  bool foundOne = false;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    device->lockDevice();
    if (!foundOne || 
        (tempDist = device->currentReadingBox(x1, y1, x2, y2, &tempPos)) < closest)
    {
      if (!foundOne)
      {
        closest = device->currentReadingBox(x1, y1, x2, y2, &closestPos);
      }
      else
      {
        closest = tempDist;
        closestPos = tempPos;
      }
      foundOne = true;
    }
    device->unlockDevice();
  }
  if (!foundOne)
    return -1;
  if (readingPos != NULL)
    *readingPos = closestPos;
  return closest;
}

double ArRobot::checkRangeDevicesCumulativeBox(double x1, double y1,
                                                     double x2, double y2,
                                                     ArPose *readingPos) const
{

  double closest, tempDist;
  ArPose closestPos, tempPos;
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;
  bool foundOne = false;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    device->lockDevice();
    if (!foundOne || 
        (tempDist = device->cumulativeReadingBox(x1, y1, x2, y2, &tempPos)) < closest)
    {
      if (!foundOne)
      {
        closest = device->cumulativeReadingBox(x1, y1, x2, y2, &closestPos);
      }
      else
      {
        closest = tempDist;
        closestPos = tempPos;
      }
      foundOne = true;
    }
    device->unlockDevice();
  }
  if (!foundOne)
    return -1;
  if (readingPos != NULL)
    *readingPos = closestPos;
  return closest;
}

void ArRobot::moveTo(ArPose pose, bool doCumulative)
{
  std::list<ArRangeDevice *>::iterator it;
  ArSensorReading *son;
  int i;

  // we need to get this one now because changing the encoder
  // transform and global pose will change the local transform
  ArTransform localTransform;
  localTransform = getToLocalTransform();

  myEncoderTransform.setTransform(myEncoderPose, pose);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); it++)
  {
    (*it)->lockDevice();
    (*it)->applyTransform(localTransform, doCumulative);
    (*it)->applyTransform(getToGlobalTransform(), doCumulative);
    (*it)->unlockDevice();
  }

  for (i = 0; i < getNumSonar(); i++)
  {
    son = getSonarReading(i);
    if (son != NULL)
    {
      son->applyTransform(localTransform);
      son->applyTransform(getToGlobalTransform());
    }
  }

  //ArLog::log(ArLog::Normal, "Robot moved to %.0f %.0f %.1f", getX(), getY(), getTh());
}

void ArRobot::moveTo(ArPose poseTo, ArPose poseFrom,
                              bool doCumulative)
{
  std::list<ArRangeDevice *>::iterator it;
  ArSensorReading *son;
  int i;

  ArPose result = myEncoderTransform.doInvTransform(poseFrom);

  // we need to get this one now because changing the encoder
  // transform and global pose will change the local transform
  ArTransform localTransform;
  localTransform = getToLocalTransform();

  myEncoderTransform.setTransform(result, poseTo);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); it++)
  {
    (*it)->lockDevice();
    (*it)->applyTransform(localTransform, doCumulative);
    (*it)->applyTransform(getToGlobalTransform(), doCumulative);
    (*it)->unlockDevice();
  }

  for (i = 0; i < getNumSonar(); i++)
  {
    son = getSonarReading(i);
    if (son != NULL)
    {
      son->applyTransform(localTransform);
      son->applyTransform(getToGlobalTransform());
    }
  }

  //ArLog::log(ArLog::Normal, "Robot moved to %.0f %.0f %.1f", getX(), getY(), getTh());
}

void ArRobot::setEncoderTransform(ArPose deadReconPos, 
                                    ArPose globalPos)
{
  myEncoderTransform.setTransform(deadReconPos, globalPos);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);
}

void ArRobot::setEncoderTransform(ArPose transformPos)
{
  myEncoderTransform.setTransform(transformPos);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);
}

ArTransform ArRobot::getEncoderTransform(void) const
{
  return myEncoderTransform;
}

void ArRobot::setDeadReconPose(ArPose pose)
{
  myEncoderPose.setPose(pose);
  myEncoderTransform.setTransform(myEncoderPose, myGlobalPose);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);
}


ArTransform ArRobot::getToGlobalTransform(void) const
{
  ArTransform trans;
  ArPose origin(0, 0, 0);
  ArPose pose = getPose();

  trans.setTransform(origin, pose);
  return trans;
}

ArTransform ArRobot::getToLocalTransform(void) const
{
  ArTransform trans;
  ArPose origin(0, 0, 0);
  ArPose pose = getPose();

  trans.setTransform(pose, origin);
  return trans;
}

void ArRobot::applyTransform(ArTransform trans, bool doCumulative)
{
  std::list<ArRangeDevice *>::iterator it;
  ArSensorReading *son;
  int i;
  
  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); it++)
  {
      (*it)->lockDevice();
      (*it)->applyTransform(trans, doCumulative);
      (*it)->unlockDevice();
  }

  for (i = 0; i < getNumSonar(); i++)
  {
    son = getSonarReading(i);
    if (son != NULL)
      son->applyTransform(trans);
  }
}

const char *ArRobot::getName(void) const
{
  return myName.c_str();
}

void ArRobot::setName(const char * name)
{
        std::list<ArRobot *> *robotList;
    std::list<ArRobot *>::iterator it;
    int i;
    char buf[1024];
 
 if (name != NULL)
  {
    myName = name;
  }
  else
  {

    

    robotList = Aria::getRobotList();
    for (i = 1, it = robotList->begin(); it != robotList->end(); it++, i++)
    {
      if (this == (*it))
      {
            if (i == 1)
                        myName = "robot";
                else
                {
                        sprintf(buf, "robot%d", i);
                        myName = buf;
                }
            return;
      }
    }
    sprintf(buf, "robot%d", robotList->size());
    myName = buf;
   }
}

void ArRobot::setEncoderCorrectionCallback(
        ArRetFunctor1<double, ArPoseWithTime> *functor)
{
  myEncoderCorrectionCB = functor;
}
ArRetFunctor1<double, ArPoseWithTime> * 
ArRobot::getEncoderCorrectionCallback(void) const
{
  return myEncoderCorrectionCB;
}

void ArRobot::setDirectMotionPrecedenceTime(int mSec)
{
  if (mSec < 0) 
    myDirectPrecedenceTime = 0;
  else
    myDirectPrecedenceTime = mSec;
}

unsigned int ArRobot::getDirectMotionPrecedenceTime(void) const
{
  return myDirectPrecedenceTime;
}


void ArRobot::clearDirectMotion(void)
{
  myTransType = TRANS_NONE;
  myLastTransType = TRANS_NONE;
  myRotType = ROT_NONE;
  myLastRotType = ROT_NONE;
  myLastActionTransVal = 0;
  myLastActionRotStopped = true;
  myLastActionRotHeading = false;
}

void ArRobot::stopStateReflection(void)
{
  myTransType = TRANS_IGNORE;
  myLastTransType = TRANS_IGNORE;
  myRotType = ROT_IGNORE;
  myLastRotType = ROT_IGNORE;
}

bool ArRobot::isDirectMotion(void) const
{
  if (myTransType ==  TRANS_NONE && myLastTransType == TRANS_NONE &&
      myRotType == ROT_NONE && myLastRotType == ROT_NONE)
    return false;
  else
    return true;
}


void ArRobot::enableMotors()
{
  comInt(ArCommands::ENABLE, 1);
}

void ArRobot::disableMotors()
{
  comInt(ArCommands::ENABLE, 0);
}


void ArRobot::setStateReflectionRefreshTime(int mSec)
{
  if (mSec < 0)
    myStateReflectionRefreshTime = 0;
  else
    myStateReflectionRefreshTime = mSec;
}

int ArRobot::getStateReflectionRefreshTime(void) const
{
  return myStateReflectionRefreshTime;
}

void ArRobot::attachKeyHandler(ArKeyHandler *keyHandler, 
                                        bool exitOnEscape, 
                                        bool useExitNotShutdown)
{
  if (myKeyHandlerCB != NULL)
    delete myKeyHandlerCB;
  myKeyHandlerCB = new ArFunctorC<ArKeyHandler>(keyHandler, 
                                            &ArKeyHandler::checkKeys);
  addSensorInterpTask("Key Handler", 50, myKeyHandlerCB);

  myKeyHandler = keyHandler;
  myKeyHandlerUseExitNotShutdown = useExitNotShutdown;
  if (exitOnEscape)
    keyHandler->addKeyHandler(ArKeyHandler::ESCAPE, &myKeyHandlerExitCB);
}

ArKeyHandler *ArRobot::getKeyHandler(void) const
{
  return myKeyHandler;
}

void ArRobot::keyHandlerExit(void)
{
  ArLog::log(ArLog::Terse, "Escape was pressed, program is exiting.");
  // if we're using exit not the keyhandler then call Aria::exit
  // instead of shutdown, this call never returns
  if (myKeyHandlerUseExitNotShutdown)
    Aria::exit();
  stopRunning();
  unlock();
  Aria::shutdown();
}

void ArRobot::setPacketsReceivedTracking(bool packetsReceivedTracking)
{
  myPacketsReceivedTracking = packetsReceivedTracking;
  myPacketsReceivedTrackingCount = 0; 
  myPacketsReceivedTrackingStarted.setToNow(); 
}

void ArRobot::ariaExitCallback(void)
{
  mySyncLoop.stopRunIfNotConnected(false);
  disconnect();
}

ArRobot::ChargeState ArRobot::getChargeState(void) const
{
  return myChargeState;
}

---