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; }
bool Modem::open() { struct termios tty; close(); if(!lockDevice()) return false; QCString dev = QFile::encodeName((*prefs).serialDevice()); const char *fdev = dev.data(); if((fd = ::open(fdev, O_RDWR | O_NOCTTY | O_NONBLOCK)) == -1) { emit errorMessage(i18n("Unable to open device '%1'. " "Please check that you have sufficient permissions.") .arg(fdev)); return false; } tcflush(fd, TCIOFLUSH); if(tcgetattr(fd, &init_tty) == -1) { int errnumber = errno; emit errorMessage(i18n("Communication setup failed (tcgetattr code: %1)") .arg(strerror(errnumber))); ::close(fd); fd = 0; return false; } memset(&tty, 0, sizeof(tty)); tty.c_iflag = IGNBRK | IGNPAR; tty.c_oflag = 0; tty.c_cflag = cflag; tty.c_lflag = 0; cfsetospeed(&tty, cspeed); cfsetispeed(&tty, cspeed); tcdrain(fd); if(tcsetattr(fd, TCSANOW, &tty) == -1) { emit errorMessage(i18n("tcsetattr() failed.")); ::close(fd); fd = 0; return false; } sn = new QSocketNotifier(fd, QSocketNotifier::Read, this, "modemsocketnotifier"); Q_CHECK_PTR(sn); connect(sn, SIGNAL(activated(int)), SLOT(readChar(int))); mOpen = true; return true; }
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(); }
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::selfLockDevice(void) { return lockDevice(); }
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; }