AREXPORT void ArLaserReflectorDevice::processReadings(void)
{
//int i;
ArSensorReading *reading;
myLaser->lockDevice();
lockDevice();
const std::list<ArSensorReading *> *rawReadings;
std::list<ArSensorReading *>::const_iterator rawIt;
rawReadings = myLaser->getRawReadings();
myCurrentBuffer.beginRedoBuffer();
if (myReflectanceThreshold < 0 || myReflectanceThreshold > 255)
myReflectanceThreshold = 0;
if (rawReadings->begin() != rawReadings->end())
{
for (rawIt = rawReadings->begin(); rawIt != rawReadings->end(); rawIt++)
{
reading = (*rawIt);
if (!reading->getIgnoreThisReading() &&
reading->getExtraInt() > myReflectanceThreshold)
myCurrentBuffer.redoReading(reading->getPose().getX(),
reading->getPose().getY());
}
}
myCurrentBuffer.endRedoBuffer();
unlockDevice();
myLaser->unlockDevice();
}
/*-------------------------------------------------------------
getSonarsReadings
-------------------------------------------------------------*/
void CActivMediaRobotBase::getSonarsReadings( bool &thereIsObservation, CObservationRange &obs )
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
obs.minSensorDistance = 0;
obs.maxSensorDistance = 30;
int i,N =THE_ROBOT->getNumSonar();
obs.sensorLabel = "BASE_SONARS";
obs.sensorConeApperture = DEG2RAD( 30 );
obs.timestamp = system::now();
obs.sensedData.clear();
unsigned int time_cnt = THE_ROBOT->getCounter();
if (m_lastTimeSonars == time_cnt)
{
thereIsObservation = false;
THE_ROBOT->unlock();
return;
}
for (i=0;i<N;i++)
{
ArSensorReading *sr = THE_ROBOT->getSonarReading(i);
if (sr->getIgnoreThisReading()) continue;
// if (!sr->isNew(time_cnt))
// {
//thereIsObservation = false;
//break;
// }
obs.sensedData.push_back( CObservationRange::TMeasurement() );
CObservationRange::TMeasurement & newObs = obs.sensedData.back();
newObs.sensorID = i;
newObs.sensorPose.x = 0.001*sr->getSensorX();
newObs.sensorPose.y = 0.001*sr->getSensorY();
newObs.sensorPose.z = 0; //0.001*sr->getSensorZ();
newObs.sensorPose.yaw = DEG2RAD( sr->getSensorTh() );
newObs.sensorPose.pitch = 0;
newObs.sensorPose.roll = 0;
newObs.sensedDistance = 0.001*THE_ROBOT->getSonarRange(i);
}
THE_ROBOT->unlock();
thereIsObservation = !obs.sensedData.empty();
// keep the last time:
if (thereIsObservation)
m_lastTimeSonars = time_cnt;
#else
MRPT_UNUSED_PARAM(thereIsObservation); MRPT_UNUSED_PARAM(obs);
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
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 ArLaser::laserProcessReadings(void)
{
// if we have no readings... don't do anything
if (myRawReadings == NULL || myRawReadings->begin() == myRawReadings->end())
return;
std::list<ArSensorReading *>::iterator sensIt;
ArSensorReading *sReading;
double x, y;
double lastX = 0.0, lastY = 0.0;
//unsigned int i = 0;
ArTime len;
len.setToNow();
bool clean;
if (myCumulativeCleanInterval <= 0 ||
(myCumulativeLastClean.mSecSince() >
myCumulativeCleanInterval))
{
myCumulativeLastClean.setToNow();
clean = true;
}
else
{
clean = false;
}
myCurrentBuffer.setPoseTaken(myRawReadings->front()->getPoseTaken());
myCurrentBuffer.setEncoderPoseTaken(
myRawReadings->front()->getEncoderPoseTaken());
myCurrentBuffer.beginRedoBuffer();
// walk the buffer of all the readings and see if we want to add them
for (sensIt = myRawReadings->begin();
sensIt != myRawReadings->end();
++sensIt)
{
sReading = (*sensIt);
// if we have ignore readings then check them here
if (!myIgnoreReadings.empty() &&
(myIgnoreReadings.find(
(int) ceil(sReading->getSensorTh())) !=
myIgnoreReadings.end()) ||
myIgnoreReadings.find(
(int) floor(sReading->getSensorTh())) !=
myIgnoreReadings.end())
sReading->setIgnoreThisReading(true);
// see if the reading is valid
if (sReading->getIgnoreThisReading())
continue;
// if we have a max range then check it here...
if (myMaxRange != 0 &&
sReading->getRange() > myMaxRange)
{
sReading->setIgnoreThisReading(true);
}
// see if the reading is valid... this is set up this way so that
// max range readings can cancel out other readings, but will
// still be ignored other than that... ones ignored for other
// reasons were skipped above
if (sReading->getIgnoreThisReading())
{
internalProcessReading(sReading->getX(), sReading->getY(),
sReading->getRange(), clean, true);
continue;
}
// get our coords
x = sReading->getX();
y = sReading->getY();
// see if we're checking on the filter near dist... if we are
// and the reading is a good one we'll check the cumulative
// buffer
if (myMinDistBetweenCurrentSquared > 0.0000001)
{
// see where the last reading was
//squaredDist = (x-lastX)*(x-lastX) + (y-lastY)*(y-lastY);
// see if the reading is far enough from the last reading
if (ArMath::squaredDistanceBetween(x, y, lastX, lastY) >
myMinDistBetweenCurrentSquared)
{
lastX = x;
lastY = y;
// since it was a good reading, see if we should toss it in
// the cumulative buffer...
internalProcessReading(x, y, sReading->getRange(), clean, false);
/* we don't do this part anymore since it wound up leaving
// too many things not really tehre... if its outside of our
// sensor angle to use to filter then don't let this one
// clean (ArMath::fabs(sReading->getSensorTh()) > 50)
// filterAddAndCleanCumulative(x, y, false); else*/
}
// it wasn't far enough, skip this one and go to the next one
else
{
continue;
}
}
// we weren't filtering the readings, but see if it goes in the
// cumulative buffer anyways
else
{
internalProcessReading(x, y, sReading->getRange(), clean, false);
}
// now drop the reading into the current buffer
myCurrentBuffer.redoReading(x, y);
//i++;
}
myCurrentBuffer.endRedoBuffer();
/* Put this in to see how long the cumulative filtering is taking
if (clean)
printf("### %ld %d\n", len.mSecSince(), myCumulativeBuffer.getBuffer()->size());
*/
internalGotReading();
}
void ArSickLogger::internalTakeReading(void)
{
const std::list<ArSensorReading *> *readings;
std::list<ArSensorReading *>::const_iterator it;
std::list<ArSensorReading *>::const_reverse_iterator rit;
ArPose poseTaken;
time_t msec;
ArSensorReading *reading;
bool usingAdjustedReadings;
// we take readings in any of the following cases if we haven't
// taken one yet or if we've been explicitly told to take one or if
// we've gone further than myDistDiff if we've turned more than
// myDegDiff if we've switched sign on velocity and gone more than
// 50 mm (so it doesn't oscilate and cause us to trigger)
if (myRobot->isConnected() && (!myFirstTaken || myTakeReadingExplicit ||
myLast.findDistanceTo(myRobot->getEncoderPose()) > myDistDiff ||
fabs(ArMath::subAngle(myLast.getTh(),
myRobot->getEncoderPose().getTh())) > myDegDiff ||
(( (myLastVel < 0 && myRobot->getVel() > 0) ||
(myLastVel > 0 && myRobot->getVel() < 0)) &&
myLast.findDistanceTo(myRobot->getEncoderPose()) > 50)))
{
myWrote = true;
mySick->lockDevice();
/// use the adjusted raw readings if we can, otherwise just use
/// the raw readings like before
if ((readings = mySick->getAdjustedRawReadings()) != NULL)
{
usingAdjustedReadings = true;
}
else
{
usingAdjustedReadings = false;
readings = mySick->getRawReadings();
}
if (readings == NULL || (it = readings->begin()) == readings->end() ||
myFile == NULL)
{
mySick->unlockDevice();
return;
}
myTakeReadingExplicit = false;
myScanNumber++;
if (usingAdjustedReadings)
ArLog::log(ArLog::Normal,
"Taking adjusted readings from the %d laser values",
readings->size());
else
ArLog::log(ArLog::Normal,
"Taking readings from the %d laser values",
readings->size());
myFirstTaken = true;
myLast = myRobot->getEncoderPose();
poseTaken = (*readings->begin())->getEncoderPoseTaken();
myLastVel = myRobot->getVel();
msec = myStartTime.mSecSince();
fprintf(myFile, "scan1Id: %d\n", myScanNumber);
fprintf(myFile, "time: %ld.%ld\n", msec / 1000, msec % 1000);
/* ScanStudio isn't using these yet so don't print them
fprintf(myFile, "velocities: %.2f %.2f\n", myRobot->getRotVel(),
myRobot->getVel());*/
internalPrintPos(poseTaken);
if (myUseReflectorValues)
{
fprintf(myFile, "reflector1: ");
if (!mySick->isLaserFlipped())
{
// make sure that the list is in increasing order
for (it = readings->begin(); it != readings->end(); it++)
{
reading = (*it);
if (!reading->getIgnoreThisReading())
fprintf(myFile, "%d ", reading->getExtraInt());
else
fprintf(myFile, "0 ");
}
}
else
{
for (rit = readings->rbegin(); rit != readings->rend(); rit++)
{
reading = (*rit);
if (!reading->getIgnoreThisReading())
fprintf(myFile, "%d ", reading->getExtraInt());
else
fprintf(myFile, "0 ");
}
}
fprintf(myFile, "\n");
}
/**
Note that the the sick1: or scan1: must be the last thing in
that timestamp, ie that you should put any other data before
it.
**/
if (myOldReadings)
{
fprintf(myFile, "sick1: ");
if (!mySick->isLaserFlipped())
{
// make sure that the list is in increasing order
for (it = readings->begin(); it != readings->end(); it++)
{
reading = (*it);
fprintf(myFile, "%d ", reading->getRange());
}
}
else
{
for (rit = readings->rbegin(); rit != readings->rend(); rit++)
{
reading = (*rit);
fprintf(myFile, "%d ", reading->getRange());
}
}
fprintf(myFile, "\n");
}
if (myNewReadings)
{
fprintf(myFile, "scan1: ");
if (!mySick->isLaserFlipped())
{
// make sure that the list is in increasing order
for (it = readings->begin(); it != readings->end(); it++)
{
reading = (*it);
if (!reading->getIgnoreThisReading())
fprintf(myFile, "%.0f %.0f ",
reading->getLocalX() - mySick->getSensorPositionX(),
reading->getLocalY() - mySick->getSensorPositionY());
else
fprintf(myFile, "0 0 ");
}
}
else
{
for (rit = readings->rbegin(); rit != readings->rend(); rit++)
{
reading = (*rit);
if (!reading->getIgnoreThisReading())
fprintf(myFile, "%.0f %.0f ",
reading->getLocalX() - mySick->getSensorPositionX(),
reading->getLocalY() - mySick->getSensorPositionY());
else
fprintf(myFile, "0 0 ");
}
}
fprintf(myFile, "\n");
}
mySick->unlockDevice();
}
}
AREXPORT void ArLineFinder::fillPointsFromLaser(void)
{
const std::list<ArSensorReading *> *readings;
std::list<ArSensorReading *>::const_iterator it;
std::list<ArSensorReading *>::const_reverse_iterator rit;
ArSensorReading *reading;
int pointCount = 0;
if (myPoints != NULL)
delete myPoints;
myPoints = new std::map<int, ArPose>;
myRangeDevice->lockDevice();
readings = myRangeDevice->getRawReadings();
if (!myFlippedFound)
{
if (readings->begin() != readings->end())
{
int size;
size = readings->size();
it = readings->begin();
// advance along 10 readings
for (int i = 0; i < 10 && i < size / 2; i++)
it++;
// see if we're flipped
if (ArMath::subAngle((*(readings->begin()))->getSensorTh(),
(*it)->getSensorTh()) > 0)
myFlipped = true;
else
myFlipped = false;
myFlippedFound = true;
//printf("@@@ LINE %d %.0f\n", myFlipped, ArMath::subAngle((*(readings->begin()))->getSensorTh(), (*it)->getSensorTh()));
}
}
if (readings->begin() == readings->end())
{
myRangeDevice->unlockDevice();
return;
}
myPoseTaken = (*readings->begin())->getPoseTaken();
if (myFlipped)
{
for (rit = readings->rbegin(); rit != readings->rend(); rit++)
{
reading = (*rit);
if (reading->getRange() > 5000 || reading->getIgnoreThisReading())
continue;
(*myPoints)[pointCount] = reading->getPose();
pointCount++;
}
}
else
{
for (it = readings->begin(); it != readings->end(); it++)
{
reading = (*it);
if (reading->getRange() > 5000 || reading->getIgnoreThisReading())
continue;
(*myPoints)[pointCount] = reading->getPose();
pointCount++;
}
}
myRangeDevice->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();
}
