I had the same problem and I tried the solution given above but nothing worked for me.
I changed the method getData in /RCPRG_laser_drivers/libLMS1xx/build/libLMS1xx/LMS1xx.cpp to the following code
char raw[DATA_BUF_LEN];
char buf[DATA_BUF_LEN];
int len=0;
if(leftovers.size() > 0) {
for(int i = 0; i < leftovers.size(); i++) {
buf[len] = leftovers[i];
len++;
}
leftovers.clear();
}
unsigned long start;
while(true) {
if(debug)
std::cout << "inside do while. ";
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(sockDesc, &rfds);
struct timeval tv;
tv.tv_sec = 0;
tv.tv_usec = 50000;
int retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv); //always return the socket number
if(debug)
std::cout << "retval: " << retval << " ";
int curLen = 0;
if (retval) {
curLen = read(sockDesc, raw, DATA_BUF_LEN); // read till this is zero
}
bool done = false;
for(int i = 0; i < curLen; i++) {
if(raw[i] == 0x03) {
leftovers.assign(raw + i + 1, raw + curLen); // copy remaining to leftovers
done = true; // this loop is important as it copies the remaining whereas in previous there were leftovers and its never the end so was stuck in the while loop
break;
} else { // we copy till we dont find ETX
buf[len] = raw[i];
len++;
}
}
if(debug)
std::cout << std::endl;
if(done) {
break;
}
}
// from here the code is same as before
buf[len-1]=0;
char* tok = strtok(buf, " ");
tok = strtok(NULL, " "); //Command
tok = strtok(NULL, " "); //VersionNumber
tok = strtok(NULL, " "); //DeviceNumber
tok = strtok(NULL, " "); //Serial number
tok = strtok(NULL, " "); //DeviceStatus
tok = strtok(NULL, " "); //MessageCounter
tok = strtok(NULL, " "); //ScanCounter
tok = strtok(NULL, " "); //PowerUpDuration
tok = strtok(NULL, " "); //TransmissionDuration
tok = strtok(NULL, " "); //InputStatus
tok = strtok(NULL, " "); //OutputStatus
tok = strtok(NULL, " "); //ReservedByteA
tok = strtok(NULL, " "); //ScanningFrequency
tok = strtok(NULL, " "); //MeasurementFrequency
tok = strtok(NULL, " ");
tok = strtok(NULL, " ");
tok = strtok(NULL, " ");
tok = strtok(NULL, " "); //NumberEncoders
int NumberEncoders;
sscanf(tok, "%d", &NumberEncoders);
//tok stop getting any data
for (int i = 0; i < NumberEncoders; i++) {
tok = strtok(NULL, " "); //EncoderPosition
tok = strtok(NULL, " "); //EncoderSpeed
}
tok = strtok(NULL, " "); //NumberChannels16Bit
int NumberChannels16Bit;
sscanf(tok, "%d", &NumberChannels16Bit);
if (debug)
printf("NumberChannels16Bit : %d\n", NumberChannels16Bit);
for (int i = 0; i < NumberChannels16Bit; i++) {
int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2
char content[6];
tok = strtok(NULL, " "); //MeasuredDataContent
sscanf(tok, "%s", content);
if (!strcmp(content, "DIST1")) {
type = 0;
} else if (!strcmp(content, "DIST2")) {
type = 1;
} else if (!strcmp(content, "RSSI1")) {
type = 2;
} else if (!strcmp(content, "RSSI2")) {
type = 3;
}
tok = strtok(NULL, " "); //ScalingFactor
tok = strtok(NULL, " "); //ScalingOffset
tok = strtok(NULL, " "); //Starting angle
tok = strtok(NULL, " "); //Angular step width
tok = strtok(NULL, " "); //NumberData
int NumberData;
sscanf(tok, "%X", &NumberData);
//std::cout<<"the type is"<<type;
if (debug)
printf("NumberData : %d\n", NumberData);
if (type == 0) {
data.dist_len1 = NumberData;
} else if (type == 1) {
data.dist_len2 = NumberData;
} else if (type == 2) {
data.rssi_len1 = NumberData;
} else if (type == 3) {
data.rssi_len2 = NumberData;
}
for (int i = 0; i < NumberData; i++) {
int dat;
tok = strtok(NULL, " "); //data
sscanf(tok, "%X", &dat);
if (type == 0) {
data.dist1[i] = dat;
} else if (type == 1) {
data.dist2[i] = dat;
} else if (type == 2) {
data.rssi1[i] = dat;
} else if (type == 3) {
data.rssi2 ...
(more)
