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(); }
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(); }
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(); }