C++ ArTime::addMSec方法代码示例

AREXPORT int ArSerialConnection::read(const char *data, unsigned int size,
				      unsigned int msWait) 
{
  struct timeval tp;		/* time interval structure for timeout */
  fd_set fdset;			/* fd set ??? */
  int n;
  long timeLeft;
  unsigned int bytesRead = 0;
  ArTime timeDone;

  if (myPort >= 0)
  {
    if (msWait >= 0)
    {
      timeDone.setToNow();
      if (!timeDone.addMSec(msWait)) {
        ArLog::log(ArLog::Normal,
                   "ArSerialConnection::read() error adding msecs (%i)",
                   msWait);
      }
      while ((timeLeft = timeDone.mSecTo()) >= 0) 
      {
	tp.tv_sec = (timeLeft) / 1000;	/* we're polling */
	tp.tv_usec = (timeLeft % 1000) * 1000;
	FD_ZERO(&fdset);
	FD_SET(myPort,&fdset);
	if (select(myPort+1,&fdset,NULL,NULL,&tp) <= 0) 
	  return bytesRead;
	if ((n = ::read(myPort, const_cast<char *>(data)+bytesRead, 
			size-bytesRead)) == -1)
	{
	  ArLog::logErrorFromOS(ArLog::Terse, "ArSerialConnection::read:  Blocking read failed.");
	  return bytesRead;
	}
	bytesRead += n;
	if (bytesRead >= size)
	  return bytesRead;
      }
      return bytesRead;
    }
    else 
    {
      n = ::read(myPort, const_cast<char *>(data), size);
      if (n == -1)
	ArLog::logErrorFromOS(ArLog::Terse, "ArSerialConnection::read:  Non-Blocking read failed.");
      return n;
    }
  }
  ArLog::log(ArLog::Normal, "ArSerialConnection::read:  Connection invalid.");
  return -1;
}

AREXPORT int ArSerialConnection::read(const char *data, unsigned int size, 
				      unsigned int msWait) 
{
  COMSTAT stat;
  unsigned long ret;
  unsigned int numToRead;
  ArTime timeDone;

  if (myPort != INVALID_HANDLE_VALUE && myStatus == STATUS_OPEN)
  {
    if (msWait > 0)
    {
      timeDone.setToNow();
      if (!timeDone.addMSec(msWait)) {
        ArLog::log(ArLog::Normal,
                   "ArSerialConnection::read() error adding msecs (%i)",
                   msWait);
      }
      while (timeDone.mSecTo() >= 0) 
      {
	if (!ClearCommError(myPort, &ret, &stat))
	  return -1;
	if (stat.cbInQue < size)
	  ArUtil::sleep(2);
	else
	  break;
      }
    }
    if (!ClearCommError(myPort, &ret, &stat))
      return -1;
    if (stat.cbInQue == 0)
      return 0;
    if (stat.cbInQue > size)
      numToRead = size;
    else
      numToRead = stat.cbInQue;
    if (ReadFile( myPort, (void *)data, numToRead, &ret, NULL))
    {
      return (int)ret;
    }
    else 
    {
      ArLog::logErrorFromOS(ArLog::Terse, "ArSerialConnection::read:  Read failed.");
      return -1;
    }
  }
  ArLog::log(ArLog::Terse, "ArSerialConnection::read: Connection invalid.");
  return -1;
}

AREXPORT int ArTcpConnection::read(const char *data, unsigned int size, 
				   unsigned int msWait)
{
  ArTime timeDone;
  unsigned int bytesRead = 0;
  int n;

  if (getStatus() != STATUS_OPEN) 
  {
    ArLog::log(ArLog::Terse, 
	       "ArTcpConnection::read: Attempt to use port that is not open.");
    return -1;
  }

  int timeToWait;  
  timeDone.setToNow();
  timeDone.addMSec(msWait);

  do 
  {
    timeToWait = timeDone.mSecTo();
    if (timeToWait < 0)
      timeToWait = 0;
    n = mySocket->read(const_cast<char *>(data) + bytesRead, size - bytesRead,
		       timeToWait);
    /*if (n == -1) 
    {
      ArLog::log("ArTcpConnection::read: read failed.");
      return -1;
      } */
    //printf("%ld %d %d\n", timeDone.mSecTo(), n, size);
    if (n != -1)
      bytesRead += n;
    if (bytesRead >= size)
      return bytesRead;
  } while (timeDone.mSecTo() >= 0);

  return bytesRead;
}

AREXPORT ArNetPacketReceiverTcp::Ret ArNetPacketReceiverTcp::readPacket(int msWait)
{
  long timeToRunFor = -1;
  int numRead = 0;
  bool printing = true;
  int ret = 0;
  unsigned char c = 0;

  ArTime timeDone;
  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::Verbose, "%sBad char in sync1 %d", myLoggingPrefix.c_str(), 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::Verbose, "%sBad char in sync2 %d, returning to sync1",                              myLoggingPrefix.c_str(), 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, 
                     "%sArNetPacketReceiverTcp::readPacket: bad packet length, it is %d which is more than max length of %d bytes or less than the minimum %d", 
                     myLoggingPrefix.c_str(), myReadLength, 
//.........这里部分代码省略.........

bool ArUrg_2_0::internalGetReading(void)
{
  ArTime readingRequested;
  std::string reading;
  char buf[1024];

  reading = "";
  /*
  if (!writeLine(myRequestString))
  {
    ArLog::log(ArLog::Terse, "Could not send request distance reading to urg");
    return false;
  }
  */

  ArTime firstByte;

  if (!readLine(buf, sizeof(buf), 1000, true, false, &firstByte) || 
      strcasecmp(buf, myRequestString) != 0)
  {
    ArLog::log(ArLog::Normal, 
	       "%s: Did not get distance reading response (%s)",
	       getName(), buf);
    return false;
  }
  // TODO this isn't the right time, but for most of what we do that
  // won't matter
  readingRequested.setToNow();
  readingRequested.addMSec(-100);

  if (!readLine(buf, sizeof(buf), 100, false, false) || 
      strcasecmp(buf, "99") != 0)		  
  {
    ArLog::log(ArLog::Normal, 
	       "%s: Bad status on distance reading response (%s)",
	       getName(), buf);
    return false;
  }

  if (!readLine(buf, sizeof(buf), 100, false, false))
  {
    ArLog::log(ArLog::Normal, 
       "%s: Could not read timestamp in distance reading response (%s)",
	       getName(), buf);
    return false;
  }
  
  while (readLine(buf, sizeof(buf), 100, false, false))
  {
    if (strlen(buf) == 0)
    {
      myReadingMutex.lock();
      myReadingRequested = readingRequested;
      myReading = reading;
      myReadingMutex.unlock();	
      if (myRobot == NULL)
	sensorInterp();
      return true;
    }
    else
    {
      reading += buf;
    }
  }

  return false;
}

/**
    @param msWait how long to block for the start of a packet, nonblocking if 0
    @return NULL if there are no packets in alloted time, otherwise a pointer
    to the packet received, if allocatePackets is true than the place that 
    called this function owns the packet and should delete the packet when 
    done... if allocatePackets is false then nothing must store a pointer to
    this packet, the packet must be used and done with by the time this 
    method is called again
*/
AREXPORT ArLMS2xxPacket *ArLMS2xxPacketReceiver::receivePacket(
	unsigned int msWait)
{
  ArLMS2xxPacket *packet;
  unsigned char c;
  char buf[2048];
  long count = 0;
  // state can be one of the STATE_ enums in the class
  int state = STATE_START;
  //unsigned int timeDone;
  //unsigned int curTime;
  long timeToRunFor;
  long packetLength;
  ArTime timeDone;
  ArTime lastDataRead;
  ArTime packetReceived;
  int numRead;


  if (myDeviceConn == NULL || 
      myDeviceConn->getStatus() != ArDeviceConnection::STATUS_OPEN)
  {
    return NULL;
  }
  
  timeDone.setToNow();
  timeDone.addMSec(msWait);
  do
  {
    timeToRunFor = timeDone.mSecTo();
    if (timeToRunFor < 0)
      timeToRunFor = 0;

    if (myDeviceConn->read((char *)&c, 1, timeToRunFor) == 0) 
    {
      if (state == STATE_START)
      {
	return NULL;
      }
      else
      {
	//ArUtil::sleep(1);
	continue;
      }
    }

    //printf("%x\n", c);
    switch (state) {
    case STATE_START:
      if (c == 0x02) // move on, resetting packet
      {
	//printf("###############\n");
	state = STATE_ADDR;
	myPacket.empty();
	myPacket.setLength(0);
	myPacket.uByteToBuf(c);
	packetReceived = myDeviceConn->getTimeRead(0);
	myPacket.setTimeReceived(packetReceived);
      }
      /*else
      {
	//printf(" BAD\n");
	}*/
      break;
    case STATE_ADDR:
      // if this is correct move on, adding this byte... this is taken
      // out in favor of a more inclusive approach, if someone ever
      // wnats to drive multiple robots off of one serial port just
      // put this back in, or I don't know, punt
      //if (c == ((unsigned char)0x80 + myReceivingAddress)) 
      if (!myUseBase0Address && c >= 0x80 && c <= 0x84)
      {
	state = STATE_START_COUNT;
	myPacket.uByteToBuf(c);
      }
      // c will always be >= 0 since its unsigned
      else if (myUseBase0Address && c <= 0x4)
      {
	state = STATE_START_COUNT;
	myPacket.uByteToBuf(c);
      }
      else // go back to beginning, packet hosed
      {
	ArLog::log(ArLog::Terse, 
		   "ArLMS2xxPacketReceiver::receivePacket: wrong address (0x%x instead of 0x%x)", c, (unsigned) 0x80 + myReceivingAddress);
	state = STATE_START;
      }
      break;
    case STATE_START_COUNT:
      packetLength = c;
      myPacket.uByteToBuf(c);
//.........这里部分代码省略.........

AREXPORT bool ArSZSeries::blockingConnect(void) {

	if (!getRunning())
		runAsync();

	myConnMutex.lock();
	if (myConn == NULL) {
		ArLog::log(ArLog::Terse,
				"%s: Could not connect because there is no connection defined",
				getName());
		myConnMutex.unlock();
		failedToConnect();
		return false;
	}


	//				myPrevSensorIntTime = myConn->getTimeRead(0);

	// PS 9/9/11 - moved this here to fix issue with setting baud in mt400.p
	laserPullUnsetParamsFromRobot();
	laserCheckParams();

	// PS 9/9/11 - add setting baud
    ArSerialConnection *serConn = NULL;
	serConn = dynamic_cast<ArSerialConnection *>(myConn);
	if (serConn != NULL)
		serConn->setBaud(atoi(getStartingBaudChoice()));

	if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN
			&& !myConn->openSimple()) {
		ArLog::log(
				ArLog::Terse,
				"%s: Could not connect because the connection was not open and could not open it",
				getName());
		myConnMutex.unlock();
		failedToConnect();
		return false;
	}

	// PS - set logging level and laser type in packet receiver class
	myReceiver.setmyInfoLogLevel(myInfoLogLevel);
	myReceiver.setmyName(getName());

	myReceiver.setDeviceConnection(myConn);
	myConnMutex.unlock();

	lockDevice();
	myTryingToConnect = true;
	unlockDevice();

	// PS 9/9/11 - moved up top
	//laserPullUnsetParamsFromRobot();
	//laserCheckParams();

	int size = ArMath::roundInt((270/.3) + 1);
	ArLog::log(myInfoLogLevel,
			"%s::blockingConnect() Setting current buffer size to %d",
			getName(), size);
	setCurrentBufferSize(size);

	ArTime timeDone;
	if (myPowerControlled)
	{
		if (!timeDone.addMSec(60 * 1000))
		{
			ArLog::log(ArLog::Normal,
					"%s::blockingConnect() error adding msecs (60 * 1000)",
					getName());
		}
	}
	else
	{
		if (!timeDone.addMSec(30 * 1000))
		{
			ArLog::log(ArLog::Normal,
					"%s::blockingConnect() error adding msecs (30 * 1000)",
					getName());
		}
	}


	ArSZSeriesPacket *packet;

	ArSZSeriesPacket sendPacket;

#if 0
	sendPacket.empty();
	sendPacket.uByteToBuf(0xA0); // stop continous sending
	sendPacket.uByteToBuf(0x00);
	sendPacket.uByteToBuf(0x1D);
	sendPacket.uByteToBuf(0x7E);

	sendPacket.finalizePacket();

	if ((myConn->write(sendPacket.getBuf(), sendPacket.getLength())) == -1)
	{
		ArLog::log(ArLog::Terse,
				"%s::blockingConnect() Could not send Stop Continuous mode to laser", getName());
		failedToConnect();
		return false;
//.........这里部分代码省略.........

void ArSZSeries::sensorInterp(void) {
	ArSZSeriesPacket *packet;

	while (1) {
		myPacketsMutex.lock();
		if (myPackets.empty()) {
			myPacketsMutex.unlock();
			return;
		}
		packet = myPackets.front();
		myPackets.pop_front();
		myPacketsMutex.unlock();

		//set up the times and poses

		ArTime time = packet->getTimeReceived();
		
		ArPose pose;
		int ret;
		int retEncoder;
		ArPose encoderPose;
		int dist;
		int j;

		unsigned char *buf = (unsigned char *) packet->getBuf();

		// this value should be found more empirically... but we used 1/75
		// hz for the lms2xx and it was fine, so here we'll use 1/50 hz for now
		if (!time.addMSec(-30)) {
			ArLog::log(ArLog::Normal,
					"%s::sensorInterp() error adding msecs (-30)", getName());
		}

		if (myRobot == NULL || !myRobot->isConnected())
		{
			pose.setPose(0, 0, 0);
			encoderPose.setPose(0, 0, 0);
		}
		else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0
				|| (retEncoder = myRobot->getEncoderPoseInterpPosition(time,
						&encoderPose)) < 0)
		{
			ArLog::log(ArLog::Normal,
					"%s::sensorInterp() reading too old to process", getName());
			delete packet;
			continue;
		}

		ArTransform transform;
		transform.setTransform(pose);

		unsigned int counter = 0;
		if (myRobot != NULL)
			counter = myRobot->getCounter();

		lockDevice();
		myDataMutex.lock();

		//std::list<ArSensorReading *>::reverse_iterator it;
		ArSensorReading *reading;

		myNumChans = packet->getNumReadings();

		double eachAngularStepWidth;
		int eachNumberData;

		// PS - test for SZ-16D, each reading is .36 degrees for 270 degrees

		if (packet->getNumReadings() == 751)
		{
			eachNumberData = packet->getNumReadings();
		}
		else
		{
			ArLog::log(ArLog::Normal,
					"%s::sensorInterp() The number of readings is not correct = %d",
					getName(), myNumChans);

			// PS 12/6/12 - unlock before continuing

			delete packet;
			myDataMutex.unlock();
			unlockDevice();
			continue;
		}

		// If we don't have any sensor readings created at all, make 'em all
		if (myRawReadings->size() == 0) {
			for (j = 0; j < eachNumberData; j++) {
				myRawReadings->push_back(new ArSensorReading);
			}
		}

		if (eachNumberData > myRawReadings->size())
		{
			ArLog::log(ArLog::Terse,
					"%s::sensorInterp() Bad data, in theory have %d readings but can only have 751... skipping this packet",
					getName(), eachNumberData);

			// PS 12/6/12 - unlock and delete before continuing
//.........这里部分代码省略.........

AREXPORT void * ArSyncLoop::runThread(void *arg)
{
  threadStarted();

  long timeToSleep;
  ArTime loopEndTime;
  std::list<ArFunctor *> *runList;
  std::list<ArFunctor *>::iterator iter;
  ArTime lastLoop;
  bool firstLoop = true;
  bool warned = false;

  if (!myRobot)
  {
    ArLog::log(ArLog::Terse, "ArSyncLoop::runThread: Trying to run the synchronous loop without a robot.");
    return(0);
  }

  if (!myRobot->getSyncTaskRoot())
  {
    ArLog::log(ArLog::Terse, "ArSyncLoop::runThread: Can not run the synchronous loop without a task tree");
    return(0);
  }

  while (myRunning)
  {

    myRobot->lock();
    if (!firstLoop && !warned && !myRobot->getNoTimeWarningThisCycle() && 
	myRobot->getCycleWarningTime() != 0 && 
	myRobot->getCycleWarningTime() > 0 && 
	lastLoop.mSecSince() > (signed int) myRobot->getCycleWarningTime())
    {
      ArLog::log(ArLog::Normal, 
 "Warning: ArRobot cycle took too long because the loop was waiting for lock.");
      ArLog::log(ArLog::Normal,
		 "\tThe cycle took %u ms, (%u ms normal %u ms warning)", 
		 lastLoop.mSecSince(), myRobot->getCycleTime(), 
		 myRobot->getCycleWarningTime());
    }
    myRobot->setNoTimeWarningThisCycle(false);
    firstLoop = false;
    warned = false;
    lastLoop.setToNow();

    loopEndTime.setToNow();
    loopEndTime.addMSec(myRobot->getCycleTime());
    myRobot->incCounter();
    myRobot->unlock();

    // note that all the stuff beyond here should maybe have a lock
    // but it should be okay because its just getting data
    myInRun = true;
    myRobot->getSyncTaskRoot()->run();
    myInRun = false;
    if (myStopRunIfNotConnected && !myRobot->isConnected())
    {
      if (myRunning)
	ArLog::log(ArLog::Normal, "Exiting robot run because of lost connection.");
      break;
    }
    timeToSleep = loopEndTime.mSecTo();
    // if the cycles chained and we're connected the packet handler will be 
    // doing the timing for us
    if (myRobot->isCycleChained() && myRobot->isConnected())
      timeToSleep = 0;

    if (!myRobot->getNoTimeWarningThisCycle() && 
	myRobot->getCycleWarningTime() != 0 && 
	myRobot->getCycleWarningTime() > 0 && 
	lastLoop.mSecSince() > (signed int) myRobot->getCycleWarningTime())
    {
      ArLog::log(ArLog::Normal, 
	"Warning: ArRobot sync tasks too long at %u ms, (%u ms normal %u ms warning)", 
		 lastLoop.mSecSince(), myRobot->getCycleTime(), 
		 myRobot->getCycleWarningTime());
      warned = true;
    }
    

    if (timeToSleep > 0)
      ArUtil::sleep(timeToSleep);
  }   
  myRobot->lock();
  myRobot->wakeAllRunExitWaitingThreads();
  myRobot->unlock();

  myRobot->lock();
  runList=myRobot->getRunExitListCopy();
  myRobot->unlock();
  for (iter=runList->begin();
       iter != runList->end(); ++iter)
    (*iter)->invoke();
  delete runList;

  threadFinished();
  return(0);
}

AREXPORT void ArRangeDevice::adjustRawReadings(bool interlaced)
{
  std::list<ArSensorReading *>::iterator rawIt;

  // make sure we have raw readings and a robot, and a delay to
  // correct for (note that if we don't have a delay to correct for
  // but have already been adjusting (ie someone changed the delay)
  // we'll just keep adjusting)
  if (myRawReadings == NULL || myRobot == NULL || 
      (myAdjustedRawReadings == NULL && myRobot->getOdometryDelay() == 0))
    return;
  

  // if we don't already have a list then make one
  if (myAdjustedRawReadings == NULL)
    myAdjustedRawReadings = new std::list<ArSensorReading *>;
  
  // if we've already adjusted these readings then don't do it again
  if (myRawReadings->begin() != myRawReadings->end() &&
      myRawReadings->front()->getAdjusted())
    return;

  std::list<ArSensorReading *>::iterator adjIt;
  ArSensorReading *adjReading;
  ArSensorReading *rawReading;

  ArTransform trans;
  ArTransform encTrans;
  ArTransform interlacedTrans;
  ArTransform interlacedEncTrans;

  bool first = true;
  bool second = true;

  int onReading;
  for (rawIt = myRawReadings->begin(), adjIt = myAdjustedRawReadings->begin(), 
       onReading = 0; 
       rawIt != myRawReadings->end(); 
       rawIt++, onReading++)
  {
    rawReading = (*rawIt);
    if (adjIt != myAdjustedRawReadings->end())
    {
      adjReading = (*adjIt);
      adjIt++;
    }
    else
    {
      adjReading = new ArSensorReading;
      myAdjustedRawReadings->push_back(adjReading);
    }
    (*adjReading) = (*rawReading);
    if (first || (interlaced && second))
    {
      ArPose origPose;
      ArPose corPose;
      ArPose origEncPose;
      ArPose corEncPose;
      ArTime corTime;


      corTime = rawReading->getTimeTaken();
      corTime.addMSec(-myRobot->getOdometryDelay());
      if (myRobot->getPoseInterpPosition(corTime, 
					 &corPose) == 1 && 
	  myRobot->getEncoderPoseInterpPosition(corTime, 
						&corEncPose) == 1)
      {
	origPose = rawReading->getPoseTaken();
	origEncPose = rawReading->getEncoderPoseTaken();
	/*
	printf("Difference was %g %g %g (rotVel %.0f, rotvel/40 %g)\n", 
	       origEncPose.getX() - corEncPose.getX(),
	       origEncPose.getY() - corEncPose.getY(),
	       origEncPose.getTh() - corEncPose.getTh(),
	       myRobot->getRotVel(), myRobot->getRotVel() / 40);
	*/
	if (first)
	{
	  trans.setTransform(origPose, corPose);
	  encTrans.setTransform(origEncPose, corEncPose);
	}
	else if (interlaced && second)
	{
	  interlacedTrans.setTransform(origPose, corPose);
	  interlacedEncTrans.setTransform(origEncPose, corEncPose);
	}
      }
      else
      {
	//printf("Couldn't correct\n");
      }

      if (first)
	first = false;
      else if (interlaced && second)
	second = false;

    }
    if (!interlaced && (onReading % 2) == 0)
//.........这里部分代码省略.........

void ArLMS1XX::sensorInterp(void)
{
  ArLMS1XXPacket *packet;
  
  while (1)
  {
    myPacketsMutex.lock();
    if (myPackets.empty())
    {
      myPacketsMutex.unlock();
      return;
    }
    packet = myPackets.front();
    myPackets.pop_front();
    myPacketsMutex.unlock();
	   
    // if its not a reading packet just skip it 
    if (strcasecmp(packet->getCommandName(), "LMDscandata") != 0)
    {
      delete packet;
      continue;
    }

    //set up the times and poses

    ArTime time = packet->getTimeReceived();
    ArPose pose;
    int ret;
    int retEncoder;
    ArPose encoderPose;
    
    // this value should be found more empirically... but we used 1/75
    // hz for the lms2xx and it was fine, so here we'll use 1/50 hz for now
    time.addMSec(-20);
    if (myRobot == NULL || !myRobot->isConnected())
    {
      pose.setPose(0, 0, 0);
      encoderPose.setPose(0, 0, 0);
    } 
    else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
	     (retEncoder = 
	      myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
    {
      ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
      delete packet;
      continue;
    }
    
    ArTransform transform;
    transform.setTransform(pose);
    
    unsigned int counter = 0; 
    if (myRobot != NULL)
      counter = myRobot->getCounter();
    
    lockDevice();
    myDataMutex.lock();
    
    int i;
    int dist;
    //int onStep;
    
    std::list<ArSensorReading *>::reverse_iterator it;
    ArSensorReading *reading;

    // read the extra stuff
    myVersionNumber = packet->bufToUByte2();
    myDeviceNumber = packet->bufToUByte2();
    mySerialNumber = packet->bufToUByte4();
    myDeviceStatus1 = packet->bufToUByte();
    myDeviceStatus2 = packet->bufToUByte();
    myMessageCounter = packet->bufToUByte2();
    myScanCounter = packet->bufToUByte2();
    myPowerUpDuration = packet->bufToUByte4();
    myTransmissionDuration = packet->bufToUByte4();
    myInputStatus1 = packet->bufToUByte();
    myInputStatus2 = packet->bufToUByte();
    myOutputStatus1 = packet->bufToUByte();
    
    myOutputStatus2 = packet->bufToUByte();
    myReserved = packet->bufToUByte2();
    myScanningFreq = packet->bufToUByte4();
    myMeasurementFreq = packet->bufToUByte4();

    if (myDeviceStatus1 != 0 || myDeviceStatus2 != 0)
      ArLog::log(myLogLevel, "%s: DeviceStatus %d %d", 
		 myDeviceStatus1, myDeviceStatus2); 

    /*
      printf("Received: %s %s ver %d devNum %d serNum %d scan %d sf %d mf %d\n", 
	   packet->getCommandType(), packet->getCommandName(), 
	   myVersionNumber, myDeviceNumber, 
	   mySerialNumber, myScanCounter, myScanningFreq, myMeasurementFreq);
    */
    myNumberEncoders = packet->bufToUByte2();
    //printf("\tencoders %d\n", myNumberEncoders);
    if (myNumberEncoders > 0)
      ArLog::log(myLogLevel, "%s: Encoders %d", getName(), myNumberEncoders);

    for (i = 0; i < myNumberEncoders; i++)
//.........这里部分代码省略.........

AREXPORT int ArLogFileConnection::read(const char *data, unsigned int size, 
				   unsigned int msWait)
{
  ArTime timeDone;
  unsigned int bytesRead = 0;
  int n;

  if (getStatus() != STATUS_OPEN) 
  {
    ArLog::log(ArLog::Terse, 
	       "ArLogFileConnection::read: Attempt to use port that is not open.");
    return -1;
  }
  
  timeDone.setToNow();
  timeDone.addMSec(msWait);

  if (stopAfter-- <= 0)
    {
      stopAfter= 1;
      return 0;
    }

  if (myFD != NULL)
    {
      char line[1000];
      if (fgets(line, 1000, myFD) == NULL) // done with file, close
        {
          close();
          return -1;
        }
      // parse the line
      int i=0;
      n = 0;
      while (line[i] != 0)
        {
          if (isdigit(line[i]))
            {
              if (isdigit(line[i+1]))
                {
                  if (isdigit(line[i+2]))
                    {
                      const_cast<char *>(data)[n++] = 
                        100 * (line[i]-'0') + 10*(line[i+1]-'0') + line[i+2]-'0';
                      i++;
                    }
                  else
                      const_cast<char *>(data)[n++] = 10*(line[i]-'0') + line[i+1]-'0';
                  i++;
                }
              else
                const_cast<char *>(data)[n++] = line[i]-'0';
            }
          i++;
        }
    }

#if 0
  if (n > 0)                    // add in checksum
    {
      int i;
      unsigned char nn;
      int c = 0;

      i = 3;
      nn = data[2] - 2;
      while (nn > 1) 
        {
          c += ((unsigned char)data[i]<<8) | (unsigned char)data[i+1];
          c = c & 0xffff;
          nn -= 2;
          i += 2;
        }
      if (nn > 0) 
        c = c ^ (int)((unsigned char) data[i]);

      const_cast<char *>(data)[n++] = (c << 8) & 0xff;
      const_cast<char *>(data)[n++] = c & 0xff;
    }
#endif

  bytesRead = n;
  return bytesRead;
}

/**
    @param msWait how long to block for the start of a packet, nonblocking if 0
    @return NULL if there are no packets in alloted time, otherwise a pointer
    to the packet received, if allocatePackets is true than the place that 
    called this function owns the packet and should delete the packet when 
    done... if allocatePackets is false then nothing must store a pointer to
    this packet, the packet must be used and done with by the time this 
    method is called again
*/
AREXPORT ArRobotPacket *ArRobotPacketReceiver::receivePacket(
	unsigned int msWait)
{
  ArRobotPacket *packet;
  unsigned char c;
  char buf[256];
  int count = 0;
  // state can be one of the STATE_ enums in the class
  int state = STATE_SYNC1;
  //unsigned int timeDone;
  //unsigned int curTime;
  long timeToRunFor;
  ArTime timeDone;
  ArTime lastDataRead;
  ArTime packetReceived;
  int numRead;

  if (myAllocatePackets)
    packet = new ArRobotPacket(mySync1, mySync2);
  else
    packet = &myPacket;

  if (packet == NULL || myDeviceConn == NULL || 
      myDeviceConn->getStatus() != ArDeviceConnection::STATUS_OPEN)
  {
    if (myAllocatePackets)
      delete packet;
    return NULL;
  }
  
  timeDone.setToNow();
  timeDone.addMSec(msWait);

  // check for log file connection, return assembled packet
  if (dynamic_cast<ArLogFileConnection *>(myDeviceConn))
  {
    packet->empty();
    packet->setLength(0);
    packetReceived = myDeviceConn->getTimeRead(0);
    packet->setTimeReceived(packetReceived);
    numRead = myDeviceConn->read(buf, 255, 0);
    if (numRead > 0)
    {
      packet->dataToBuf(buf, numRead);
      packet->resetRead();
      return packet;
    }
    else
    {
      if (myAllocatePackets)
	delete packet;
      return NULL;
    }
  }      
  

  do
    {
      timeToRunFor = timeDone.mSecTo();
      if (timeToRunFor < 0)
        timeToRunFor = 0;

      if (myDeviceConn->read((char *)&c, 1, timeToRunFor) == 0) 
        {
          if (state == STATE_SYNC1)
            {
              if (myAllocatePackets)
                delete packet;
              return NULL;
            }
          else
            {
              //ArUtil::sleep(1);
              continue;
            }
        }

      switch (state) {
      case STATE_SYNC1:
        if (c == mySync1) // move on, resetting packet
          {
            state = STATE_SYNC2;
            packet->empty();
            packet->setLength(0);
            packet->uByteToBuf(c);
            packetReceived = myDeviceConn->getTimeRead(0);
            packet->setTimeReceived(packetReceived);
          }
        else
	{
          //printf("Bad sync1 %d\n", c);
//.........这里部分代码省略.........

ArSZSeriesPacket *ArSZSeriesPacketReceiver::receivePacket(unsigned int msWait,
		bool startMode) {

	ArSZSeriesPacket *packet;
	unsigned char c;
	long timeToRunFor;
	ArTime timeDone;
	ArTime lastDataRead;
	ArTime packetReceived;
	int i;

	if (myConn == NULL || myConn->getStatus()
			!= ArDeviceConnection::STATUS_OPEN) {
		return NULL;
	}

	timeDone.setToNow();
	if (!timeDone.addMSec(msWait)) {
		ArLog::log(ArLog::Terse, "%s::receivePacket() error adding msecs (%i)",
				myName, msWait);
	}
	//msWait = 100;

	do {
		timeToRunFor = timeDone.mSecTo();
		if (timeToRunFor < 0)
			timeToRunFor = 0;
		/*
		 ArLog::log(ArLog::Terse,
		 "%s::receivePacket() timeToRunFor = %d",
		 myName, timeToRunFor);
		 */

		myPacket.empty();
		myPacket.setLength(0);
		myReadCount = 0;

		unsigned char firstbytelen;
		unsigned char secondbytelen;
		unsigned char temp[4];

		unsigned char crcbuf[10000];
		memset(crcbuf, 0, 10000);
		memset(myReadBuf, 0, 100000);
		int n = 0;
#if 0
		bool nonzero = true;
		int zerocnt = 0;
		char prev_c = 0x30;
		while (nonzero)
		{
			if ((myConn->read((char *) &c, 1, msWait)) > 0)
			{
				if (((c == 0x00) && (zerocnt == 0)) || ((c == 0x00) && (prev_c == 0x00)))
				{
					zerocnt++;
					prev_c = c;
					if (zerocnt == 4)
			          nonzero = false;
				}
				else
				{
					zerocnt = 0;
					prev_c = 0x30;
				}
		    }
		} // endwhile

		//printf("WE FOUND A 4 ZERO's\n");
		packetReceived = myConn->getTimeRead(0);
		myPacket.setTimeReceived(packetReceived);
#endif
//#if 0
		// look for initial sequence 0x00 0x00 0x00 0x00
		for (i = 0; i < 4; i++) {
			if ((myConn->read((char *) &c, 1, msWait)) > 0) {
				if (c != 0x00) {
					//printf("char = %x\n",c);
					//ArLog::log(ArLog::Terse,
					 //                "ArSZSeries::receivePacket() error reading first 4 bytes of header");
					break;
				}
				if (i == 0) {
					packetReceived = myConn->getTimeRead(0);
					//ArTime previousTime = myPacket.getTimeReceived();
					myPacket.setTimeReceived(packetReceived);
					//ArLog::log(ArLog::Normal,
					//		"ms since = %d",
					//		packetReceived.mSecSince(previousTime));
				}
			} else {
				/* don't log this if we are in starting mode, means laser is not connecting */
				if (startMode)
					ArLog::log(ArLog::Terse,
							"%s::receivePacket() myConn->read error (header)",
							myName);
				return NULL;
			}
		} // end for

//.........这里部分代码省略.........

int main(void)
{
  printf("\nTesting platform localtime (broken-down) struct:\n");
  //struct tm t;
  //ArUtil::localtime(&t);

  struct tm t;
  ArUtil::localtime(&t);
  printf("ArUtil::localtime() returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n",
    t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
  time_t yesterday = time(NULL) - (24*60*60) ;
  ArUtil::localtime(&yesterday, &t);
  printf("ArUtil::localtime(time(NULL) - 24hours, struct tm*) returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n",
    t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);

  printf("\nCurrent time as strings:\n");
  char year[5], month[3], day[3], hour[3], min[3], sec[3];
  ArUtil::putCurrentYearInString(year, 5);
  ArUtil::putCurrentMonthInString(month, 3);
  ArUtil::putCurrentDayInString(day, 3);
  ArUtil::putCurrentHourInString(hour, 3);
  ArUtil::putCurrentMinuteInString(min, 3);
  ArUtil::putCurrentSecondInString(sec, 3);
  printf("  Year:%s, Month:%s, Day:%s, Hour:%s, Min:%s, Sec:%s\n",
      year, month, day, hour, min, sec);

  printf("\nTesting ArTime:\n");
  ArTime start, test;
  
  printf("Setting an ArTime object \"start\" to now...\n");
  start.setToNow();
  start.log();
  printf("Sleeping 4 secs\n");
  ArUtil::sleep(4000);
  printf("Setting an ArTime object \"test\" to now...\n");
  test.setToNow();
  test.log();

  printf("ms of \"test\" since start %ld\n", test.mSecSince(start));
  printf("seconds \"test\" since start %ld\n", test.secSince(start));
  printf("ms of start since \"test\" %ld\n", start.mSecSince(test));
  printf("seconds \"start\" test %ld\n", start.secSince(test));
  printf("\"start\" is before \"test\"? %d\n", test.isBefore(start));
  printf("\"start\" is after \"test\"? %d\n", test.isAfter(start));
  printf("\"test\" is before \"start\"? %d\n", start.isBefore(test));
  printf("\"test\" is after \"start\"? %d\n", start.isAfter(test));
  printf("ms from \"start\" to now %ld\n", start.mSecTo());
  printf("s from \"start\" to now %ld\n", start.secTo());
  printf("ms since \"start\" %ld\n", start.mSecSince());
  printf("s since \"start\" %ld\n", start.secSince());
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  printf("s from \"test\" stamp to now %ld\n", test.secTo());

  printf("Testing addMSec, adding 200 mSec\n");
  test.addMSec(200);
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  printf("Testing addMSec, subtracting 300 mSec\n");
  test.addMSec(-300);
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  printf("Testing addMSec, adding 20.999 seconds\n");
  test.addMSec(20999);
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  printf("Testing addMSec, subtracting 23.5 seconds\n");
  test.addMSec(-23500);
  printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
  
  ArTime timeDone;
  printf("Setting ArTime object \"done\" to now.\n");
  timeDone.setToNow();
  timeDone.addMSec(1000);
  printf("Making sure the add works in the right direction, adding a second to a timestamp set now\n");
  printf("Reading: %ld\n", timeDone.mSecTo());
  printf("Sleeping 20 ms\n");
  ArUtil::sleep(20);
  printf("Reading: %ld\n", timeDone.mSecTo());
  printf("Sleeping 2 seconds\n");
  ArUtil::sleep(2000);
  printf("Reading: %ld\n", timeDone.mSecTo());

  puts("\nslamming ArUtil::localtime() from a bunch of threads with the same input time...");
  time_t now = time(NULL);
  class LocaltimeTestThread : public virtual ArASyncTask 
  {
  private:
    time_t time;
  public:
    LocaltimeTestThread(time_t t) : time(t) {}
    virtual void *runThread(void *) {
      struct tm t;
      ArUtil::localtime(&time, &t);
      printf("ArUtil::localtime() returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n", t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
      return 0;
    }
  };
  for(int i = 0; i < 200; ++i)
    (new LocaltimeTestThread(now))->runAsync();

  ArUtil::sleep(5000);

  printf("test is done.\n");
//.........这里部分代码省略.........