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 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();
}
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 Joydrive::drive(void)
{
int trans, rot;
ArPose pose;
ArPose rpose;
ArTransform transform;
ArRangeDevice *dev;
ArSensorReading *son;
if (!myRobot->isConnected())
{
printf("Lost connection to the robot, exiting\n");
exit(0);
}
printf("\rx %6.1f y %6.1f th %6.1f",
myRobot->getX(), myRobot->getY(), myRobot->getTh());
fflush(stdout);
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
{
if (ArMath::fabs(myRobot->getVel()) < 10.0)
myRobot->comInt(ArCommands::ENABLE, 1);
myJoyHandler.getAdjusted(&rot, &trans);
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
time(NULL) - myLastPress > 1)
{
myLastPress = time(NULL);
printf("\n");
switch (myTest)
{
case 1:
printf("Moving back to the origin.\n");
pose.setPose(0, 0, 0);
myRobot->moveTo(pose);
break;
case 2:
printf("Moving over a meter.\n");
pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
myRobot->moveTo(pose);
break;
case 3:
printf("Doing a transform test....\n");
printf("\nOrigin should be transformed to the robots coords.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(0, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
printf("\nRobot coords should be transformed to the origin.\n");
transform = myRobot->getToLocalTransform();
pose = myRobot->getPose();
pose = transform.doTransform(pose);
rpose.setPose(0, 0, 0);
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
break;
case 4:
printf("Doing a tranform test...\n");
printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(-1000, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
printf("Probable Success\n");
else
printf("#### FAILURE\n");
break;
case 5:
printf("Doing a transform test on range devices..\n");
printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
dev = myRobot->findRangeDevice("sonar");
if (dev == NULL)
{
printf("No sonar on the robot, can't do the test.\n");
break;
}
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
pose = myRobot->getPose();
pose.setX(pose.getX() + 4000);
pose.setY(pose.getY() + 4000);
myRobot->moveTo(pose);
printf("Moved robot.\n");
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
break;
case 6:
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
printf("Disconnecting from the robot, then reconnecting.\n");
myRobot->disconnect();
myRobot->blockingConnect();
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
break;
default:
printf("No test for second button.\n");
break;
}
}
}
