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

/**
   Initialize the P2 Arm class. This must be called before anything else. The
   setRobot() must be called to let ArP2Arm know what instance of an ArRobot
   to use. It talks to the robot and makes sure that there is an arm on it
   and it is in a good condition. The AROS/P2OS arm servers take care of AUX
   port serial communications with the P2 Arm controller.
*/
AREXPORT ArP2Arm::State ArP2Arm::init()
{
  ArLog::log(ArLog::Normal, "Initializing the arm.");

  ArTime now;

  if (myInited)
    return(ALREADY_INITED);

  if (!myRobot || !myRobot->isRunning() || !myRobot->isConnected())
    return(ROBOT_NOT_SETUP);

  Aria::addUninitCallBack(&myAriaUninitCB, ArListPos::FIRST);
  ArLog::log(ArLog::Verbose, "Adding the P2 Arm packet handler.");
  myRobot->addPacketHandler(&myArmPacketHandler, ArListPos::FIRST);
  now.setToNow();
  if (!comArmStats(StatusSingle))
    return(COMM_FAILED);
  ArUtil::sleep(100);
  if (!comArmInfo())
    return(COMM_FAILED);
  ArUtil::sleep(300);

  if (!now.isAfter(myLastStatusTime) || !now.isAfter(myLastInfoTime))
    return(COMM_FAILED);

  if (!(myStatus & ArmGood))
    return(NO_ARM_FOUND);

  myInited=true;

  return(SUCCESS);
}

AREXPORT int ArInterpolation::getPose(ArTime timeStamp,
					  ArPose *position)
{
  std::list<ArTime>::iterator tit;
  std::list<ArPose>::iterator pit;
  
  ArPose thisPose;
  ArTime thisTime;
  ArPose lastPose;
  ArTime lastTime;

  ArTime nowTime;
  long total;
  long toStamp;
  double percentage;
  ArPose retPose;
  
  myDataMutex.lock();
  // find the time we want
  for (tit = myTimes.begin(), pit = myPoses.begin();
       tit != myTimes.end() && pit != myPoses.end(); 
       ++tit, ++pit)
  {
    lastTime = thisTime;
    lastPose = thisPose;

    thisTime = (*tit);
    thisPose = (*pit);

    //printf("## %d %d %d b %d at %d after %d\n", timeStamp.getMSec(), thisTime.getMSec(), timeStamp.mSecSince(thisTime), timeStamp.isBefore(thisTime), timeStamp.isAt(thisTime), timeStamp.isAfter(thisTime));
    //if (timeStamp.isBefore(thisTime) || timeStamp.isAt(thisTime))
    if (!timeStamp.isAfter(thisTime))
    {
      //printf("Found one!\n");
      break;
    } 

  }
  // if we're at the end then it was too long ago
  if (tit == myTimes.end() || pit == myPoses.end())
  {
    //printf("Too old\n");
    myDataMutex.unlock();
    return -2;
  }
  
  // this is for forecasting (for the brave)
  if ((tit == myTimes.begin() || pit == myPoses.begin()) && 
      !timeStamp.isAt((*tit)))
  {
    //printf("Too new %d %d\n", tit == myTimes.begin(), pit == myPoses.begin());
  
    thisTime = (*tit);
    thisPose = (*pit);
    tit++;
    pit++;  
    if (tit == myTimes.end() || pit == myPoses.end())
    {
      //printf("Not enough data\n");
      myDataMutex.unlock();
      return -3;
    }
    lastTime = (*tit);
    lastPose = (*pit);
    nowTime.setToNow();
    total = thisTime.mSecSince(lastTime);
    if (total == 0)
      total = 100;
    toStamp = nowTime.mSecSince(thisTime);
    percentage = (double)toStamp/(double)total;
    //printf("Total time %d, to stamp %d, percentage %.2f\n", total, toStamp, percentage);
    if (percentage > 50)
    {
      myDataMutex.unlock();
      return -1;
    }

    retPose.setX(thisPose.getX() + 
		 (thisPose.getX() - lastPose.getX()) * percentage);
    retPose.setY(thisPose.getY() + 
		 (thisPose.getY() - lastPose.getY()) * percentage);
    retPose.setTh(ArMath::addAngle(thisPose.getTh(),
				   ArMath::subAngle(thisPose.getTh(),
						    lastPose.getTh())
				   * percentage));
    *position = retPose;
    myDataMutex.unlock();
    return 0;
  }

  // this is the actual interpolation

  //printf("Woo hoo!\n");

  total = thisTime.mSecSince(lastTime);
  toStamp = thisTime.mSecSince(timeStamp);
  percentage = (double)toStamp/(double)total;
  //printf("Total time %d, to stamp %d, percentage %.2f\n", 	 total, toStamp, percentage);
  retPose.setX(thisPose.getX() + 
	      (lastPose.getX() - thisPose.getX()) * percentage); 
//.........这里部分代码省略.........