/**
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;
}
int main(void)
{
ArPose p1(100, 100, 0), p2(1000, 1000, 90), p3;
ArTransform trans;
trans.setTransform(p1);
p3.setPose(-900,-900,0);
p3 = trans.doTransform(p3);
p3.log();
p2.setPose(200, 200, 0);
trans.setTransform(p1, p2);
p3.setPose(0,0,0);
p3 = trans.doInvTransform(p3);
p3.log();
}
/**
@param sx the coords of the sensor return relative to sensor (mm)
@param sy the coords of the sensor return relative to sensor (mm)
@param robotPose the robot's pose when the reading was taken
@param encoderPose the robot's encoder pose when the reading was taken
@param trans transform of reading from local to global position
@param counter the counter from the robot when the sensor reading was taken
@param timeTaken the time the reading was taken
@param ignoreThisReading if this reading should be ignored or not
@param extraInt extra laser device-specific value associated with this
reading (e.g. SICK LMS-200 reflectance)
*/
AREXPORT void ArSensorReading::newData(int sx, int sy, ArPose robotPose,
ArPose encoderPose, ArTransform trans,
unsigned int counter, ArTime timeTaken,
bool ignoreThisReading, int extraInt)
{
// TODO calculate the x and y position of the sensor
double rx, ry;
myRange = (int)sqrt((double)(sx*sx + sy*sy));
myCounterTaken = counter;
myReadingTaken = robotPose;
myEncoderPoseTaken = encoderPose;
rx = getSensorX() + sx;
ry = getSensorY() + sy;
myLocalReading.setPose(rx, ry);
myReading = trans.doTransform(myLocalReading);
myTimeTaken = timeTaken;
myIgnoreThisReading = ignoreThisReading;
myExtraInt = extraInt;
myAdjusted = false;
}
/**
This function is called every 100 milliseconds.
*/
AREXPORT void ArIRs::processReadings(void)
{
ArUtil::BITS bit = ArUtil::BIT0;
if(myParams.haveTableSensingIR())
{
for (int i = 0; i < myParams.getNumIR(); ++i)
{
switch(i)
{
case 0:
bit = ArUtil::BIT0;
break;
case 1:
bit = ArUtil::BIT1;
break;
case 2:
bit = ArUtil::BIT2;
break;
case 3:
bit = ArUtil::BIT3;
break;
case 4:
bit = ArUtil::BIT4;
break;
case 5:
bit = ArUtil::BIT5;
break;
case 6:
bit = ArUtil::BIT6;
break;
case 7:
bit = ArUtil::BIT7;
break;
}
if(myParams.haveNewTableSensingIR() && myRobot->getIODigInSize() > 3)
{
if((myParams.getIRType(i) && !(myRobot->getIODigIn(3) & bit)) ||
(!myParams.getIRType(i) && (myRobot->getIODigIn(3) & bit)))
{
if(cycleCounters[i] < myParams.getIRCycles(i))
{
cycleCounters[i] = cycleCounters[i] + 1;
}
else
{
cycleCounters[i] = 1;
ArPose pose;
pose.setX(myParams.getIRX(i));
pose.setY(myParams.getIRY(i));
ArTransform global = myRobot->getToGlobalTransform();
pose = global.doTransform(pose);
myCurrentBuffer.addReading(pose.getX(), pose.getY());
}
}
else
{
cycleCounters[i] = 1;
}
}
else
{
if(!(myRobot->getDigIn() & bit))
{
if(cycleCounters[i] < myParams.getIRCycles(i))
{
cycleCounters[i] = cycleCounters[i] + 1;
}
else
{
cycleCounters[i] = 1;
ArPose pose;
pose.setX(myParams.getIRX(i));
pose.setY(myParams.getIRY(i));
ArTransform global = myRobot->getToGlobalTransform();
pose = global.doTransform(pose);
myCurrentBuffer.addReading(pose.getX(), pose.getY());
}
}
else
{
cycleCounters[i] = 1;
}
}
}
}
}
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();
}
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();
}
/**
@param trans the transform to apply to the encoder pose taken
*/
AREXPORT void ArSensorReading::applyEncoderTransform(ArTransform trans)
{
myEncoderPoseTaken = trans.doTransform(myEncoderPoseTaken);
}
/**
@param trans the transform to apply to the reading and where the reading was taken
*/
AREXPORT void ArSensorReading::applyTransform(ArTransform trans)
{
myReading = trans.doTransform(myReading);
myReadingTaken = trans.doTransform(myReadingTaken);
}
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 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 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;
}
}
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;
}
}
}