cob_sick_lms1xx: lms1xx.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * LMS1xx.cpp
00003  *
00004  *  Created on: 09-08-2010
00005  *  Author: Konrad Banachowicz
00006  ***************************************************************************
00007  *   This library is free software; you can redistribute it and/or         *
00008  *   modify it under the terms of the GNU Lesser General Public            *
00009  *   License as published by the Free Software Foundation; either          *
00010  *   version 2.1 of the License, or (at your option) any later version.    *
00011  *                                                                         *
00012  *   This library is distributed in the hope that it will be useful,       *
00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00015  *   Lesser General Public License for more details.                       *
00016  *                                                                         *
00017  *   You should have received a copy of the GNU Lesser General Public      *
00018  *   License along with this library; if not, write to the Free Software   *
00019  *   Foundation, Inc., 59 Temple Place,                                    *
00020  *   Suite 330, Boston, MA  02111-1307  USA                                *
00021  *                                                                         *
00022  ***************************************************************************/
00023 
00024 #include <sys/types.h>
00025 #include <sys/socket.h>
00026 #include <netinet/in.h>
00027 #include <arpa/inet.h>
00028 #include <cstdio>
00029 #include <cstdlib>
00030 #include <cstring>
00031 #include <unistd.h>
00032 #include <iostream>
00033 #include <errno.h>
00034 
00035 #include "lms1xx.h"
00036 
00037 LMS1xx::LMS1xx() :
00038         connected(false) {
00039         debug = false;
00040 }
00041 
00042 LMS1xx::~LMS1xx() {
00043 
00044 }
00045 
00046 void LMS1xx::connect(std::string host, int port) {
00047         if (!connected) {
00048                 sockDesc = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
00049                 if (sockDesc) {
00050                         struct sockaddr_in stSockAddr;
00051                         int Res;
00052                         stSockAddr.sin_family = PF_INET;
00053                         stSockAddr.sin_port = htons(port);
00054                         Res = inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr);
00055 
00056                         int ret = ::connect(sockDesc, (struct sockaddr *) &stSockAddr,
00057                                         sizeof stSockAddr);
00058                         if (ret == 0) {
00059                                 connected = true;
00060                         }
00061                 }
00062         }
00063 }
00064 
00065 void LMS1xx::disconnect() {
00066         if (connected) {
00067                 close(sockDesc);
00068                 connected = false;
00069         }
00070 }
00071 
00072 bool LMS1xx::isConnected() {
00073         return connected;
00074 }
00075 
00076 void LMS1xx::startMeas() {
00077         char buf[100];
00078         sprintf(buf, "%c%s%c", 0x02, "sMN LMCstartmeas", 0x03);
00079 
00080         write(sockDesc, buf, strlen(buf));
00081 
00082         int len = read(sockDesc, buf, 100);
00083         //      if (buf[0] != 0x02)
00084         //              std::cout << "invalid packet recieved" << std::endl;
00085         //      if (debug) {
00086         //              buf[len] = 0;
00087         //              std::cout << buf << std::endl;
00088         //      }
00089 }
00090 
00091 void LMS1xx::stopMeas() {
00092         char buf[100];
00093         sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03);
00094 
00095         write(sockDesc, buf, strlen(buf));
00096 
00097         int len = read(sockDesc, buf, 100);
00098         //      if (buf[0] != 0x02)
00099         //              std::cout << "invalid packet recieved" << std::endl;
00100         //      if (debug) {
00101         //              buf[len] = 0;
00102         //              std::cout << buf << std::endl;
00103         //      }
00104 }
00105 
00106 status_t LMS1xx::queryStatus() {
00107         char buf[100];
00108         sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03);
00109 
00110         write(sockDesc, buf, strlen(buf));
00111 
00112         int len = read(sockDesc, buf, 100);
00113         //      if (buf[0] != 0x02)
00114         //              std::cout << "invalid packet recieved" << std::endl;
00115         //      if (debug) {
00116         //              buf[len] = 0;
00117         //              std::cout << buf << std::endl;
00118         //      }
00119         int ret;
00120         sscanf((buf + 10), "%d", &ret);
00121 
00122         return (status_t) ret;
00123 }
00124 
00125 void LMS1xx::login() {
00126         char buf[100];
00127         sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
00128 
00129         write(sockDesc, buf, strlen(buf));
00130 
00131         int len = read(sockDesc, buf, 100);
00132         //      if (buf[0] != 0x02)
00133         //              std::cout << "invalid packet recieved" << std::endl;
00134         //      if (debug) {
00135         //              buf[len] = 0;
00136         //              std::cout << buf << std::endl;
00137         //      }
00138 }
00139 
00140 scanCfg LMS1xx::getScanCfg() const {
00141         scanCfg cfg;
00142         char buf[100];
00143         sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
00144 
00145         write(sockDesc, buf, strlen(buf));
00146 
00147         int len = read(sockDesc, buf, 100);
00148         //      if (buf[0] != 0x02)
00149         //              std::cout << "invalid packet recieved" << std::endl;
00150         //      if (debug) {
00151         //              buf[len] = 0;
00152         //              std::cout << buf << std::endl;
00153         //      }
00154 
00155         sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency,
00156                         &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle);
00157         return cfg;
00158 }
00159 
00160 void LMS1xx::setScanCfg(const scanCfg &cfg) {
00161         char buf[100];
00162         sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg",
00163                         cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle,
00164                         cfg.stopAngle, 0x03);
00165 
00166         write(sockDesc, buf, strlen(buf));
00167 
00168         int len = read(sockDesc, buf, 100);
00169 
00170         buf[len - 1] = 0;
00171 }
00172 
00173 void LMS1xx::setScanDataCfg(const scanDataCfg &cfg) {
00174         char buf[100];
00175         sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02,
00176                         "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0,
00177                         cfg.resolution, cfg.encoder, cfg.position ? 1 : 0,
00178                         cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03);
00179         if(debug)
00180                 printf("%s\n", buf);
00181         write(sockDesc, buf, strlen(buf));
00182 
00183         int len = read(sockDesc, buf, 100);
00184         buf[len - 1] = 0;
00185 }
00186 
00187 void LMS1xx::scanContinous(int start) {
00188         char buf[100];
00189         sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03);
00190 
00191         write(sockDesc, buf, strlen(buf));
00192 
00193         int len = read(sockDesc, buf, 100);
00194 
00195         if (buf[0] != 0x02)
00196                 printf("invalid packet recieved\n");
00197 
00198         if (debug) {
00199                 buf[len] = 0;
00200                 printf("%s\n", buf);
00201         }
00202 
00203         if (start = 0) {
00204                 for (int i = 0; i < 10; i++)
00205                         read(sockDesc, buf, 100);
00206         }
00207 }
00208 
00209 bool LMS1xx::getData(scanData& data) {
00210         char buf[20000];
00211         fd_set rfds;
00212         struct timeval tv;
00213         int retval, len = 0;
00214         int bytes_read = 0;
00215         int inc = 0;
00216         int r_off = 0;
00217 
00218         do{
00219                 FD_ZERO(&rfds);
00220                 FD_SET(sockDesc, &rfds);
00221                 tv.tv_sec = 1;
00222                 tv.tv_usec = 0;
00223                 bytes_read = 0; inc = 0;
00224                 retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv);
00225                 if(retval)
00226                 {
00227                         bytes_read = read(sockDesc, buf, 1);
00228                         if(bytes_read > 1)
00229                         {
00230                                 //std::cerr<<"Error in getData: received "<< bytes_read <<" bytes"<<std::endl;
00231                                 inc = bytes_read;
00232                                 inc --;
00233                         }
00234                         else if(bytes_read < 1)
00235                         {
00236                                 //std::cerr<<"Error in getData: received "<< bytes_read <<" bytes"<<std::endl;
00237                         }
00238                 }
00239                 else
00240                 {
00241                         //std::cerr<<"select failed during find start: "<<strerror(errno)<<std::endl;
00242                 }
00243 
00244         }while(buf[0+inc] != 0x02);
00245         len += bytes_read;
00246 
00247 
00248         do {
00249                 bool bFoundEnd = false;
00250                 bool bFoundStart = false;
00251                 if(len >= 20000)
00252                 {
00253                         int i;
00254                         for(i = 0; i < len; i++)
00255                         {
00256                                 if(buf[i] == 0x03)
00257                                 {
00258                                         len = i+1;
00259                                         bFoundEnd = true;
00260                                         //std::cerr<<"Found end!\n";
00261                                 }
00262                                 if(bFoundEnd && buf[i] == 0x02)
00263                                 {
00264                                         //std::cerr<<"Found start!\n";
00265                                         bFoundStart = true;
00266                                 }
00267                         }
00268                         if(!bFoundEnd)
00269                         {
00270                                 char flushBuffer[100000];
00271                                 int bytesFlushed = recv(sockDesc, flushBuffer, 100000, 0);
00272                                 //std::cerr<<"Flushed read buffer"<<std::endl;
00273                                 return false;
00274                         }
00275                 }
00276                 if(!bFoundEnd)
00277                 {
00278                         FD_ZERO(&rfds);
00279                         FD_SET(sockDesc, &rfds);
00280 
00281                         tv.tv_sec = 0;
00282                         tv.tv_usec = 100000;
00283                         bytes_read = 0;
00284                         retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv);
00285                         if (retval)
00286                         {
00287                                 bytes_read = read(sockDesc, buf + len, 20000 - len);
00288                                 if(bytes_read < 1)
00289                                 {
00290                                         //std::cerr<<"Error in getData: received "<< bytes_read <<" bytes"<<std::endl;
00291                                 }
00292                                 else
00293                                         len += bytes_read;
00294                         }
00295                         else
00296                         {
00297                                 //std::cerr<<"select failed during get data: "<<strerror(errno)<<std::endl;
00298                         }
00299                 }
00300         } while ((buf[len - 1] != 0x03));
00301 
00302         //      if (debug)
00303         //              std::cout << "scan data recieved" << std::endl;
00304         buf[len - 1] = 0;
00305         char* tok = strtok(buf, " "); //Type of command
00306         tok = strtok(NULL, " "); //Command
00307         tok = strtok(NULL, " "); //VersionNumber
00308         tok = strtok(NULL, " "); //DeviceNumber
00309         tok = strtok(NULL, " "); //Serial number
00310         tok = strtok(NULL, " "); //DeviceStatus
00311         tok = strtok(NULL, " "); //MessageCounter
00312         tok = strtok(NULL, " "); //ScanCounter
00313         tok = strtok(NULL, " "); //PowerUpDuration
00314         tok = strtok(NULL, " "); //TransmissionDuration
00315         tok = strtok(NULL, " "); //InputStatus
00316         tok = strtok(NULL, " "); //OutputStatus
00317         tok = strtok(NULL, " "); //ReservedByteA
00318         tok = strtok(NULL, " "); //ScanningFrequency
00319         tok = strtok(NULL, " "); //MeasurementFrequency
00320         tok = strtok(NULL, " ");
00321         tok = strtok(NULL, " ");
00322         tok = strtok(NULL, " ");
00323         tok = strtok(NULL, " "); //NumberEncoders
00324         int NumberEncoders;
00325         sscanf(tok, "%d", &NumberEncoders);
00326         for (int i = 0; i < NumberEncoders; i++) {
00327                 tok = strtok(NULL, " "); //EncoderPosition
00328                 tok = strtok(NULL, " "); //EncoderSpeed
00329         }
00330 
00331         tok = strtok(NULL, " "); //NumberChannels16Bit
00332         int NumberChannels16Bit;
00333         sscanf(tok, "%d", &NumberChannels16Bit);
00334         if (debug)
00335                 printf("NumberChannels16Bit : %d\n", NumberChannels16Bit);
00336         for (int i = 0; i < NumberChannels16Bit; i++) {
00337                 int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2
00338                 char content[6];
00339                 tok = strtok(NULL, " "); //MeasuredDataContent
00340                 sscanf(tok, "%s", content);
00341                 if (!strcmp(content, "DIST1")) {
00342                         type = 0;
00343                 } else if (!strcmp(content, "DIST2")) {
00344                         type = 1;
00345                 } else if (!strcmp(content, "RSSI1")) {
00346                         type = 2;
00347                 } else if (!strcmp(content, "RSSI2")) {
00348                         type = 3;
00349                 }
00350                 tok = strtok(NULL, " "); //ScalingFactor
00351                 tok = strtok(NULL, " "); //ScalingOffset
00352                 tok = strtok(NULL, " "); //Starting angle
00353                 tok = strtok(NULL, " "); //Angular step width
00354                 tok = strtok(NULL, " "); //NumberData
00355                 int NumberData;
00356                 sscanf(tok, "%X", &NumberData);
00357 
00358                 if (debug)
00359                         printf("NumberData : %d\n", NumberData);
00360 
00361                 if (type == 0) {
00362                         data.dist_len1 = NumberData;
00363                 } else if (type == 1) {
00364                         data.dist_len2 = NumberData;
00365                 } else if (type == 2) {
00366                         data.rssi_len1 = NumberData;
00367                 } else if (type == 3) {
00368                         data.rssi_len2 = NumberData;
00369                 }
00370 
00371                 for (int i = 0; i < NumberData; i++) {
00372                         int dat;
00373                         tok = strtok(NULL, " "); //data
00374                         sscanf(tok, "%X", &dat);
00375 
00376                         if (type == 0) {
00377                                 data.dist1[i] = dat;
00378                         } else if (type == 1) {
00379                                 data.dist2[i] = dat;
00380                         } else if (type == 2) {
00381                                 data.rssi1[i] = dat;
00382                         } else if (type == 3) {
00383                                 data.rssi2[i] = dat;
00384                         }
00385 
00386                 }
00387         }
00388 
00389         tok = strtok(NULL, " "); //NumberChannels8Bit
00390         int NumberChannels8Bit;
00391         sscanf(tok, "%d", &NumberChannels8Bit);
00392         if (debug)
00393                 printf("NumberChannels8Bit : %d\n", NumberChannels8Bit);
00394         for (int i = 0; i < NumberChannels8Bit; i++) {
00395                 int type = -1;
00396                 char content[6];
00397                 tok = strtok(NULL, " "); //MeasuredDataContent
00398                 sscanf(tok, "%s", content);
00399                 if (!strcmp(content, "DIST1")) {
00400                         type = 0;
00401                 } else if (!strcmp(content, "DIST2")) {
00402                         type = 1;
00403                 } else if (!strcmp(content, "RSSI1")) {
00404                         type = 2;
00405                 } else if (!strcmp(content, "RSSI2")) {
00406                         type = 3;
00407                 }
00408                 tok = strtok(NULL, " "); //ScalingFactor
00409                 tok = strtok(NULL, " "); //ScalingOffset
00410                 tok = strtok(NULL, " "); //Starting angle
00411                 tok = strtok(NULL, " "); //Angular step width
00412                 tok = strtok(NULL, " "); //NumberData
00413                 int NumberData;
00414                 sscanf(tok, "%X", &NumberData);
00415 
00416                 if (debug)
00417                         printf("NumberData : %d\n", NumberData);
00418 
00419                 if (type == 0) {
00420                         data.dist_len1 = NumberData;
00421                 } else if (type == 1) {
00422                         data.dist_len2 = NumberData;
00423                 } else if (type == 2) {
00424                         data.rssi_len1 = NumberData;
00425                 } else if (type == 3) {
00426                         data.rssi_len2 = NumberData;
00427                 }
00428                 for (int i = 0; i < NumberData; i++) {
00429                         int dat;
00430                         tok = strtok(NULL, " "); //data
00431                         sscanf(tok, "%X", &dat);
00432 
00433                         if (type == 0) {
00434                                 data.dist1[i] = dat;
00435                         } else if (type == 1) {
00436                                 data.dist2[i] = dat;
00437                         } else if (type == 2) {
00438                                 data.rssi1[i] = dat;
00439                         } else if (type == 3) {
00440                                 data.rssi2[i] = dat;
00441                         }
00442                 }
00443         }
00444         return true;
00445 }
00446 
00447 void LMS1xx::saveConfig() {
00448         char buf[100];
00449         sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
00450 
00451         write(sockDesc, buf, strlen(buf));
00452 
00453         int len = read(sockDesc, buf, 100);
00454         //      if (buf[0] != 0x02)
00455         //              std::cout << "invalid packet recieved" << std::endl;
00456         //      if (debug) {
00457         //              buf[len] = 0;
00458         //              std::cout << buf << std::endl;
00459         //      }
00460 }
00461 
00462 void LMS1xx::startDevice() {
00463         char buf[100];
00464         sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
00465 
00466         write(sockDesc, buf, strlen(buf));
00467 
00468         int len = read(sockDesc, buf, 100);
00469         //      if (buf[0] != 0x02)
00470         //              std::cout << "invalid packet recieved" << std::endl;
00471         //      if (debug) {
00472         //              buf[len] = 0;
00473         //              std::cout << buf << std::endl;
00474         //      }
00475 }