void ArLaserFilter::processReadings(void)
{
myLaser->lockDevice();
selfLockDevice();
const std::list<ArSensorReading *> *rdRawReadings;
std::list<ArSensorReading *>::const_iterator rdIt;
if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
{
selfUnlockDevice();
myLaser->unlockDevice();
return;
}
size_t rawSize = myRawReadings->size();
size_t rdRawSize = myLaser->getRawReadings()->size();
while (rawSize < rdRawSize)
{
myRawReadings->push_back(new ArSensorReading);
rawSize++;
}
// set where the pose was taken
myCurrentBuffer.setPoseTaken(
myLaser->getCurrentRangeBuffer()->getPoseTaken());
myCurrentBuffer.setEncoderPoseTaken(
myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());
std::list<ArSensorReading *>::iterator it;
ArSensorReading *rdReading;
ArSensorReading *reading;
#ifdef DEBUGRANGEFILTER
FILE *file = NULL;
file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
#endif
std::map<int, ArSensorReading *> readingMap;
int numReadings = 0;
// first pass to copy the readings and put them into a map
for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
rdIt != rdRawReadings->end() && it != myRawReadings->end();
rdIt++, it++)
{
rdReading = (*rdIt);
reading = (*it);
*reading = *rdReading;
readingMap[numReadings] = reading;
numReadings++;
}
char buf[1024];
int i;
int j;
ArSensorReading *lastAddedReading = NULL;
// now walk through the readings to filter them
for (i = 0; i < numReadings; i++)
{
reading = readingMap[i];
// if we're ignoring this reading then just get on with life
if (reading->getIgnoreThisReading())
continue;
if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
{
reading->setIgnoreThisReading(true);
continue;
}
if (lastAddedReading != NULL)
{
if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f too close from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
reading->setIgnoreThisReading(true);
continue;
}
#ifdef DEBUGRANGEFILTER
else if (file != NULL)
fprintf(file, "%.1f from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
}
buf[0] = '\0';
bool goodAll = true;
bool goodAny = false;
if (myAnyFactor <= 0)
goodAny = true;
for (j = i - 1;
(j >= 0 && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j--)
{
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d*", buf, reading->getRange());
#endif
for (j = i + 1;
(j < numReadings && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j++)
{
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
}
if (!goodAll || !goodAny)
reading->setIgnoreThisReading(true);
else
lastAddedReading = reading;
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file,
"%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(),
good ? 'g' : 'b', buf);
#endif
}
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
laserProcessReadings();
selfUnlockDevice();
myLaser->unlockDevice();
}
void ArLaserFilter::processReadings(void)
{
myLaser->lockDevice();
selfLockDevice();
const std::list<ArSensorReading *> *rdRawReadings;
std::list<ArSensorReading *>::const_iterator rdIt;
if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
{
selfUnlockDevice();
myLaser->unlockDevice();
return;
}
size_t rawSize = myRawReadings->size();
size_t rdRawSize = myLaser->getRawReadings()->size();
while (rawSize < rdRawSize)
{
myRawReadings->push_back(new ArSensorReading);
rawSize++;
}
// set where the pose was taken
myCurrentBuffer.setPoseTaken(
myLaser->getCurrentRangeBuffer()->getPoseTaken());
myCurrentBuffer.setEncoderPoseTaken(
myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());
std::list<ArSensorReading *>::iterator it;
ArSensorReading *rdReading;
ArSensorReading *reading;
#ifdef DEBUGRANGEFILTER
FILE *file = NULL;
//file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
file = ArUtil::fopen("/tmp/filter", "w");
#endif
std::map<int, ArSensorReading *> readingMap;
int numReadings = 0;
// first pass to copy the readings and put them into a map
for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
rdIt != rdRawReadings->end() && it != myRawReadings->end();
rdIt++, it++)
{
rdReading = (*rdIt);
reading = (*it);
*reading = *rdReading;
readingMap[numReadings] = reading;
numReadings++;
}
// if we're not doing any filtering, just short circuit out now
if (myAllFactor <= 0 && myAnyFactor <= 0 && myAnyMinRange <= 0)
{
laserProcessReadings();
copyReadingCount(myLaser);
selfUnlockDevice();
myLaser->unlockDevice();
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
return;
}
char buf[1024];
int i;
int j;
//ArSensorReading *lastAddedReading = NULL;
// now walk through the readings to filter them
for (i = 0; i < numReadings; i++)
{
reading = readingMap[i];
// if we're ignoring this reading then just get on with life
if (reading->getIgnoreThisReading())
continue;
/* Taking this check out since the base class does it now and if
* it gets marked ignore now it won't get used for clearing
* cumulative readings
if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f beyond max range at %d\n",
reading->getSensorTh(), reading->getRange());
#endif
reading->setIgnoreThisReading(true);
continue;
}
*/
if (myAnyMinRange >= 0 && reading->getRange() < myAnyMinRange &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle))
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f within min range at %d\n",
reading->getSensorTh(), reading->getRange());
#endif
reading->setIgnoreThisReading(true);
continue;
}
/*
if (lastAddedReading != NULL)
{
if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50)
{
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file, "%.1f too close from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
reading->setIgnoreThisReading(true);
continue;
}
#ifdef DEBUGRANGEFILTER
else if (file != NULL)
fprintf(file, "%.1f from last %6.0f\n",
reading->getSensorTh(),
lastAddedReading->getPose().findDistanceTo(
reading->getPose()));
#endif
}
*/
buf[0] = '\0';
bool goodAll = true;
bool goodAny = false;
bool goodMinRange = true;
if (myAnyFactor <= 0)
goodAny = true;
for (j = i - 1;
(j >= 0 && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j--)
{
/* You can't skip these, or you get onesided filtering
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
*/
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
if (myAnyMinRange > 0 &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) &&
readingMap[j]->getRange() <= myAnyMinRange)
goodMinRange = false;
}
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d*", buf, reading->getRange());
#endif
for (j = i + 1;
(j < numReadings && //good &&
fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
reading->getSensorTh())) <= myAngleToCheck);
j++)
{
// you can't ignore these or you get one sided filtering
/*
if (readingMap[j]->getIgnoreThisReading())
{
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6s", buf, "i");
#endif
continue;
}
*/
#ifdef DEBUGRANGEFILTER
sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
if (myAllFactor > 0 &&
!checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAllFactor))
goodAll = false;
if (myAnyFactor > 0 &&
checkRanges(reading->getRange(),
readingMap[j]->getRange(), myAnyFactor))
goodAny = true;
if (myAnyMinRange > 0 &&
(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) &&
readingMap[j]->getRange() <= myAnyMinRange)
goodMinRange = false;
}
if (!goodAll || !goodAny || !goodMinRange)
reading->setIgnoreThisReading(true);
/*
else
lastAddedReading = reading;
*/
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fprintf(file,
"%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(),
goodAll && goodAny && goodMinRange ? 'g' : 'b', buf);
#endif
}
#ifdef DEBUGRANGEFILTER
if (file != NULL)
fclose(file);
#endif
laserProcessReadings();
copyReadingCount(myLaser);
selfUnlockDevice();
myLaser->unlockDevice();
}