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 ArUrg::failedToConnect(void)
{
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserFailedConnect();
}
AREXPORT void * ArUrg::runThread(void *arg)
{
while (getRunning())
{
lockDevice();
if (myStartConnect)
{
myStartConnect = false;
myTryingToConnect = true;
unlockDevice();
blockingConnect();
lockDevice();
myTryingToConnect = false;
unlockDevice();
continue;
}
unlockDevice();
if (!myIsConnected)
{
ArUtil::sleep(100);
continue;
}
// 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;
}
internalGetReading();
ArUtil::sleep(1);
}
return NULL;
}
AREXPORT ArSZSeries::~ArSZSeries() {
Aria::remExitCallback(&myAriaExitCB);
if (myRobot != NULL) {
myRobot->remRangeDevice(this);
myRobot->remLaser(this);
myRobot->remSensorInterpTask(&mySensorInterpTask);
}
if (myRawReadings != NULL) {
ArUtil::deleteSet(myRawReadings->begin(), myRawReadings->end());
myRawReadings->clear();
delete myRawReadings;
myRawReadings = NULL;
}
lockDevice();
if (isConnected())
disconnect();
unlockDevice();
}
void Modem::close()
{
timer->stop();
delete sn;
sn = 0;
if ( fd ) {
tcflush( fd, TCIOFLUSH );
tcsetattr( fd, TCSANOW, &init_tty );
::close( fd );
fd = 0;
}
xreset();
unlockDevice();
mOpen = false;
}
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 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::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;
}
}
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 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;
}
AREXPORT void ArForbiddenRangeDevice::processReadings(void)
{
ArPose intersection;
std::list<ArLineSegment *>::iterator it;
lockDevice();
myDataMutex.lock();
myCurrentBuffer.beginRedoBuffer();
if (!myIsEnabled)
{
myCurrentBuffer.endRedoBuffer();
myDataMutex.unlock();
unlockDevice();
return;
}
ArLineSegment *segment;
ArPose start;
double startX;
double startY;
ArPose end;
double angle;
double length;
double gone;
double sin;
double cos;
double atX;
double atY;
double robotX = myRobot->getX();
double robotY = myRobot->getY();
double max = (double) myMaxRange;
double maxSquared = (double) myMaxRange * (double) myMaxRange;
ArTime startingTime;
//startingTime.setToNow();
// now see if the end points of the segments are too close to us
for (it = mySegments.begin(); it != mySegments.end(); it++)
{
segment = (*it);
// if either end point or some perpindicular point is close to us
// add the line's data
if (ArMath::squaredDistanceBetween(
segment->getX1(), segment->getY1(),
myRobot->getX(), myRobot->getY()) < maxSquared ||
ArMath::squaredDistanceBetween(
segment->getX2(), segment->getY2(),
myRobot->getX(), myRobot->getY()) < maxSquared ||
segment->getPerpDist(myRobot->getPose()) < max)
{
start.setPose(segment->getX1(), segment->getY1());
end.setPose(segment->getX2(), segment->getY2());
angle = start.findAngleTo(end);
cos = ArMath::cos(angle);
sin = ArMath::sin(angle);
startX = start.getX();
startY = start.getY();
length = start.findDistanceTo(end);
// first put in the start point if we should
if (ArMath::squaredDistanceBetween(
startX, startY, robotX, robotY) < maxSquared)
myCurrentBuffer.redoReading(start.getX(), start.getY());
// now walk the length of the line and see if we should put the points in
for (gone = 0; gone < length; gone += myDistanceIncrement)
{
atX = startX + gone * cos;
atY = startY + gone * sin;
if (ArMath::squaredDistanceBetween(
atX, atY, robotX, robotY) < maxSquared)
myCurrentBuffer.redoReading(atX, atY);
}
// now check the end point
if (end.squaredFindDistanceTo(myRobot->getPose()) < maxSquared)
myCurrentBuffer.redoReading(end.getX(), end.getY());
}
}
myDataMutex.unlock();
// and we're done
myCurrentBuffer.endRedoBuffer();
unlockDevice();
//printf("%d\n", startingTime.mSecSince());
}
AREXPORT bool ArSZSeries::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;
}
// myPrevSensorIntTime = myConn->getTimeRead(0);
// PS 9/9/11 - moved this here to fix issue with setting baud in mt400.p
laserPullUnsetParamsFromRobot();
laserCheckParams();
// PS 9/9/11 - add setting baud
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;
}
// PS - set logging level and laser type in packet receiver class
myReceiver.setmyInfoLogLevel(myInfoLogLevel);
myReceiver.setmyName(getName());
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
// PS 9/9/11 - moved up top
//laserPullUnsetParamsFromRobot();
//laserCheckParams();
int size = ArMath::roundInt((270/.3) + 1);
ArLog::log(myInfoLogLevel,
"%s::blockingConnect() Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
{
if (!timeDone.addMSec(60 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (60 * 1000)",
getName());
}
}
else
{
if (!timeDone.addMSec(30 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (30 * 1000)",
getName());
}
}
ArSZSeriesPacket *packet;
ArSZSeriesPacket sendPacket;
#if 0
sendPacket.empty();
sendPacket.uByteToBuf(0xA0); // stop continous sending
sendPacket.uByteToBuf(0x00);
sendPacket.uByteToBuf(0x1D);
sendPacket.uByteToBuf(0x7E);
sendPacket.finalizePacket();
if ((myConn->write(sendPacket.getBuf(), sendPacket.getLength())) == -1)
{
ArLog::log(ArLog::Terse,
"%s::blockingConnect() Could not send Stop Continuous mode to laser", getName());
failedToConnect();
return false;
}
#endif
// Build the Start Continuous sending packet and set it
// once we get a response, then we are connected, note
// the response needs to be a real reading
sendPacket.empty();
// command id = 0x91
//sendPacket.uByteToBuf(145);
sendPacket.uByteToBuf(0x91);
// note communication ID default is 0
// this value is set via the SZ Configurator
// ???? not sure what to do if it fails
// and put in CRC - from manual, this is
// specific to the communication ID = 0
sendPacket.uByteToBuf(0);
sendPacket.uByteToBuf(43);
sendPacket.uByteToBuf(218);
unsigned short crc = myReceiver.CRC16((unsigned char *)sendPacket.getBuf(), 2);
#if 0
// other communications IDs and CRC
// communication ID =1
sendPacket.uByteToBuf(1);
sendPacket.uByteToBuf(59);
sendPacket.uByteToBuf(251);
// communication ID =2
sendPacket.uByteToBuf(2);
sendPacket.uByteToBuf(11);
sendPacket.uByteToBuf(152);
// communication ID =3
sendPacket.uByteToBuf(3);
sendPacket.uByteToBuf(27);
sendPacket.uByteToBuf(185);
#endif
sendPacket.finalizePacket();
IFDEBUG(
int i;
char x[100000];
printf("buffer with len = %d: ",sendPacket.getLength());
for (i = 0;i < sendPacket.getLength();i++)
{
printf("0x%x ",sendPacket.getBuf()[i] & 0xff);
//sprintf(&x[i], "%2x", (char *)sendPacket.getBuf()[i]);
}
printf("\n");
//ArLog::log(ArLog::Terse,
// "%s::blockingConnect() write Buffer = %s", getName(), x);
); // end IFDEBUG
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::filterCallback(void)
{
std::list<ArPoseWithTime *>::iterator it;
lockDevice();
// first filter the current readings based on time
if (myMaxSecondsToKeepCurrent > 0 &&
myCurrentBuffer.getSize() > 0)
{
// just walk through and make sure nothings too far away
myCurrentBuffer.beginInvalidationSweep();
for (it = getCurrentBuffer()->begin();
it != getCurrentBuffer()->end();
++it)
{
if ((*it)->getTime().secSince() >= myMaxSecondsToKeepCurrent)
myCurrentBuffer.invalidateReading(it);
}
myCurrentBuffer.endInvalidationSweep();
}
// okay done with current, now do the cumulative
bool doingDist = true;
bool doingAge = true;
if (myCumulativeBuffer.getSize() == 0)
{
unlockDevice();
return;
}
double squaredFarDist = (myMaxDistToKeepCumulative *
myMaxDistToKeepCumulative);
if (squaredFarDist < 1)
doingDist = false;
if (myMaxSecondsToKeepCumulative <= 0)
doingAge = false;
if (!doingDist && !doingAge)
{
unlockDevice();
return;
}
// just walk through and make sure nothings too far away
myCumulativeBuffer.beginInvalidationSweep();
for (it = getCumulativeBuffer()->begin();
it != getCumulativeBuffer()->end();
++it)
{
// if its closer to a reading than the filter near dist, just return
if (doingDist &&
myRobot->getPose().squaredFindDistanceTo(*(*it)) > squaredFarDist)
myCumulativeBuffer.invalidateReading(it);
else if (doingAge &&
(*it)->getTime().secSince() >= myMaxSecondsToKeepCumulative)
myCumulativeBuffer.invalidateReading(it);
}
myCumulativeBuffer.endInvalidationSweep();
unlockDevice();
}
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 int ArLaserFilter::selfUnlockDevice(void)
{
return unlockDevice();
}
AREXPORT bool ArUrg_2_0::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 we have a starting baud and are a serial port, then change the
// baud rate... not by default this will set it to 0 baud which'll
// cause the serial stuff not to touch it
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();
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;
}
}
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();
}
AREXPORT bool ArLMS1XX::blockingConnect(void)
{
char buf[1024];
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;
}
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;
}
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
int size = (270 / .25 + 1);
ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
timeDone.addMSec(60 * 1000);
else
timeDone.addMSec(30 * 1000);
ArLMS1XXPacket *packet;
ArLMS1XXPacket sendPacket;
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("SetAccessMode");
sendPacket.uByteToBuf(0x3); // level
sendPacket.strToBuf("F4724744"); // hashed password
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "SetAccessMode")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 1)
{
ArLog::log(myLogLevel, "%s: Changed access mode (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Could not change access mode (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("mLMPsetscancfg");
sendPacket.byte4ToBuf(5000); // scanning freq
sendPacket.byte2ToBuf(1); // number segments
sendPacket.byte4ToBuf(getIncrementChoiceDouble() * 10000); // angle resolution
//sendPacket.byte4ToBuf((getStartDegrees() + 90) * 10000); // starting angle
sendPacket.byte4ToBuf(-45 * 10000); // can't change starting angle
//sendPacket.byte4ToBuf((getEndDegrees() + 90) * 10000); // ending angle
sendPacket.byte4ToBuf(225 * 10000); // can't change ending angle
sendPacket.finalizePacket();
ArLog::log(myLogLevel, "%s: setscancfg: %s", getName(),
sendPacket.getBuf());
if ((packet = sendAndRecv(timeDone, &sendPacket, "mLMPsetscancfg")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 0)
{
ArLog::log(myLogLevel, "%s: setscancfg succeeded (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Setscancfg failed (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sWN");
sendPacket.strToBuf("LMDscandatacfg");
sendPacket.uByte2ToBuf(0x1); // output channel
sendPacket.uByteToBuf(0x0); // remission
sendPacket.uByteToBuf(0x0); // remission resolution
sendPacket.uByteToBuf(0x0); // unit
sendPacket.uByte2ToBuf(0x0); // encoder
sendPacket.uByteToBuf(0x0); // position
sendPacket.uByteToBuf(0x0); // device name
sendPacket.uByteToBuf(0x0); // comment
sendPacket.uByteToBuf(0x0); // time
sendPacket.byteToBuf(5); // which scan
//sendPacket.byteToBuf(1); // which scan
sendPacket.finalizePacket();
ArLog::log(myLogLevel, "%s: scandatacfg: %s", getName(), sendPacket.getBuf());
if ((packet = sendAndRecv(timeDone, &sendPacket, "LMDscandatacfg")) != NULL)
{
ArLog::log(myLogLevel, "%s: scandatacfg succeeded", getName());
delete packet;
packet = NULL;
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("Run");
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "Run")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 1)
{
ArLog::log(myLogLevel, "%s: Run succeeded (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Could not run (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
/* when asking one at a time
sendPacket.empty();
sendPacket.strToBuf("sRN");
sendPacket.strToBuf("LMDscandata");
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "LMDscandata")) != NULL)
{
ArLog::log(myLogLevel, "%s: Got %s scan data %d", getName(),
packet->getCommandType(), packet->getLength());
myPacketsMutex.lock();
myPackets.push_back(packet);
myPacketsMutex.unlock();
sensorInterp();
ArLog::log(myLogLevel, "%s: Processed scan data", getName());
}
else
{
failedToConnect();
return false;
}
*/
sendPacket.empty();
sendPacket.strToBuf("sEN");
sendPacket.strToBuf("LMDscandata");
sendPacket.uByteToBuf(1);
sendPacket.finalizePacket();
//printf("(%s)\n", sendPacket.getBuf());
// just ask for continuous data
if (!myConn->write(sendPacket.getBuf(), sendPacket.getLength()))
{
ArLog::log(ArLog::Terse,
"%s: Could not send %s to laser", getName(), "LMDscandata");
failedToConnect();
return false;
}
while (timeDone.mSecTo())
{
packet = myReceiver.receivePacket(1000);
if (packet != NULL &&
strcasecmp(packet->getCommandType(), "sSN") == 0 &&
strcasecmp(packet->getCommandName(), "LMDscandata") == 0)
{
delete packet;
packet = NULL;
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else if (packet != NULL)
{
ArLog::log(myLogLevel, "%s: Got %s %s (%d long)", getName(),
packet->getCommandType(), packet->getCommandName(),
packet->getLength());
delete packet;
packet = NULL;
}
}
ArLog::log(ArLog::Terse,
"%s: Did not get scandata back from laser", getName());
failedToConnect();
return false;
}
