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