本文整理汇总了C++中ArTime::isAt方法的典型用法代码示例。如果您正苦于以下问题：C++ ArTime::isAt方法的具体用法？C++ ArTime::isAt怎么用？C++ ArTime::isAt使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ArTime的用法示例。
在下文中一共展示了ArTime::isAt方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: getPose
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);
//.........这里部分代码省略.........
开发者ID:eilo，项目名称:Evolucion-Artificial-y-Robotica-Autonoma-en-Robots-Pioneer-P3-DX，代码行数:101，代码来源:ArInterpolation.cpp