AREXPORT void ArSonarDevice::processReadings(void)
{
int i;
ArSensorReading *reading;
lockDevice();
for (i = 0; i < myRobot->getNumSonar(); i++)
{
reading = myRobot->getSonarReading(i);
if (reading == NULL || !reading->isNew(myRobot->getCounter()))
continue;
addReading(reading->getX(), reading->getY());
}
// delete too-far readings
std::list<ArPoseWithTime *> *readingList;
std::list<ArPoseWithTime *>::iterator it;
double dx, dy, rx, ry;
myCumulativeBuffer.beginInvalidationSweep();
readingList = myCumulativeBuffer.getBuffer();
rx = myRobot->getX();
ry = myRobot->getY();
// walk through the list and see if this makes any old readings bad
if (readingList != NULL)
{
for (it = readingList->begin(); it != readingList->end(); ++it)
{
dx = (*it)->getX() - rx;
dy = (*it)->getY() - ry;
if ((dx*dx + dy*dy) > (myFilterFarDist * myFilterFarDist))
myCumulativeBuffer.invalidateReading(it);
}
}
myCumulativeBuffer.endInvalidationSweep();
// leave this unlock here or the world WILL end
unlockDevice();
}
AREXPORT void ArIrrfDevice::processReadings(void)
{
//int i;
double rx, ry, nx, ny, dx, dy, dist;
ArSensorReading *reading;
std::list<ArSensorReading *>::iterator rawIt;
std::list<ArPoseWithTime *> *readingList;
std::list<ArPoseWithTime *>::iterator readIt;
lockDevice();
rx = myRobot->getX();
ry = myRobot->getY();
//i=0;
for (rawIt = myRawReadings->begin();rawIt != myRawReadings->end();rawIt++)
{
reading = (*rawIt);
nx = reading->getX();
ny = reading->getY();
dx = nx - rx;
dy = nx - ry;
dist = (dx*dx) + (dy*dy);
if (!reading->isNew(myRobot->getCounter()))
continue;
if (dist < (myMaxRange * myMaxRange))
myCurrentBuffer.addReading(nx, ny);
if (dist < (myCumulativeMaxRange * myCumulativeMaxRange))
{
myCumulativeBuffer.beginInvalidationSweep();
readingList = myCumulativeBuffer.getBuffer();
if (readingList != NULL)
{
for (readIt = readingList->begin();
readIt != readingList->end();
readIt++)
{
dx = (*readIt)->getX() - nx;
dy = (*readIt)->getY() - ny;
if ((dx*dx + dy*dy) < (myFilterNearDist * myFilterNearDist))
myCumulativeBuffer.invalidateReading(readIt);
}
}
myCumulativeBuffer.endInvalidationSweep();
myCumulativeBuffer.addReading(nx, ny);
}
}
readingList = myCumulativeBuffer.getBuffer();
rx = myRobot->getX();
ry = myRobot->getY();
myCumulativeBuffer.beginInvalidationSweep();
if (readingList != NULL)
{
for (readIt = readingList->begin(); readIt != readingList->end();readIt++)
{
dx = (*readIt)->getX() - rx;
dy = (*readIt)->getY() - ry;
if ((dx*dx + dy*dy) > (myFilterFarDist * myFilterFarDist))
myCumulativeBuffer.invalidateReading(readIt);
}
}
myCumulativeBuffer.endInvalidationSweep();
unlockDevice();
}
