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