ArNetPacketReceiverTcp.cpp

Go to the documentation of this file.

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

#include "Aria.h"
#include "ArExport.h"
#include "ArNetPacketReceiverTcp.h"
#ifndef WIN32
#include "errno.h"
#endif

AREXPORT ArNetPacketReceiverTcp::ArNetPacketReceiverTcp()
{
  mySocket = NULL;
  myProcessPacketCB = NULL;
  mySync1 = 0xF;
  mySync2 = 0xC;
  myState = STATE_SYNC1;
  myQuiet = false;
}

AREXPORT ArNetPacketReceiverTcp::~ArNetPacketReceiverTcp()
{

}

AREXPORT void ArNetPacketReceiverTcp::setSocket(ArSocket *socket)
{
  mySocket = socket;
}

AREXPORT ArSocket *ArNetPacketReceiverTcp::getSocket(void)
{
  return mySocket;
}

AREXPORT bool ArNetPacketReceiverTcp::readData(void)
{
  Ret ret;

  while (1)
  {
    ret = readPacket(100);
    if (ret == RET_TIMED_OUT)
    {
      if (!myQuiet)
        ArLog::log(ArLog::Terse, "ReadTcp timed out");
      return true;
    }
    else if (ret == RET_CONN_CLOSED)
    {
      unsigned char *bytes = (unsigned char *)mySocket->inAddr();
      if (!myQuiet)
        ArLog::log(ArLog::Terse, "Connection to %d.%d.%d.%d closed",
                   bytes[0], bytes[1], bytes[2], bytes[3]);
      return false;
    }
    else if (ret == RET_FAILED_READ)
    {
      // if neither of these true's trap it the false is returned cause it was some bad error
#ifdef WIN32
      if (WSAGetLastError() == WSAEWOULDBLOCK)
        return true;
      else if (!myQuiet)
        ArLog::log(ArLog::Terse, "Failed on read TCP, error %d", WSAGetLastError());
#endif
#ifndef WIN32
      if (errno == EAGAIN)
        return true;
      else if (!myQuiet)
        ArLog::log(ArLog::Terse, "Failed on read TCP, error %d", errno);
#endif
      if (!myQuiet)
        ArLog::log(ArLog::Terse, "Failed on the tcp read");
      return false;
    }
    else if (ret == RET_BAD_PACKET)
    {
      // well... there was a bad checksum, keep going
    }
    else if (ret == RET_GOT_PACKET)
    {
      if (myProcessPacketCB != NULL)
      {
        //printf("Got in a packet of %d\n", myPacket.getCommand());
        myPacket.setPacketSource(ArNetPacket::TCP);
        myProcessPacketCB->invoke(&myPacket);
      }
    }
    else
    {
      if (!myQuiet)
        ArLog::log(ArLog::Terse, "ArNetPacketReceiverTcp: bad Ret value %d", ret);
      return false;
    }
  }
}

AREXPORT ArNetPacketReceiverTcp::Ret ArNetPacketReceiverTcp::readPacket(int msWait)
{
  long timeToRunFor;
  ArTime timeDone;
  int numRead;
  bool printing = true;
  int ret;
  unsigned char c;
  timeDone.setToNow();
  timeDone.addMSec(msWait);

  //printf("Read packet!\n");
  do
  {
    timeToRunFor = timeDone.mSecTo();
    if (timeToRunFor < 0)
      timeToRunFor = 0;

    if (myState != STATE_ACQUIRE_DATA)
    {
      c = 0;
      if ((ret = mySocket->read((char *)&c, 1, 0/*timeToRunFor*/)) == -1) 
      {
        if (myState == STATE_SYNC1)
        {
          return RET_FAILED_READ;
        }
        else
        {
          //ArUtil::sleep(1);
          continue;
        }
      }
      else if (ret == 0)
      {
        return RET_CONN_CLOSED;
      }
    }
    
    /*
    if (myState != STATE_ACQUIRE_DATA)
    {
      printf("%d", myState);
      printf(" %d\n", c);
    }
    */
    //else
    //{
    //printf("\n");
    //}
    switch (myState) {
    case STATE_SYNC1:
      if (c == mySync1) // move on, resetting packet
      {
        myState = STATE_SYNC2;
        myPacket.empty();
        myPacket.setLength(0);
        myPacket.uByteToBuf(c);
      }
      else
      {
        if (printing)
          ArLog::log(ArLog::Terse, "Bad char in sync1 %d", c);
        return RET_BAD_PACKET;
      }
      break;
    case STATE_SYNC2:
      if (c == mySync2) // move on, adding this byte
      {
        myState = STATE_LENGTH1;
        myPacket.uByteToBuf(c);
      }
      else // go back to beginning, packet hosed
      {
        if (printing)
          ArLog::log(ArLog::Terse, "Bad char in sync2 %d, returning to sync1", c);
        myState = STATE_SYNC1;
        return RET_BAD_PACKET;
      }
      break;
    case STATE_LENGTH1:
      myState = STATE_LENGTH2;
      myReadLength = ((unsigned int)c & 0xff);
      myPacket.uByteToBuf(c);
      break;
    case STATE_LENGTH2:
      myState = STATE_ACQUIRE_DATA;
      myReadLength += ((unsigned int)c & 0xff) << 8;
      myPacket.uByteToBuf(c);
      myReadCount = 0;
      break;
    case STATE_ACQUIRE_DATA:
      if (myReadLength > ArNetPacket::MAX_LENGTH || 
          myReadLength < myPacket.getHeaderLength() + myPacket.getFooterLength())
      {
        if (!myQuiet)
          ArLog::log(ArLog::Normal, 
                     "ArNetPacketReceiverTcp::readPacket: bad packet length, it is %d which is more than max length of %d bytes or less than the minimum %d", 
                     myReadLength, ArNetPacket::MAX_LENGTH,
                     myPacket.getHeaderLength() + myPacket.getFooterLength());
        myState = STATE_SYNC1;
        return RET_BAD_PACKET;
      }
      // here we read until we get as much as we want, OR until
      // we go 100 ms without data... its arbitrary but it doesn't happen often
      // and it'll mean a bad packet anyways
      if (myReadCount < myReadLength - 4)
      {
        numRead = mySocket->read(myReadBuff + myReadCount, 
                                 myReadLength - myReadCount - 4);
        //printf("numRead %d myReadLength %d\n", numRead, myReadLength);
        // trap if it wasn't data
        if (numRead == 0)
          return RET_CONN_CLOSED;
        else if (numRead < 0)
          return RET_FAILED_READ;
        // if it was data add it to our number read
        myReadCount += numRead;
      }
      if (myReadCount > myReadLength - 4)
      {
        if (!myQuiet)
          ArLog::log(ArLog::Terse, 
                     "Read is greater than it should be at %d > %d", 
                     myReadCount, myReadLength - 4);
      }
      if (myReadCount == myReadLength - 4)
      {
        myPacket.dataToBuf(myReadBuff, myReadCount);
        if (myPacket.verifyCheckSum()) 
        {
          myPacket.resetRead();
          // take off the footer from the packets length Variable
          /* put this in if you want to see the packets received
             //printf("Input ");
             myPacket.log();
          */
          // you can also do this next line if you only care about type
          //printf("Input %x\n", myPacket.getCommand());
          //myPacket.log();
          myState = STATE_SYNC1;
          return RET_GOT_PACKET;
        }
        else 
        {
          myPacket.resetRead();
          if (!myQuiet)
            ArLog::log(ArLog::Normal, 
                       "ArNetPacketReceiverTcp::receivePacket: bad packet, bad checksum on packet %d", myPacket.getCommand());
          myState = STATE_SYNC1;
          return RET_BAD_PACKET;
        }
      }
      break;
    default:
      break;
    }
  } while (timeDone.mSecTo() >= 0 || myState != STATE_SYNC1);
  
  return RET_TIMED_OUT;

}


AREXPORT void ArNetPacketReceiverTcp::setProcessPacketCB(
        ArFunctor1<ArNetPacket *> *functor)
{
  myProcessPacketCB = functor;
}

AREXPORT ArFunctor1<ArNetPacket *> *ArNetPacketReceiverTcp::getProcessPacketCB(void)
{
  return myProcessPacketCB;
}

---