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);
}
myDataMutex.unlock();
int previous = getCumulativeBuffer()->size();
laserProcessReadings();
int now = getCumulativeBuffer()->size();
unlockDevice();
}
void ArLaser::internalProcessReading(double x, double y,
unsigned int range, bool clean,
bool onlyClean)
{
if (myCumulativeBuffer.getSize() == 0)
return;
// make sure we really want to clean
if (clean && myCumulativeCleanDistSquared < 1)
clean = false;
std::list<ArPoseWithTime *>::iterator cit;
bool addReading = true;
//double squaredDist;
ArLineSegment line;
double xTaken = myCurrentBuffer.getPoseTaken().getX();
double yTaken = myCurrentBuffer.getPoseTaken().getY();
ArPose intersection;
ArPoseWithTime reading(x, y);
// if we're not cleaning and its further than we're keeping track of
// readings ignore it... replaced with the part thats 'until here'
/*
if (!clean &&
myMaxInsertDistCumulative > 1 &&
range > myMaxInsertDistCumulative)
return;
*/
if (onlyClean)
addReading = false;
if (myMaxInsertDistCumulative > 1 &&
range > myMaxInsertDistCumulative)
addReading = false;
if (!clean && !addReading)
return;
// until here
// if we're cleaning we start our sweep
if (clean)
myCumulativeBuffer.beginInvalidationSweep();
// run through all the readings
for (cit = getCumulativeBuffer()->begin();
cit != getCumulativeBuffer()->end();
++cit)
{
// if its closer to a reading than the filter near dist, just return
if (addReading && myMinDistBetweenCumulativeSquared < .0000001 ||
(ArMath::squaredDistanceBetween(x, y, (*cit)->getX(), (*cit)->getY()) <
myMinDistBetweenCumulativeSquared))
{
// if we're not cleaning it and its too close just return,
// otherwise keep going (to clear out invalid readings)
if (!clean)
return;
addReading = false;
}
// see if this reading invalidates some other readings by coming too close
if (clean)
{
// set up our line
line.newEndPoints(x, y, xTaken, yTaken);
// see if the cumulative buffer reading perpindicular intersects
// this line segment, and then see if its too close if it does,
// but if the intersection is very near the endpoint then leave it
if (line.getPerpPoint((*cit), &intersection) &&
(intersection.squaredFindDistanceTo(*(*cit)) <
myCumulativeCleanDistSquared) &&
(intersection.squaredFindDistanceTo(reading) >
50 * 50))
{
//printf("Found one too close to the line\n");
myCumulativeBuffer.invalidateReading(cit);
}
}
}
// if we're cleaning finish the sweep
if (clean)
myCumulativeBuffer.endInvalidationSweep();
// toss the reading in
if (addReading)
myCumulativeBuffer.addReading(x, y);
}
AREXPORT void ArRangeDevice::filterCallback(void)
{
std::list<ArPoseWithTime *>::iterator it;
lockDevice();
// first filter the current readings based on time
if (myMaxSecondsToKeepCurrent > 0 &&
myCurrentBuffer.getSize() > 0)
{
// just walk through and make sure nothings too far away
myCurrentBuffer.beginInvalidationSweep();
for (it = getCurrentBuffer()->begin();
it != getCurrentBuffer()->end();
++it)
{
if ((*it)->getTime().secSince() >= myMaxSecondsToKeepCurrent)
myCurrentBuffer.invalidateReading(it);
}
myCurrentBuffer.endInvalidationSweep();
}
// okay done with current, now do the cumulative
bool doingDist = true;
bool doingAge = true;
if (myCumulativeBuffer.getSize() == 0)
{
unlockDevice();
return;
}
double squaredFarDist = (myMaxDistToKeepCumulative *
myMaxDistToKeepCumulative);
if (squaredFarDist < 1)
doingDist = false;
if (myMaxSecondsToKeepCumulative <= 0)
doingAge = false;
if (!doingDist && !doingAge)
{
unlockDevice();
return;
}
// just walk through and make sure nothings too far away
myCumulativeBuffer.beginInvalidationSweep();
for (it = getCumulativeBuffer()->begin();
it != getCumulativeBuffer()->end();
++it)
{
// if its closer to a reading than the filter near dist, just return
if (doingDist &&
myRobot->getPose().squaredFindDistanceTo(*(*it)) > squaredFarDist)
myCumulativeBuffer.invalidateReading(it);
else if (doingAge &&
(*it)->getTime().secSince() >= myMaxSecondsToKeepCumulative)
myCumulativeBuffer.invalidateReading(it);
}
myCumulativeBuffer.endInvalidationSweep();
unlockDevice();
}