bool ArUrg::internalGetReading(void)
{
ArTime readingRequested;
std::string reading;
char buf[1024];
reading = "";
readingRequested.setToNow();
if (!writeLine(myRequestString))
{
ArLog::log(ArLog::Terse, "Could not send request distance reading to urg");
return false;
}
if (!readLine(buf, sizeof(buf), 10000) ||
strcasecmp(buf, myRequestString) != 0)
{
ArLog::log(ArLog::Normal, "%s: Did not get distance reading response",
getName());
return false;
}
if (!readLine(buf, sizeof(buf), 10000) ||
strcasecmp(buf, "0") != 0)
{
ArLog::log(ArLog::Normal,
"%s: Bad status on distance reading response (%c)",
getName(), buf[0]);
return false;
}
while (readLine(buf, sizeof(buf), 10000))
{
if (strlen(buf) == 0)
{
myReadingMutex.lock();
myReadingRequested = readingRequested;
myReading = reading;
myReadingMutex.unlock();
if (myRobot == NULL)
sensorInterp();
return true;
}
else
{
reading += buf;
}
}
return false;
}
bool ArUrg_2_0::internalGetReading(void)
{
ArTime readingRequested;
std::string reading;
char buf[1024];
reading = "";
/*
if (!writeLine(myRequestString))
{
ArLog::log(ArLog::Terse, "Could not send request distance reading to urg");
return false;
}
*/
ArTime firstByte;
if (!readLine(buf, sizeof(buf), 1000, true, false, &firstByte) ||
strcasecmp(buf, myRequestString) != 0)
{
ArLog::log(ArLog::Normal,
"%s: Did not get distance reading response (%s)",
getName(), buf);
return false;
}
// TODO this isn't the right time, but for most of what we do that
// won't matter
readingRequested.setToNow();
readingRequested.addMSec(-100);
if (!readLine(buf, sizeof(buf), 100, false, false) ||
strcasecmp(buf, "99") != 0)
{
ArLog::log(ArLog::Normal,
"%s: Bad status on distance reading response (%s)",
getName(), buf);
return false;
}
if (!readLine(buf, sizeof(buf), 100, false, false))
{
ArLog::log(ArLog::Normal,
"%s: Could not read timestamp in distance reading response (%s)",
getName(), buf);
return false;
}
while (readLine(buf, sizeof(buf), 100, false, false))
{
if (strlen(buf) == 0)
{
myReadingMutex.lock();
myReadingRequested = readingRequested;
myReading = reading;
myReadingMutex.unlock();
if (myRobot == NULL)
sensorInterp();
return true;
}
else
{
reading += buf;
}
}
return false;
}
AREXPORT void * ArLMS1XX::runThread(void *arg)
{
char buf[1024];
ArLMS1XXPacket *packet;
/*
ArTime dataRequested;
ArLMS1XXPacket requestPacket;
requestPacket.strToBuf("sRN");
requestPacket.strToBuf("LMDscandata");
requestPacket.finalizePacket();
*/
while (getRunning())
{
lockDevice();
if (myStartConnect)
{
myStartConnect = false;
myTryingToConnect = true;
unlockDevice();
blockingConnect();
lockDevice();
myTryingToConnect = false;
unlockDevice();
continue;
}
unlockDevice();
if (!myIsConnected)
{
ArUtil::sleep(100);
continue;
}
/*
dataRequested.setToNow();
if (myConn == NULL || !myConn->write(requestPacket.getBuf(),
requestPacket.getLength()))
{
ArLog::log(ArLog::Terse, "Could not send packets request to lms1XX");
continue;
}
*/
while (getRunning() && myIsConnected &&
(packet = myReceiver.receivePacket(50, true)) != NULL)
{
myPacketsMutex.lock();
myPackets.push_back(packet);
myPacketsMutex.unlock();
if (myRobot == NULL)
sensorInterp();
// if we have a robot but it isn't running yet then don't have a
// connection failure
if (laserCheckLostConnection())
{
ArLog::log(ArLog::Terse,
"%s: Lost connection to the laser because of error. Nothing received for %g seconds (greater than the timeout of %g).", getName(),
myLastReading.mSecSince()/1000.0,
getConnectionTimeoutSeconds());
myIsConnected = false;
laserDisconnectOnError();
continue;
}
}
ArUtil::sleep(1);
//ArUtil::sleep(2000);
//ArUtil::sleep(500);
}
return NULL;
}
