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