本文整理汇总了C++中ArSensorReading::newData方法的典型用法代码示例。如果您正苦于以下问题:C++ ArSensorReading::newData方法的具体用法?C++ ArSensorReading::newData怎么用?C++ ArSensorReading::newData使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ArSensorReading
的用法示例。
在下文中一共展示了ArSensorReading::newData方法的5个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: packetHandler
/**
This is the packet handler for the PB9 data, which is sent via the micro
controller, to the client. This will read the data from the packets,
and then call processReadings to filter add the data to the current and
cumulative buffers.
*/
AREXPORT bool ArIrrfDevice::packetHandler(ArRobotPacket *packet)
{
int /*portNum,*/ i, dist, packetCounter;
double conv;
ArTransform packetTrans;
std::list<ArSensorReading *>::iterator it;
ArSensorReading *reading;
ArPose pose;
ArTransform encoderTrans;
ArPose encoderPose;
pose = myRobot->getPose();
conv = 2.88;
packetTrans.setTransform(pose);
packetCounter = myRobot->getCounter();
if (packet->getID() != 0x10)
return false;
// Which Aux port the IRRF is connected to
//portNum =
packet->bufToByte2();
encoderTrans = myRobot->getEncoderTransform();
encoderPose = encoderTrans.doInvTransform(pose);
i = 0;
for (i=0, it = myRawReadings->begin();it != myRawReadings->end();it++, i++)
{
reading = (*it);
dist = (int) ((packet->bufToUByte2()) / conv);
reading->newData(dist, pose, encoderPose, packetTrans, packetCounter, packet->getTimeReceived());
}
myLastReading.setToNow();
processReadings();
return true;
}
示例2: sensorInterp
void ArUrg::sensorInterp(void)
{
ArTime readingRequested;
std::string reading;
myReadingMutex.lock();
if (myReading.empty())
{
myReadingMutex.unlock();
return;
}
readingRequested = myReadingRequested;
reading = myReading;
myReading = "";
myReadingMutex.unlock();
ArTime time = readingRequested;
ArPose pose;
int ret;
int retEncoder;
ArPose encoderPose;
//time.addMSec(-13);
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());
return;
}
ArTransform transform;
transform.setTransform(pose);
unsigned int counter = 0;
if (myRobot != NULL)
counter = myRobot->getCounter();
lockDevice();
myDataMutex.lock();
//double angle;
int i;
int len = reading.size();
int range;
int big;
int little;
//int onStep;
std::list<ArSensorReading *>::reverse_iterator it;
ArSensorReading *sReading;
bool ignore;
for (it = myRawReadings->rbegin(), i = 0;
it != myRawReadings->rend() && i < len - 1;
it++, i += 2)
{
ignore = false;
big = reading[i] - 0x30;
little = reading[i+1] - 0x30;
range = (big << 6 | little);
if (range < 20)
{
/* Well that didn't work...
// if the range is 1 to 5 that means it has low intensity, which
// could be black or maybe too far... try ignoring it and see if
// it helps with too much clearing
if (range >= 1 || range <= 5)
ignore = true;
*/
range = 4096;
}
sReading = (*it);
sReading->newData(range, pose, encoderPose, transform, counter,
time, ignore, 0);
}
myDataMutex.unlock();
laserProcessReadings();
unlockDevice();
}
示例3: sensorInterp
//.........这里部分代码省略.........
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
delete packet;
myDataMutex.unlock();
unlockDevice();
continue;
}
std::list<ArSensorReading *>::iterator it;
double atDeg;
int onReading;
double start;
double increment;
eachAngularStepWidth = .36;
if (myFlipped) {
start = mySensorPose.getTh() + 135;
increment = -eachAngularStepWidth;
} else {
start = -(mySensorPose.getTh() + 135);
increment = eachAngularStepWidth;
}
int readingIndex;
bool ignore = false;
for (atDeg = start,
it = myRawReadings->begin(),
readingIndex = 0,
onReading = 0;
onReading < eachNumberData;
atDeg += increment,
it++,
readingIndex++,
onReading++)
{
reading = (*it);
dist = (((buf[readingIndex * 2] & 0x3f)<< 8) | (buf[(readingIndex * 2) + 1]));
// note max distance is 16383 mm, if the measurement
// object is not there, distance will still be 16383
/*
ArLog::log(ArLog::Normal,
"reading %d first half = 0x%x, second half = 0x%x dist = %d",
readingIndex, buf[(readingIndex *2)+1], buf[readingIndex], dist);
*/
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()), atDeg);
reading->newData(dist, pose, encoderPose, transform, counter, time,
ignore, 0); // no reflector yet
//printf("dist = %d, pose = %d, encoderPose = %d, transform = %d, counter = %d, time = %d, igore = %d",
// dist, pose, encoderPose, transform, counter,
// time, ignore);
}
/*
ArLog::log(ArLog::Normal,
"Received: %s %s scan %d numReadings %d",
packet->getCommandType(), packet->getCommandName(),
myScanCounter, onReading);
*/
myDataMutex.unlock();
/*
ArLog::log(
ArLog::Terse,
"%s::sensorInterp() Telegram number = %d ",
getName(), packet->getTelegramNumByte2());
*/
laserProcessReadings();
unlockDevice();
delete packet;
}
}
示例4: if
void ArUrg_2_0::sensorInterp(void)
{
ArTime readingRequested;
std::string reading;
myReadingMutex.lock();
if (myReading.empty())
{
myReadingMutex.unlock();
return;
}
readingRequested = myReadingRequested;
reading = myReading;
myReading = "";
myReadingMutex.unlock();
ArTime time = readingRequested;
ArPose pose;
int ret;
int retEncoder;
ArPose encoderPose;
//time.addMSec(-13);
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());
return;
}
ArTransform transform;
transform.setTransform(pose);
unsigned int counter = 0;
if (myRobot != NULL)
counter = myRobot->getCounter();
lockDevice();
myDataMutex.lock();
//double angle;
int i;
int len = reading.size();
int range;
int giant;
int big;
int little;
//int onStep;
std::list<ArSensorReading *>::reverse_iterator it;
ArSensorReading *sReading;
int iMax;
int iIncr;
if (myUseThreeDataBytes)
{
iMax = len - 2;
iIncr = 3;
}
else
{
iMax = len - 1;
iIncr = 2;
}
bool ignore;
for (it = myRawReadings->rbegin(), i = 0;
it != myRawReadings->rend() && i < iMax; //len - 2;
it++, i += iIncr) //3)
{
ignore = false;
if (myUseThreeDataBytes)
{
giant = reading[i] - 0x30;
big = reading[i+1] - 0x30;
little = reading[i+2] - 0x30;
range = (giant << 12 | big << 6 | little);
}
else
{
big = reading[i] - 0x30;
little = reading[i+1] - 0x30;
range = (big << 6 | little);
}
if (range < myDMin)
range = myDMax+1;
sReading = (*it);
sReading->newData(range, pose, encoderPose, transform, counter,
time, ignore, 0);
//.........这里部分代码省略.........
示例5: if
//.........这里部分代码省略.........
*/
/*
printf("\t\t%s\tscl %d %d ang %g %g num %d\n",
eachChanMeasured,
eachScalingFactor, eachScalingOffset,
eachStartingAngle, eachAngularStepWidth,
eachNumberData);
*/
// If we don't have any sensor readings created at all, make 'em all
if (myRawReadings->size() == 0)
for (i = 0; i < eachNumberData; i++)
myRawReadings->push_back(new ArSensorReading);
if (eachNumberData > myRawReadings->size())
{
ArLog::log(ArLog::Terse, "%s: Bad data, in theory have %d readings but can only have 541... skipping this packet\n", getName(), eachNumberData);
printf("%s\n", packet->getBuf());
continue;
}
std::list<ArSensorReading *>::iterator it;
double atDeg;
int onReading;
double start;
double increment;
if (myFlipped)
{
start = mySensorPose.getTh() + eachStartingAngle - 90.0 + eachAngularStepWidth * eachNumberData;
increment = -eachAngularStepWidth;
}
else
{
start = mySensorPose.getTh() + eachStartingAngle - 90.0;
increment = eachAngularStepWidth;
}
bool ignore;
for (//atDeg = mySensorPose.getTh() + eachStartingAngle - 90.0,
//atDeg = mySensorPose.getTh() + eachStartingAngle - 90.0 + eachAngularStepWidth * eachNumberData,
atDeg = start,
it = myRawReadings->begin(),
onReading = 0;
onReading < eachNumberData;
//atDeg += eachAngularStepWidth,
//atDeg -= eachAngularStepWidth,
atDeg += increment,
it++,
onReading++)
{
ignore = false;
if (atDeg < getStartDegrees() || atDeg > getEndDegrees())
ignore = true;
reading = (*it);
dist = packet->bufToUByte2();
if (dist == 0)
{
ignore = true;
}
/*
else if (!ignore && dist < 150)
{
//ignore = true;
ArLog::log(ArLog::Normal, "%s: Reading at %.1f %s is %d (not ignoring, just warning)",
getName(), atDeg,
eachChanMeasured, dist);
}
*/
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()),
atDeg);
reading->newData(dist, pose, encoderPose, transform, counter,
time, ignore, 0); // no reflector yet
}
/*
ArLog::log(ArLog::Normal,
"Received: %s %s scan %d numReadings %d",
packet->getCommandType(), packet->getCommandName(),
myScanCounter, onReading);
*/
}
myDataMutex.unlock();
laserProcessReadings();
unlockDevice();
delete packet;
}
}