void ULaserDevice::send(char * msg) { int v; char * s, *d; // //strncpy(sendStr, msg, MAX_SEND_MSG_LNG); s = msg; d = sendStr; while (*s != '\0') { if (*s == ':') { v = strtol(&s[1], &s, 0); if (*s == ':') s++; *d = char(v); d++; } else *d++ = *s++; } *d = '\0'; sendStrCnt = (d - sendStr); sendNewData = true; if (not isPortOpen()) start(); }
DynamixelDriver::DynamixelDriver(const char *port, unsigned int baudrate, DriverMode mode) { _portHandler = dynamixel::PortHandler::getPortHandler(port); if(isPortOpen()) { _portHandler->setBaudRate(baudrate); _portHandler->setPacketTimeout(10000.0); switch (mode) { case COMBINE: _packetHandlerVer1 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL1); _packetHandlerVer2 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL2); break; case PROTOCOL1: _packetHandlerVer1 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL1); _packetHandlerVer2 = NULL; break; case PROTOCOL2: _packetHandlerVer2 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL2); _packetHandlerVer1 = NULL; break; } } else ROS_INFO("[%s]: Port %s can't be open", ros::this_node::getName().c_str(), port); }
const char * ULaserDevice::print(const char * preString, char * buff, int buffCnt) { snprintf(buff, buffCnt, "%s %s on %s open=%s w=%ddeg, " "res=%4.2fdeg, rate=%.1f/s scan=%lu\n", preString, getName(), devName, bool2str(isPortOpen()), modeAngleScan, angleResolution, getMsgRate(), getSerial()); return buff; }
void ULaserDevice::threadRunLoop() { int STAT_INTERVAL = 1000; //struct timeval t1, t2; double dt; bool gotData; UTime t; // threadRunning = true; loopCnt = 0; t.Now(); while (not threadStop) { // send new data to device if (sendNewData) { // debug printf("ULaserDevice::threadRunLoop sending to device '%s'\n", sendStr); // debug end sendToDevice(sendStr, sendStrCnt); sendNewData = false; } // receive data from device // may be measurements or not gotData = receiveData(); // do other things as appropriate if (not isPortOpen()) Wait(0.1); if (not gotData) Wait(0.001); // changed to 1 ms if(int(statGoodCnt + statBadCnt) == STAT_INTERVAL ) { dt = t.getTimePassed(); t.Now(); if (verbose) { printf("Got %d Good and %d Bad packets (total %d) %3.1f%% good\n", statGoodCnt, statBadCnt, statGoodCnt + statBadCnt, (float)(statGoodCnt) / (float)(statGoodCnt + statBadCnt) * 100.0); printf("in %7.3f seconds or %6.2f scan/sec\n", dt, (float)(statGoodCnt)/dt); } // statMsgRate = double(statGoodCnt)/dt; statGoodCnt = 0; statBadCnt = 0; } loopCnt++; } threadRunning = false; }
bool ULaserDevice::start() { bool result = true; pthread_attr_t thAttr; // if ((not modeSimulated) and (not isPortOpen())) { // open port result = openPort(); } // if ((isPortOpen() or modeSimulated) and not threadRunning) { pthread_attr_init(&thAttr); // threadStop = false; // create socket server thread result = (pthread_create(&threadHandle, &thAttr, &threadRunSick, (void *)this) == 0); pthread_attr_destroy(&thAttr); } return result; }
void ULaserDevice::stop(bool justClosePort) { if (isPortOpen()) { // stop contiious mode and close serial port closePort(); } if (threadRunning and not justClosePort) { // stop and join thread threadStop = true; pthread_join(threadHandle, NULL); } // close log too (if open) datalog.closeLog(); }
const char * UHokuyo::getNameFromDevice() { int i; // if (isPortOpen()) { closePort(); } // open portand ensure that receive thread is running start(); if (isPortOpen()) { // wait for port to open ... lock(); Wait(0.1); printf("Name request [V\\n]...\n"); // request forst message - vill probably be lost sendToDevice("V\n", 2); printf("Waiting for reply ...\n"); Wait(0.1); // request again sendToDevice("V\n", 2); // wait for reply i = 0; unlock(); while (strlen(getName()) < 10) { // wait for reply Wait(0.1); i++; if (i > 20) break; } printf("Finished after <= %g secs\n", 0.1 * i); closePort(); } return getName(); }
void ULaserDevice::createBaseVars() { double d; if (vars != NULL) { var.maxRange = vars->addVar("maxRange", maxValidRange, "d", "(r) Max valid range for device"); var.name = vars->addVarA("name", name, "s", "(r) device name"); var.versionInfo = vars->addVarA("versionInfo", name, "s", "(r) version info from device (if available)"); var.scanres = vars->addVar("scanRes", angleResolution, "d", "(r) resoulution in degrees between scans."); var.scanwidth = vars->addVar("scanWidth", modeAngleScan, "d", "(r) width of scan - in xy plane symmetric around x-axis"); var.serial = vars->addVar("scannumber", serial, "d", "(r) next unused scan number"); var.type = vars->addVarA("device", devName, "s", "(r) device source name"); var.pose = vars->addVarA("pose", "0 0 0 0 0 0", "6d", "(r/w) pose (x,y,z,a,p,k) relative to robot. 2D pose is (x,y,k)"); d = isPortOpen(); var.isOpen = vars->addVar("isOpen", d, "d", "(r) is device open (delivers data)"); var.framerate = vars->addVar("framerate", 1.0, "d", "(r) delivered framerate (Hz) - may not work for all devices"); var.scanDelay = vars->addVar("scanDelay", 0.0, "d", "(r/w) Delay time from laser scan to timestamp (approx 1/(2 * framerate))"); } }
bool UHokuyo::receiveData() { bool gotData = false; int n, i = 0; char * dend = NULL; UTime t; bool gotRngData = false; const int MSL = 500; char s[MSL]; // if (modeSimulated) { //gotData = receiveSimulatedData(&length); Wait(0.1); } else { if (dataCnt > 0) { // test for old data if (dataTime.getTimePassed() > 1.0) { printf("Discarded '%s'\n", dataBuf); for (n = 0; n < dataCnt; n++) printf("%02x,", dataBuf[n]); printf("\n"); printf("Discarded %d bytes of old data\n", dataCnt); statBadCnt++; dataCnt = 0; } } lock(); if (isPortOpen()) { // set timeout to ~30 ms n = receiveFromDevice(&dataBuf[dataCnt], MAX_DATA_LNG - dataCnt - 1, 0.015); if (/*repeatGetScan and*/ (n > 0)) { // request more data requestMoreData(); } //if (n > 0) // printf("(%d bytes)", n); } else n = 0; unlock(); if (n > 0) { dataCnt += n; dataBuf[dataCnt] = '\0'; dend = strstr(dataBuf, "\n\n"); // debug /* if (verbose and (datalog != NULL)) { // log the received part of the buffer dataBuf[n] = '\0'; fprintf(datalog, "%4d (n=%3d) got:'%s'\n", dataCnt, n, dataBuf); }*/ // debug end gotData = (dend != NULL); } } // while (gotData) { // message length badSeries++; n = dend - dataBuf; // debug if (n > dataCnt) { printf("UHokuyo::receiveData message end found after buffer end? n=%d dataCnt=%d\n", n, dataCnt); } if (verbose) { snprintf(s, MSL, "In %d 'gotData' dataCnt=%d n=%d %c%c%c%c%c%c%c%c%c... after %g sec\n", i, dataCnt, n, dataBuf[0], dataBuf[1], dataBuf[2],dataBuf[3], dataBuf[4], dataBuf[5], dataBuf[6], dataBuf[7], dataBuf[8], dataTime.getTimePassed()); laslog.toLog("urg:", s); } //printf("msg:%s", dataBuf); i++; // debug end switch (dataBuf[0]) { case 'V': dend[1] = '\0'; if (dataBuf[1] == '\n') { // some garbage may also start with a V if (versionInfoCnt < 5 or verbose) printf("Got version (%d) info ((grp %d) %d chars):'%s'\n", versionInfoCnt, i, dataCnt, dataBuf); decodeName(dataBuf); versionInfoCnt++; if (verbose) laslog.toLog(" - V:", &dataBuf[4]); } break; case 'G': if (dataBuf[10] != '0') { // must be '0 to be valid scandata // printf("Bad data set laser may be off (bdSeries=%d) - send an on-command (\\nL1\\n)\n", badSeries); sendToDevice("\nL1\n", 4); break; } badSeries = 0; if (lasData == NULL) lasData = new ULaserData(); if (lasData != NULL) { // save data into decoded buffer lasData->lock(); lasData->setDeviceNum(deviceNum); gotRngData = decodeData(dataBuf, n, lasData); lasData->setMaxValidRange(maxValidRange); //if (repeatGetScan and not gotRngData) //{ // error message - request new data // // send request for more data right away // lock(); // //sendToDevice("00038401\n", 9); // //sendToDevice("00076801\n", 9); // unlock(); //} if (gotRngData) statGoodCnt++; lasData->setMirror(mirrorData); lasData->unlock(); // if (gotRngData) // do pending push commands. // No need to set data as locked, as // a client scanGet may use data at the same time as a scanpush, // as none of these will modify the 'lasData' structure. gotNewScan(lasData); if (verbose) { // debug logging snprintf(s, MSL, "UHokuyo::receiveData: scan=%6s %drngs In %2d msgCnt=%4d/%4d : %c%c%c%c%c%c%c%c%c %c %c%c... after %g sec\n", bool2str(gotRngData), lasData->getRangeCnt(), i, n, dataCnt, dataBuf[0], dataBuf[1], dataBuf[2],dataBuf[3], dataBuf[4], dataBuf[5], dataBuf[6], dataBuf[7], dataBuf[8], dataBuf[10], dataBuf[12], dataBuf[13], dataTime.getTimePassed()); laslog.toLog(" - G:", s); // to screen also printf("%s", s); } dataTime.Now(); } break; default: // got error // discard to first newline /* char * nl = strchr(dataBuf, '\n'); if (nl < dend and nl > dataBuf) { // newline found, // set new data end (dend) to one earlier, as if it was // a real "\n\n" command termination. // this is to avoid a situation where OK scans // are part of an error-respond. dend = --nl; }*/ // close string and restart dend[1] = '\0'; // debug if (verbose) { snprintf(s, MSL, "UHokuyo::receiveData: garbage msg %4d of %4d chars in buffer:'%c%c%c...'\n", n, dataCnt, dataBuf[0], dataBuf[1], dataBuf[2]); laslog.toLog(" - ", s); } //printf("UHokuyo::receiveData: ignored garbage (%d chars):'%s'\n", dataCnt, dataBuf); //rp = true; //repeatGetScan; // no other than G???... should be // received in repeat mode, well // sometimes a LFLF is in the error message //repeatGetScan = false; /*closePort(); printf("Got unknown garbage:--------Closing port\n"); Wait(0.2); if (rp) { // repeat is enabled - request dummy data printf("-------------restarting in repeat mode\n"); getNewestData(NULL, 0, false); //repeatGetScan = true; }*/ statBadCnt++; break; } // discard the used (or unknown) data n = dataCnt - (dend - dataBuf) - 2; memmove(dataBuf, dend + 2, n); // reduce available data count dataCnt = n; dataBuf[dataCnt] = '\0'; // test for more data in buffer gotData = dataCnt > 3; if (gotData) { // is it a full message dend = strstr(dataBuf, "\n\n"); gotData = (dend != NULL); //printf("****Got more messages in one read (OK, but indicate latency)\n"); } } return gotRngData; }
bool UHokuyo::getNewestData(ULaserData * dest, unsigned long lastSerial, int fake) { bool result = false; UTime t; bool isOK; // result = (dest != NULL); if (result) { if (fake > 0) getFakeScan( dest, lastSerial, fake); else { // live data if (not isPortOpen()) { // open port (in get scan mode) // and start read thread start(); if (isPortOpen()) { // wait for port to open printf("Opening port for scan ...\n"); Wait(0.1); lock(); sendToDevice("\nV\n", 3); //isOK = sendToDevice("\nG04572501\n", 11); unlock(); //printf("Waiting for reply 1 to [G38438501\\n] ...\n"); Wait(0.1); lock(); isOK = sendToDevice("\nG04572501\n", 11); unlock(); //printf("Waiting for reply 2 to [G38438501\\n] ...\n"); Wait(0.3); if (isOK) printf("Opening port for scan ... is OK\n"); badSeries = 0; versionInfoCnt = 0; } } if (isPortOpen()) { // get a full scan of data t = dataRxTime; } result = (lasData != NULL) and (dest != NULL); if (result) { lasData->lock(); //printf("Last serial = %lu, got data fromserial %lu, valid=%s\n", // lastSerial, lasData->getSerial(), bool2str(lasData->isValid())); result = lasData->isValid() and // and ((lasData->getScanTime() - t) > 0.001); (lasData->getSerial() > lastSerial); if (result) { dest->copy(lasData); // save to logfile (if open) if (datalogUsedScans) logThisScan(lasData); } else dest->setValid(false); lasData->unlock(); } else if (dest != NULL) dest->setValid(false); } if (result) dest->setDeviceNum( deviceNum); } return result; }