/**
* @brief publish sonar readings
*/
void AriaCore::publish_sonar()
{
sensor_msgs::PointCloud msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = m_frameIDSonar;
m_robot.lock();
{
int i;
ArSensorReading* pRead;
for (i=0; i<m_robot.getNumSonar(); ++i) {
pRead = m_robot.getSonarReading(i);
if (!pRead) {
ROS_WARN("Did not receive a sonar reading on %d", i);
continue;
}
geometry_msgs::Point32 point;
point.x = pRead->getLocalX();
point.y = pRead->getLocalY();
point.z = 0.0;
msg.points.push_back(point);
}
}
m_robot.unlock();
m_pubSonar.publish(msg);
}
void Mapping::keepRendering()
{
resetMap();
while(run)
{
if(lasers != NULL)
{
lasers->clear();
delete lasers;
}
mRobot->readingSensors();
lasers = mRobot->getLaserRanges();
sonares = mRobot->getSonarRanges();
sensores = sonares;
ArSensorReading ar = sensores->at(0); //Apenas para pegar as informações de posição do robo no momento da tomada de informações.
updateRoboPosition(ar.getXTaken(),ar.getYTaken(),ar.getThTaken());
switch(this->technique)
{
case MappingTechnique::DeadReckoning:
calculateMapDeadReckoning();
break;
case MappingTechnique::BAYES:
calculateMapBayes();
break;
case MappingTechnique::HIMM:
calculateMapHIMM();
break;
}
ArUtil::sleep(33);
}
thread->exit();
thread->wait();
}
void laserRequest_and_odom(ArServerClient *client, ArNetPacket *packet)
{
robot.lock();
ArNetPacket sending;
sending.empty();
ArLaser* laser = robot.findLaser(1);
if(!laser){
printf("Could not connect to Laser... exiting\n");
Aria::exit(1);}
laser->lockDevice();
const std::list<ArSensorReading*> *sensorReadings = laser->getRawReadings(); // see ArRangeDevice interface doc
sending.byte4ToBuf((ArTypes::Byte4)(sensorReadings->size()));
for (std::list<ArSensorReading*>::const_iterator it2= sensorReadings->begin(); it2 != sensorReadings->end(); ++it2){
ArSensorReading* laserRead =*it2;
sending.byte4ToBuf((ArTypes::Byte4)(laserRead->getRange()));
//printf("%i,%i:",laserRead->getRange(),laserRead->getIgnoreThisReading());
}
sending.byte4ToBuf((ArTypes::Byte4)(robot.getX()));
sending.byte4ToBuf((ArTypes::Byte4)(robot.getY()));
sending.byte4ToBuf((ArTypes::Byte4)(robot.getTh()));
sending.byte4ToBuf((ArTypes::Byte4)(robot.getVel()));
sending.byte4ToBuf((ArTypes::Byte4)(robot.getRotVel()));
//printf("%1f,%1f,%1f\n",robot.getX(),robot.getY(),robot.getTh());
laser->unlockDevice();
robot.unlock();
sending.finalizePacket();
//sending.printHex();
client->sendPacketTcp(&sending);
}
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 readSonars(ArRobot& robot, int numSonar){
char angle[64], value[64];
G_id += 1;
ArSensorReading* sonarReading;
string res;
sprintf(value,"id=%d;",G_id);
res += value;
for (int i=0; i < numSonar; i++){
sonarReading = robot.getSonarReading(i);
sprintf(value,"v%d=%05d;", i, sonarReading->getRange());
res += value;
//cout << "Sonar reading " << i << " = " << sonarReading->getRange() << " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
}
res += "\n";
fseek(G_SONAR_FD, SEEK_SET, 0);
fwrite(res.c_str(), sizeof(char), res.size(), G_SONAR_FD);
}
/**
This is the packet handler for the PB9 data, which is sent via the micro
controller, to the client. This will read the data from the packets,
and then call processReadings to filter add the data to the current and
cumulative buffers.
*/
AREXPORT bool ArIrrfDevice::packetHandler(ArRobotPacket *packet)
{
int /*portNum,*/ i, dist, packetCounter;
double conv;
ArTransform packetTrans;
std::list<ArSensorReading *>::iterator it;
ArSensorReading *reading;
ArPose pose;
ArTransform encoderTrans;
ArPose encoderPose;
pose = myRobot->getPose();
conv = 2.88;
packetTrans.setTransform(pose);
packetCounter = myRobot->getCounter();
if (packet->getID() != 0x10)
return false;
// Which Aux port the IRRF is connected to
//portNum =
packet->bufToByte2();
encoderTrans = myRobot->getEncoderTransform();
encoderPose = encoderTrans.doInvTransform(pose);
i = 0;
for (i=0, it = myRawReadings->begin();it != myRawReadings->end();it++, i++)
{
reading = (*it);
dist = (int) ((packet->bufToUByte2()) / conv);
reading->newData(dist, pose, encoderPose, packetTrans, packetCounter, packet->getTimeReceived());
}
myLastReading.setToNow();
processReadings();
return true;
}
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 sonar_stop(ArRobot* robot)
{
int numSonar; //Number of sonar on the robot
int i; //Counter for looping
//int j;
numSonar = robot->getNumSonar(); //Get number of sonar
ArSensorReading* sonarReading; //To hold each reading
//for (j = 1; j < 6; j++)
//{
robot->setVel(200); //Set translational velocity to 200 mm/s
for (;;)
{
for (i = 0; i < numSonar; i++) //Loop through sonar
{
sonarReading = robot->getSonarReading(i); //Get each sonar reading
cout << "Sonar reading " << i << " = " << sonarReading->getRange()
<< " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
//getchar();
if (sonarReading->getSensorTh() > -90 && sonarReading->getSensorTh() < 90 && sonarReading->getRange() < 500)
robot->setVel(0);
}
}
//<< " Angle " << i << " = " <<
//printf("Sonar Reading", i, "=",sonarReading)
//robot->unlock(); //Lock robot during set up 18
//robot->comInt(ArCommands::ENABLE, 1); //Turn on the motors 19
//robot->setVel(200); //Set translational velocity to 200 mm/s
//if (sonarReading[1] < 500)
//{
//robot->setRotVel(0);
//}
}
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 ArUrg::sensorInterp(void)
{
ArTime readingRequested;
std::string reading;
myReadingMutex.lock();
if (myReading.empty())
{
myReadingMutex.unlock();
return;
}
readingRequested = myReadingRequested;
reading = myReading;
myReading = "";
myReadingMutex.unlock();
ArTime time = readingRequested;
ArPose pose;
int ret;
int retEncoder;
ArPose encoderPose;
//time.addMSec(-13);
if (myRobot == NULL || !myRobot->isConnected())
{
pose.setPose(0, 0, 0);
encoderPose.setPose(0, 0, 0);
}
else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
(retEncoder =
myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
{
ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
return;
}
ArTransform transform;
transform.setTransform(pose);
unsigned int counter = 0;
if (myRobot != NULL)
counter = myRobot->getCounter();
lockDevice();
myDataMutex.lock();
//double angle;
int i;
int len = reading.size();
int range;
int big;
int little;
//int onStep;
std::list<ArSensorReading *>::reverse_iterator it;
ArSensorReading *sReading;
bool ignore;
for (it = myRawReadings->rbegin(), i = 0;
it != myRawReadings->rend() && i < len - 1;
it++, i += 2)
{
ignore = false;
big = reading[i] - 0x30;
little = reading[i+1] - 0x30;
range = (big << 6 | little);
if (range < 20)
{
/* Well that didn't work...
// if the range is 1 to 5 that means it has low intensity, which
// could be black or maybe too far... try ignoring it and see if
// it helps with too much clearing
if (range >= 1 || range <= 5)
ignore = true;
*/
range = 4096;
}
sReading = (*it);
sReading->newData(range, pose, encoderPose, transform, counter,
time, ignore, 0);
}
myDataMutex.unlock();
laserProcessReadings();
unlockDevice();
}
AREXPORT bool ArUrg::setParamsBySteps(int startingStep, int endingStep,
int clusterCount, bool flipped)
{
if (startingStep < 0 || startingStep > 768 ||
endingStep < 0 || endingStep > 768 ||
startingStep > endingStep ||
clusterCount < 1)
{
ArLog::log(ArLog::Normal,
"%s::setParamsBySteps: Bad params (starting %d ending %d clusterCount %d)",
getName(), startingStep, endingStep, clusterCount);
return false;
}
myDataMutex.lock();
myStartingStep = startingStep;
myEndingStep = endingStep;
myClusterCount = clusterCount;
myFlipped = flipped;
sprintf(myRequestString, "G%03d%03d%02d", myStartingStep, endingStep,
clusterCount);
myClusterMiddleAngle = 0;
if (myClusterCount > 1)
myClusterMiddleAngle = myClusterCount * 0.3515625 / 2.0;
if (myRawReadings != NULL)
{
ArUtil::deleteSet(myRawReadings->begin(), myRawReadings->end());
myRawReadings->clear();
delete myRawReadings;
myRawReadings = NULL;
}
myRawReadings = new std::list<ArSensorReading *>;
ArSensorReading *reading;
int onStep;
double angle;
for (onStep = myStartingStep;
onStep < myEndingStep;
onStep += myClusterCount)
{
/// FLIPPED
if (!myFlipped)
angle = ArMath::subAngle(ArMath::subAngle(135, onStep * 0.3515625),
myClusterMiddleAngle);
else
angle = ArMath::addAngle(ArMath::addAngle(-135, onStep * 0.3515625),
myClusterMiddleAngle);
reading = new ArSensorReading;
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()),
ArMath::addAngle(angle,
mySensorPose.getTh()));
myRawReadings->push_back(reading);
//printf("%.1f ", reading->getSensorTh());
}
myDataMutex.unlock();
return true;
}
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;
}
}
}
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();
}
}
void RosAriaNode::publish()
{
// Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
pos = robot->getPose();
tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s.
position.twist.twist.linear.y = robot->getLatVel()/1000.0;
position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
position.header.frame_id = frame_id_odom;
position.child_frame_id = frame_id_base_link;
position.header.stamp = ros::Time::now();
pose_pub.publish(position);
ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f",
position.header.stamp.toSec(),
(double)position.pose.pose.position.x,
(double)position.pose.pose.position.y,
(double)position.pose.pose.orientation.w,
(double) position.twist.twist.linear.x,
(double) position.twist.twist.linear.y,
(double) position.twist.twist.angular.z
);
// publishing transform odom->base_link
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = frame_id_odom;
odom_trans.child_frame_id = frame_id_base_link;
odom_trans.transform.translation.x = pos.getX()/1000;
odom_trans.transform.translation.y = pos.getY()/1000;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
odom_broadcaster.sendTransform(odom_trans);
// getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
int stall = robot->getStallValue();
unsigned char front_bumpers = (unsigned char)(stall >> 8);
unsigned char rear_bumpers = (unsigned char)(stall);
bumpers.header.frame_id = frame_id_bumper;
bumpers.header.stamp = ros::Time::now();
std::stringstream bumper_info(std::stringstream::out);
// Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
{
bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
bumper_info << " " << (front_bumpers & (1 << (i+1)));
}
ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());
bumper_info.str("");
// Rear bumpers have reverse order (rightmost is LSB)
unsigned int numRearBumpers = robot->getNumRearBumpers();
for (unsigned int i=0; i<numRearBumpers; i++)
{
bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
}
ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
bumpers_pub.publish(bumpers);
//Publish battery information
// TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option
std_msgs::Float64 batteryVoltage;
batteryVoltage.data = robot->getRealBatteryVoltageNow();
voltage_pub.publish(batteryVoltage);
if(robot->haveStateOfCharge())
{
std_msgs::Float32 soc;
soc.data = robot->getStateOfCharge()/100.0;
state_of_charge_pub.publish(soc);
}
// publish recharge state if changed
char s = robot->getChargeState();
if(s != recharge_state.data)
{
ROS_INFO("RosAria: publishing new recharge state %d.", s);
recharge_state.data = s;
recharge_state_pub.publish(recharge_state);
}
// publish motors state if changed
bool e = robot->areMotorsEnabled();
if(e != motors_state.data || !published_motors_state)
{
ROS_INFO("RosAria: publishing new motors state %d.", e);
motors_state.data = e;
motors_state_pub.publish(motors_state);
published_motors_state = true;
}
// Publish sonar information, if enabled.
if (use_sonar) {
sensor_msgs::PointCloud cloud; //sonar readings.
cloud.header.stamp = position.header.stamp; //copy time.
// sonar sensors relative to base_link
cloud.header.frame_id = frame_id_sonar;
// Log debugging info
std::stringstream sonar_debug_info;
sonar_debug_info << "Sonar readings: ";
for (int i = 0; i < robot->getNumSonar(); i++) {
ArSensorReading* reading = NULL;
reading = robot->getSonarReading(i);
if(!reading) {
ROS_WARN("RosAria: Did not receive a sonar reading.");
continue;
}
// getRange() will return an integer between 0 and 5000 (5m)
sonar_debug_info << reading->getRange() << " ";
// local (x,y). Appears to be from the centre of the robot, since values may
// exceed 5000. This is good, since it means we only need 1 transform.
// x & y seem to be swapped though, i.e. if the robot is driving north
// x is north/south and y is east/west.
//
//ArPose sensor = reading->getSensorPosition(); //position of sensor.
// sonar_debug_info << "(" << reading->getLocalX()
// << ", " << reading->getLocalY()
// << ") from (" << sensor.getX() << ", "
// << sensor.getY() << ") ;; " ;
//add sonar readings (robot-local coordinate frame) to cloud
geometry_msgs::Point32 p;
p.x = reading->getLocalX() / 1000.0;
p.y = reading->getLocalY() / 1000.0;
p.z = 0.0;
cloud.points.push_back(p);
}
ROS_DEBUG_STREAM(sonar_debug_info.str());
sonar_pub.publish(cloud);
}
}
void ArLMS1XX::sensorInterp(void)
{
ArLMS1XXPacket *packet;
while (1)
{
myPacketsMutex.lock();
if (myPackets.empty())
{
myPacketsMutex.unlock();
return;
}
packet = myPackets.front();
myPackets.pop_front();
myPacketsMutex.unlock();
// if its not a reading packet just skip it
if (strcasecmp(packet->getCommandName(), "LMDscandata") != 0)
{
delete packet;
continue;
}
//set up the times and poses
ArTime time = packet->getTimeReceived();
ArPose pose;
int ret;
int retEncoder;
ArPose encoderPose;
// this value should be found more empirically... but we used 1/75
// hz for the lms2xx and it was fine, so here we'll use 1/50 hz for now
time.addMSec(-20);
if (myRobot == NULL || !myRobot->isConnected())
{
pose.setPose(0, 0, 0);
encoderPose.setPose(0, 0, 0);
}
else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
(retEncoder =
myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
{
ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
delete packet;
continue;
}
ArTransform transform;
transform.setTransform(pose);
unsigned int counter = 0;
if (myRobot != NULL)
counter = myRobot->getCounter();
lockDevice();
myDataMutex.lock();
int i;
int dist;
//int onStep;
std::list<ArSensorReading *>::reverse_iterator it;
ArSensorReading *reading;
// read the extra stuff
myVersionNumber = packet->bufToUByte2();
myDeviceNumber = packet->bufToUByte2();
mySerialNumber = packet->bufToUByte4();
myDeviceStatus1 = packet->bufToUByte();
myDeviceStatus2 = packet->bufToUByte();
myMessageCounter = packet->bufToUByte2();
myScanCounter = packet->bufToUByte2();
myPowerUpDuration = packet->bufToUByte4();
myTransmissionDuration = packet->bufToUByte4();
myInputStatus1 = packet->bufToUByte();
myInputStatus2 = packet->bufToUByte();
myOutputStatus1 = packet->bufToUByte();
myOutputStatus2 = packet->bufToUByte();
myReserved = packet->bufToUByte2();
myScanningFreq = packet->bufToUByte4();
myMeasurementFreq = packet->bufToUByte4();
if (myDeviceStatus1 != 0 || myDeviceStatus2 != 0)
ArLog::log(myLogLevel, "%s: DeviceStatus %d %d",
myDeviceStatus1, myDeviceStatus2);
/*
printf("Received: %s %s ver %d devNum %d serNum %d scan %d sf %d mf %d\n",
packet->getCommandType(), packet->getCommandName(),
myVersionNumber, myDeviceNumber,
mySerialNumber, myScanCounter, myScanningFreq, myMeasurementFreq);
*/
myNumberEncoders = packet->bufToUByte2();
//printf("\tencoders %d\n", myNumberEncoders);
if (myNumberEncoders > 0)
ArLog::log(myLogLevel, "%s: Encoders %d", getName(), myNumberEncoders);
for (i = 0; i < myNumberEncoders; i++)
{
packet->bufToUByte4();
packet->bufToUByte2();
//printf("\t\t%d\t%d %d\n", i, eachEncoderPosition, eachEncoderSpeed);
}
myNumChans = packet->bufToUByte2();
if (myNumChans > 1)
ArLog::log(myLogLevel, "%s: NumChans %d", getName(), myNumChans);
//printf("\tnumchans %d\n", myNumChans);
char eachChanMeasured[1024];
int eachScalingFactor;
int eachScalingOffset;
double eachStartingAngle;
double eachAngularStepWidth;
int eachNumberData;
for (i = 0; i < myNumChans; i++)
{
eachChanMeasured[0] = '\0';
packet->bufToStr(eachChanMeasured, sizeof(eachChanMeasured));
// if this isn't the data we want then skip it
if (strcasecmp(eachChanMeasured, "DIST1") != 0 &&
strcasecmp(eachChanMeasured, "DIST2") != 0)
continue;
eachScalingFactor = packet->bufToUByte4(); // FIX should be real
eachScalingOffset = packet->bufToUByte4(); // FIX should be real
eachStartingAngle = packet->bufToByte4() / 10000.0;
eachAngularStepWidth = packet->bufToUByte2() / 10000.0;
eachNumberData = packet->bufToUByte2();
/*
ArLog::log(myLogLevel, "%s: %s start %.1f step %.2f numReadings %d",
getName(), eachChanMeasured,
eachStartingAngle, eachAngularStepWidth, eachNumberData);
*/
/*
printf("\t\t%s\tscl %d %d ang %g %g num %d\n",
eachChanMeasured,
eachScalingFactor, eachScalingOffset,
eachStartingAngle, eachAngularStepWidth,
eachNumberData);
*/
// If we don't have any sensor readings created at all, make 'em all
if (myRawReadings->size() == 0)
for (i = 0; i < eachNumberData; i++)
myRawReadings->push_back(new ArSensorReading);
if (eachNumberData > myRawReadings->size())
{
ArLog::log(ArLog::Terse, "%s: Bad data, in theory have %d readings but can only have 541... skipping this packet\n", getName(), eachNumberData);
printf("%s\n", packet->getBuf());
continue;
}
std::list<ArSensorReading *>::iterator it;
double atDeg;
int onReading;
double start;
double increment;
if (myFlipped)
{
start = mySensorPose.getTh() + eachStartingAngle - 90.0 + eachAngularStepWidth * eachNumberData;
increment = -eachAngularStepWidth;
}
else
{
start = mySensorPose.getTh() + eachStartingAngle - 90.0;
increment = eachAngularStepWidth;
}
bool ignore;
for (//atDeg = mySensorPose.getTh() + eachStartingAngle - 90.0,
//atDeg = mySensorPose.getTh() + eachStartingAngle - 90.0 + eachAngularStepWidth * eachNumberData,
atDeg = start,
it = myRawReadings->begin(),
onReading = 0;
onReading < eachNumberData;
//atDeg += eachAngularStepWidth,
//atDeg -= eachAngularStepWidth,
atDeg += increment,
it++,
onReading++)
{
ignore = false;
if (atDeg < getStartDegrees() || atDeg > getEndDegrees())
ignore = true;
reading = (*it);
dist = packet->bufToUByte2();
if (dist == 0)
{
ignore = true;
}
/*
else if (!ignore && dist < 150)
{
//ignore = true;
ArLog::log(ArLog::Normal, "%s: Reading at %.1f %s is %d (not ignoring, just warning)",
getName(), atDeg,
eachChanMeasured, dist);
}
*/
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()),
atDeg);
reading->newData(dist, pose, encoderPose, transform, counter,
time, ignore, 0); // no reflector yet
}
/*
ArLog::log(ArLog::Normal,
"Received: %s %s scan %d numReadings %d",
packet->getCommandType(), packet->getCommandName(),
myScanCounter, onReading);
*/
}
myDataMutex.unlock();
laserProcessReadings();
unlockDevice();
delete packet;
}
}
AREXPORT void ArRangeDevice::adjustRawReadings(bool interlaced)
{
std::list<ArSensorReading *>::iterator rawIt;
// make sure we have raw readings and a robot, and a delay to
// correct for (note that if we don't have a delay to correct for
// but have already been adjusting (ie someone changed the delay)
// we'll just keep adjusting)
if (myRawReadings == NULL || myRobot == NULL ||
(myAdjustedRawReadings == NULL && myRobot->getOdometryDelay() == 0))
return;
// if we don't already have a list then make one
if (myAdjustedRawReadings == NULL)
myAdjustedRawReadings = new std::list<ArSensorReading *>;
// if we've already adjusted these readings then don't do it again
if (myRawReadings->begin() != myRawReadings->end() &&
myRawReadings->front()->getAdjusted())
return;
std::list<ArSensorReading *>::iterator adjIt;
ArSensorReading *adjReading;
ArSensorReading *rawReading;
ArTransform trans;
ArTransform encTrans;
ArTransform interlacedTrans;
ArTransform interlacedEncTrans;
bool first = true;
bool second = true;
int onReading;
for (rawIt = myRawReadings->begin(), adjIt = myAdjustedRawReadings->begin(),
onReading = 0;
rawIt != myRawReadings->end();
rawIt++, onReading++)
{
rawReading = (*rawIt);
if (adjIt != myAdjustedRawReadings->end())
{
adjReading = (*adjIt);
adjIt++;
}
else
{
adjReading = new ArSensorReading;
myAdjustedRawReadings->push_back(adjReading);
}
(*adjReading) = (*rawReading);
if (first || (interlaced && second))
{
ArPose origPose;
ArPose corPose;
ArPose origEncPose;
ArPose corEncPose;
ArTime corTime;
corTime = rawReading->getTimeTaken();
corTime.addMSec(-myRobot->getOdometryDelay());
if (myRobot->getPoseInterpPosition(corTime,
&corPose) == 1 &&
myRobot->getEncoderPoseInterpPosition(corTime,
&corEncPose) == 1)
{
origPose = rawReading->getPoseTaken();
origEncPose = rawReading->getEncoderPoseTaken();
/*
printf("Difference was %g %g %g (rotVel %.0f, rotvel/40 %g)\n",
origEncPose.getX() - corEncPose.getX(),
origEncPose.getY() - corEncPose.getY(),
origEncPose.getTh() - corEncPose.getTh(),
myRobot->getRotVel(), myRobot->getRotVel() / 40);
*/
if (first)
{
trans.setTransform(origPose, corPose);
encTrans.setTransform(origEncPose, corEncPose);
}
else if (interlaced && second)
{
interlacedTrans.setTransform(origPose, corPose);
interlacedEncTrans.setTransform(origEncPose, corEncPose);
}
}
else
{
//printf("Couldn't correct\n");
}
if (first)
first = false;
else if (interlaced && second)
second = false;
}
if (!interlaced && (onReading % 2) == 0)
{
adjReading->applyTransform(trans);
adjReading->applyEncoderTransform(encTrans);
}
else
{
adjReading->applyTransform(interlacedTrans);
adjReading->applyEncoderTransform(interlacedEncTrans);
}
/*
if (fabs(adjReading->getEncoderPoseTaken().getX() -
corEncPose.getX()) > 1 ||
fabs(adjReading->getEncoderPoseTaken().getY() -
corEncPose.getY()) > 1 ||
fabs(ArMath::subAngle(adjReading->getEncoderPoseTaken().getTh(),
corEncPose.getTh())) > .2)
printf("(%.0f %.0f %.0f) should be (%.0f %.0f %.0f)\n",
adjReading->getEncoderPoseTaken().getX(),
adjReading->getEncoderPoseTaken().getY(),
adjReading->getEncoderPoseTaken().getTh(),
corEncPose.getX(), corEncPose.getY(), corEncPose.getTh());
*/
adjReading->setAdjusted(true);
rawReading->setAdjusted(true);
}
}
void ArUrg_2_0::sensorInterp(void)
{
ArTime readingRequested;
std::string reading;
myReadingMutex.lock();
if (myReading.empty())
{
myReadingMutex.unlock();
return;
}
readingRequested = myReadingRequested;
reading = myReading;
myReading = "";
myReadingMutex.unlock();
ArTime time = readingRequested;
ArPose pose;
int ret;
int retEncoder;
ArPose encoderPose;
//time.addMSec(-13);
if (myRobot == NULL || !myRobot->isConnected())
{
pose.setPose(0, 0, 0);
encoderPose.setPose(0, 0, 0);
}
else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
(retEncoder =
myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
{
ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
return;
}
ArTransform transform;
transform.setTransform(pose);
unsigned int counter = 0;
if (myRobot != NULL)
counter = myRobot->getCounter();
lockDevice();
myDataMutex.lock();
//double angle;
int i;
int len = reading.size();
int range;
int giant;
int big;
int little;
//int onStep;
std::list<ArSensorReading *>::reverse_iterator it;
ArSensorReading *sReading;
int iMax;
int iIncr;
if (myUseThreeDataBytes)
{
iMax = len - 2;
iIncr = 3;
}
else
{
iMax = len - 1;
iIncr = 2;
}
bool ignore;
for (it = myRawReadings->rbegin(), i = 0;
it != myRawReadings->rend() && i < iMax; //len - 2;
it++, i += iIncr) //3)
{
ignore = false;
if (myUseThreeDataBytes)
{
giant = reading[i] - 0x30;
big = reading[i+1] - 0x30;
little = reading[i+2] - 0x30;
range = (giant << 12 | big << 6 | little);
}
else
{
big = reading[i] - 0x30;
little = reading[i+1] - 0x30;
range = (big << 6 | little);
}
if (range < myDMin)
range = myDMax+1;
sReading = (*it);
sReading->newData(range, pose, encoderPose, transform, counter,
time, ignore, 0);
}
myDataMutex.unlock();
int previous = getCumulativeBuffer()->size();
laserProcessReadings();
int now = getCumulativeBuffer()->size();
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();
}
void ArSZSeries::sensorInterp(void) {
ArSZSeriesPacket *packet;
while (1) {
myPacketsMutex.lock();
if (myPackets.empty()) {
myPacketsMutex.unlock();
return;
}
packet = myPackets.front();
myPackets.pop_front();
myPacketsMutex.unlock();
//set up the times and poses
ArTime time = packet->getTimeReceived();
ArPose pose;
int ret;
int retEncoder;
ArPose encoderPose;
int dist;
int j;
unsigned char *buf = (unsigned char *) packet->getBuf();
// this value should be found more empirically... but we used 1/75
// hz for the lms2xx and it was fine, so here we'll use 1/50 hz for now
if (!time.addMSec(-30)) {
ArLog::log(ArLog::Normal,
"%s::sensorInterp() error adding msecs (-30)", getName());
}
if (myRobot == NULL || !myRobot->isConnected())
{
pose.setPose(0, 0, 0);
encoderPose.setPose(0, 0, 0);
}
else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0
|| (retEncoder = myRobot->getEncoderPoseInterpPosition(time,
&encoderPose)) < 0)
{
ArLog::log(ArLog::Normal,
"%s::sensorInterp() reading too old to process", getName());
delete packet;
continue;
}
ArTransform transform;
transform.setTransform(pose);
unsigned int counter = 0;
if (myRobot != NULL)
counter = myRobot->getCounter();
lockDevice();
myDataMutex.lock();
//std::list<ArSensorReading *>::reverse_iterator it;
ArSensorReading *reading;
myNumChans = packet->getNumReadings();
double eachAngularStepWidth;
int eachNumberData;
// PS - test for SZ-16D, each reading is .36 degrees for 270 degrees
if (packet->getNumReadings() == 751)
{
eachNumberData = packet->getNumReadings();
}
else
{
ArLog::log(ArLog::Normal,
"%s::sensorInterp() The number of readings is not correct = %d",
getName(), myNumChans);
// PS 12/6/12 - unlock before continuing
delete packet;
myDataMutex.unlock();
unlockDevice();
continue;
}
// If we don't have any sensor readings created at all, make 'em all
if (myRawReadings->size() == 0) {
for (j = 0; j < eachNumberData; j++) {
myRawReadings->push_back(new ArSensorReading);
}
}
if (eachNumberData > myRawReadings->size())
{
ArLog::log(ArLog::Terse,
"%s::sensorInterp() Bad data, in theory have %d readings but can only have 751... skipping this packet",
getName(), eachNumberData);
// PS 12/6/12 - unlock and delete before continuing
delete packet;
myDataMutex.unlock();
unlockDevice();
continue;
}
std::list<ArSensorReading *>::iterator it;
double atDeg;
int onReading;
double start;
double increment;
eachAngularStepWidth = .36;
if (myFlipped) {
start = mySensorPose.getTh() + 135;
increment = -eachAngularStepWidth;
} else {
start = -(mySensorPose.getTh() + 135);
increment = eachAngularStepWidth;
}
int readingIndex;
bool ignore = false;
for (atDeg = start,
it = myRawReadings->begin(),
readingIndex = 0,
onReading = 0;
onReading < eachNumberData;
atDeg += increment,
it++,
readingIndex++,
onReading++)
{
reading = (*it);
dist = (((buf[readingIndex * 2] & 0x3f)<< 8) | (buf[(readingIndex * 2) + 1]));
// note max distance is 16383 mm, if the measurement
// object is not there, distance will still be 16383
/*
ArLog::log(ArLog::Normal,
"reading %d first half = 0x%x, second half = 0x%x dist = %d",
readingIndex, buf[(readingIndex *2)+1], buf[readingIndex], dist);
*/
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()), atDeg);
reading->newData(dist, pose, encoderPose, transform, counter, time,
ignore, 0); // no reflector yet
//printf("dist = %d, pose = %d, encoderPose = %d, transform = %d, counter = %d, time = %d, igore = %d",
// dist, pose, encoderPose, transform, counter,
// time, ignore);
}
/*
ArLog::log(ArLog::Normal,
"Received: %s %s scan %d numReadings %d",
packet->getCommandType(), packet->getCommandName(),
myScanCounter, onReading);
*/
myDataMutex.unlock();
/*
ArLog::log(
ArLog::Terse,
"%s::sensorInterp() Telegram number = %d ",
getName(), packet->getTelegramNumByte2());
*/
laserProcessReadings();
unlockDevice();
delete packet;
}
}
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();
}
/*-------------------------------------------------------------
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
}
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();
}
AREXPORT bool ArUrg_2_0::setParamsBySteps(int startingStep, int endingStep,
int clusterCount, bool flipped)
{
if (startingStep > endingStep || clusterCount < 1)
{
ArLog::log(ArLog::Normal,
"%s::setParamsBySteps: Bad params (starting %d ending %d clusterCount %d)",
getName(), startingStep, endingStep, clusterCount);
return false;
}
if (startingStep < myAMin)
startingStep = myAMin;
if (endingStep > myAMax)
endingStep = myAMax;
myDataMutex.lock();
myStartingStep = startingStep;
myEndingStep = endingStep;
myClusterCount = clusterCount;
myFlipped = flipped;
//sprintf(myRequestString, "G%03d%03d%02d", myStartingStep, endingStep,
//clusterCount);
int baudRate = 0;
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
baudRate = serConn->getBaud();
// only use the three data bytes if our range needs it, and if the baud rate can support it
if (myMaxRange > 4095 && (baudRate == 0 || baudRate > 57600))
{
myUseThreeDataBytes = true;
sprintf(myRequestString, "MD%04d%04d%02d%01d%02d",
myStartingStep, myEndingStep, myClusterCount,
0, // scan interval
0 // number of scans to send (forever)
);
}
else
{
myUseThreeDataBytes = false;
if (myMaxRange > 4094)
myMaxRange = 4094;
sprintf(myRequestString, "MS%04d%04d%02d%01d%02d",
myStartingStep, myEndingStep, myClusterCount,
0, // scan interval
0 // number of scans to send (forever)
);
}
myClusterMiddleAngle = 0;
if (myClusterCount > 1)
//myClusterMiddleAngle = myClusterCount * 0.3515625 / 2.0;
myClusterMiddleAngle = myClusterCount * myStepSize / 2.0;
if (myRawReadings != NULL)
{
ArUtil::deleteSet(myRawReadings->begin(), myRawReadings->end());
myRawReadings->clear();
delete myRawReadings;
myRawReadings = NULL;
}
myRawReadings = new std::list<ArSensorReading *>;
ArSensorReading *reading;
int onStep;
double angle;
for (onStep = myStartingStep;
onStep < myEndingStep;
onStep += myClusterCount)
{
/// FLIPPED
if (!myFlipped)
//angle = ArMath::subAngle(ArMath::subAngle(135,
// onStep * 0.3515625),
angle = ArMath::subAngle(ArMath::subAngle(myStepFirst,
onStep * myStepSize),
myClusterMiddleAngle);
else
//angle = ArMath::addAngle(ArMath::addAngle(-135,
// onStep * 0.3515625),
angle = ArMath::addAngle(ArMath::addAngle(-myStepFirst,
onStep * myStepSize),
myClusterMiddleAngle);
reading = new ArSensorReading;
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()),
ArMath::addAngle(angle,
mySensorPose.getTh()));
myRawReadings->push_back(reading);
//printf("%.1f ", reading->getSensorTh());
}
myDataMutex.unlock();
return true;
}
void sonarPrinter(void)
{
fprintf(stdout, "in sonarPrinter()\n"); fflush(stdout);
double scale = (double)half_size / (double)sonar.getMaxRange();
/*
ArSonarDevice *sd;
std::list<ArPoseWithTime *>::iterator it;
std::list<ArPoseWithTime *> *readings;
ArPose *pose;
sd = (ArSonarDevice *)robot->findRangeDevice("sonar");
if (sd != NULL)
{
sd->lockDevice();
readings = sd->getCurrentBuffer();
if (readings != NULL)
{
for (it = readings->begin(); it != readings->end(); ++it)
{
pose = (*it);
//pose->log();
}
}
sd->unlockDevice();
}
*/
double range;
double angle;
/*
* example to show how to find closest readings within polar sections
*/
printf("Closest readings within polar sections:\n");
double start_angle = -45;
double end_angle = 45;
range = sonar.currentReadingPolar(start_angle, end_angle, &angle);
printf(" front quadrant: %5.0f ", range);
//if (range != sonar.getMaxRange())
if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
//if (isInitialized && range != sonar.getMaxRange())
if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
{
double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
double y = range * sin(vpMath::rad(angle));
// Conversion in pixels so that the robot frame is in the middle of the image
double j = -y * scale + half_size; // obstacle position
double i = -x * scale + half_size;
vpDisplay::display(I);
vpDisplay::displayLine(I, half_size, half_size, 0, 0, vpColor::red, 5);
vpDisplay::displayLine(I, half_size, half_size, 0, 2*half_size-1, vpColor::red, 5);
vpDisplay::displayLine(I, half_size, half_size, i, j, vpColor::green, 3);
vpDisplay::displayCross(I, i, j, 7, vpColor::blue);
}
#endif
range = sonar.currentReadingPolar(-135, -45, &angle);
printf(" right quadrant: %5.0f ", range);
//if (range != sonar.getMaxRange())
if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
range = sonar.currentReadingPolar(45, 135, &angle);
printf(" left quadrant: %5.0f ", range);
//if (range != sonar.getMaxRange())
if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
range = sonar.currentReadingPolar(-135, 135, &angle);
printf(" back quadrant: %5.0f ", range);
//if (range != sonar.getMaxRange())
if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
/*
* example to show how get all sonar sensor data
*/
ArSensorReading *reading;
for (int sensor = 0; sensor < robot->getNumSonar(); sensor++)
{
reading = robot->getSonarReading(sensor);
if (reading != NULL)
{
angle = reading->getSensorTh();
range = reading->getRange();
double sx = reading->getSensorX(); // position of the sensor in the robot frame
double sy = reading->getSensorY();
double ox = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
double oy = range * sin(vpMath::rad(angle));
double x = sx + ox; // position of the obstacle in the robot frame
double y = sy + oy;
// Conversion in pixels so that the robot frame is in the middle of the image
double sj = -sy * scale + half_size; // sensor position
double si = -sx * scale + half_size;
double j = -y * scale + half_size; // obstacle position
double i = -x * scale + half_size;
// printf("%d x: %.1f y: %.1f th: %.1f d: %d\n", sensor, reading->getSensorX(),
// reading->getSensorY(), reading->getSensorTh(), reading->getRange());
#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
//if (isInitialized && range != sonar.getMaxRange())
if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
{
vpDisplay::displayLine(I, si, sj, i, j, vpColor::blue, 2);
vpDisplay::displayCross(I, si, sj, 7, vpColor::blue);
char legend[15];
sprintf(legend, "%d: %1.2fm", sensor, float(range)/1000);
vpDisplay::displayCharString(I, i-7, j+7, legend, vpColor::blue);
}
#endif
}
}
#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
if (isInitialized)
vpDisplay::flush(I);
#endif
fflush(stdout);
}
