AREXPORT void ArLaser::laserConnect(void)
{
// figure out how many readings we can have and set the current
// buffer size to that
double degrees;
myLastReading.setToNow();
if (canSetDegrees())
{
//degrees = fabs(ArMath::subAngle(getStartDegrees(), getEndDegrees()));
degrees = fabs(getStartDegrees() - getEndDegrees());
ArLog::log(myInfoLogLevel,
"%s: Using degrees settings of %g to %g for %g degrees",
getName(), getStartDegrees(), getEndDegrees(),
degrees);
}
else if (canChooseDegrees())
{
degrees = getDegreesChoiceDouble();
ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees",
getName(), degrees);
}
else
{
degrees = 360;
ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName());
}
double increment;
if (canSetIncrement())
{
increment = getIncrement();
ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
getName(), increment);
}
else if (canChooseIncrement())
{
increment = getIncrementChoiceDouble();
ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
getName(), increment);
}
else
{
// PS 10/20/11 - This was missing causing buffer size to be very large
// set this to the lowest, note both the SZ and S3 are setting the buffer
// size but it's being overriden by this procedure - do we want to fix
// this or just leave it at the max value 360/.25=1440???
increment = .25;
ArLog::log(ArLog::Terse, "%s: Don't have any settings for increment, arbitrarily using .25", getName());
}
int size = (int)ceil(degrees / increment) + 1;
ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArLog::log(myInfoLogLevel, "%s: Connected", getName());
myConnectCBList.invoke();
}
AREXPORT void ArLaser::laserConnect(void)
{
// figure out how many readings we can have and set the current
// buffer size to that
double degrees;
myLastReading.setToNow();
if (canSetDegrees())
{
degrees = fabs(getStartDegrees() - getEndDegrees());
ArLog::log(myInfoLogLevel,
"%s: Using degrees settings of %g to %g for %g degrees",
getName(), getStartDegrees(), getEndDegrees(),
degrees);
}
else if (canChooseDegrees())
{
degrees = getDegreesChoiceDouble();
ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees",
getName(), degrees);
}
else
{
degrees = 360;
ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName());
}
double increment;
if (canSetIncrement())
{
increment = getIncrement();
ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
getName(), increment);
}
else if (canChooseIncrement())
{
increment = getIncrementChoiceDouble();
ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
getName(), increment);
}
else
{
ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 1000 for current buffer size", getName());
}
int size = (int)ceil(degrees / increment) + 1;
ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArLog::log(myInfoLogLevel, "%s: Connected", getName());
myConnectCBList.invoke();
}
AREXPORT bool ArUrg::blockingConnect(void)
{
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
bool ArUrg_2_0::internalConnect(void)
{
bool ret = true;
char buf[1024];
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
bool alreadyAtAutobaud = false;
// empty the buffer...
/*
sendCommandAndRecvStatus(
"RS", "reset",
buf, sizeof(buf), 1000);
readLine(buf, sizeof(buf), 1, true, false);
sendCommandAndRecvStatus(
"SCIP2.0", "SCIP 2.0 request",
buf, sizeof(buf), 1000);
*/
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
ArTime startedFlushing;
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
buf[0] = '\0';
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
// if we didn't get it and have an autobaud, try it at what the autobaud rate is
if (serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
alreadyAtAutobaud = true;
serConn->setBaud(atoi(getAutoBaudChoice()));
ArUtil::sleep(100);
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
startedFlushing.setToNow();
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after falling back to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after falling back to autobaudchoice",
getName());
return false;
}
}
// if we don't have a serial port or no autobaud then we can't
// change the baud, so just fail
else
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response (%s)",
getName(), buf);
return false;
}
}
// if we want to autobaud, then give it a whirl
if (!alreadyAtAutobaud && serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
// empty the buffer from the last version request
while (readLine(buf, sizeof(buf), 100, true, false));
// now change the baud...
sprintf(buf, "SS%06d", atoi(getAutoBaudChoice()));
if (!writeLine(buf))
return false;
ArUtil::sleep(100);
//serConn->setBaud(115200);
serConn->setBaud(atoi(getAutoBaudChoice()));
// wait a second for the baud to change...
ArUtil::sleep(100);
// empty the buffer from the baud change
while (readLine(buf, sizeof(buf), 100, true, false));
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after switching to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after switching to autobaudchoice",
getName());
return false;
}
ArLog::log(ArLog::Verbose, "%s: Switched to %s baud rate",
getName(), getAutoBaudChoice());
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0)
myVendor = &buf[5];
else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0)
myProduct = &buf[5];
else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0)
myFirmwareVersion = &buf[5];
else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0)
myProtocolVersion = &buf[5];
else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0)
mySerialNumber = &buf[5];
else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
myStat = &buf[5];
}
if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() ||
myProtocolVersion.empty() || mySerialNumber.empty())
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in version response",
getName());
return false;
}
if (!(ret = sendCommandAndRecvStatus(
"PP", "parameter info request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad response to parameter info request",
getName());
return false;
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "MODL:", strlen("MODL:")) == 0)
myModel = &buf[5];
else if (strncasecmp(buf, "DMIN:", strlen("DMIN:")) == 0)
myDMin = atoi(&buf[5]);
else if (strncasecmp(buf, "DMAX:", strlen("DMAX:")) == 0)
myDMax = atoi(&buf[5]);
else if (strncasecmp(buf, "ARES:", strlen("ARES:")) == 0)
myARes = atoi(&buf[5]);
else if (strncasecmp(buf, "AMIN:", strlen("AMIN:")) == 0)
myAMin = atoi(&buf[5]);
else if (strncasecmp(buf, "AMAX:", strlen("AMAX:")) == 0)
myAMax = atoi(&buf[5]);
else if (strncasecmp(buf, "AFRT:", strlen("AFRT:")) == 0)
myAFront = atoi(&buf[5]);
else if (strncasecmp(buf, "SCAN:", strlen("SCAN:")) == 0)
myScan = atoi(&buf[5]);
}
if (myModel.empty() || myDMin == 0 || myDMax == 0 || myARes == 0 ||
myAMin == 0 || myAMax == 0 || myAFront == 0 || myScan == 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in parameter info response",
getName());
return false;
}
myStepSize = 360.0 / myARes;
myStepFirst = myAFront * myStepSize;
if (myMaxRange > myDMax)
setMaxRange(myDMax);
//log();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
//myLogMore = true;
// myLogMore = false;
ArUtil::sleep(100);
//printf("myRequestString %s\n", myRequestString);
if (!(ret = sendCommandAndRecvStatus(
myRequestString, "request distance reading",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on distance reading response (%s)",
getName(), buf);
return false;
}
//myLogMore = false;
ArTime started;
started.setToNow();
while (started.secSince() < 10 &&
readLine(buf, sizeof(buf), 10000, true, false))
{
if (strlen(buf) == 0)
return true;
}
ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
getName());
return false;
}
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;
}
}