LMS1xx.cpp
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/time.h>
00025 #include <sys/types.h>
00026 #include <sys/socket.h>
00027 #include <netinet/in.h>
00028 #include <arpa/inet.h>
00029 #include <cstdio>
00030 #include <cstdlib>
00031 #include <cstring>
00032 #include <errno.h>
00033 #include <fcntl.h>
00034 #include <unistd.h>
00035 
00036 #include "LMS1xx/LMS1xx.h"
00037 #include "console_bridge/console.h"
00038 
00039 LMS1xx::LMS1xx() : connected_(false)
00040 {
00041 }
00042 
00043 LMS1xx::~LMS1xx()
00044 {
00045 }
00046 
00047 void LMS1xx::connect(std::string host, int port)
00048 {
00049   if (!connected_)
00050   {
00051     logDebug("Creating non-blocking socket.");
00052     socket_fd_ = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
00053     if (socket_fd_)
00054     {
00055       struct sockaddr_in stSockAddr;
00056       stSockAddr.sin_family = PF_INET;
00057       stSockAddr.sin_port = htons(port);
00058       inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr);
00059 
00060       logDebug("Connecting socket to laser.");
00061       int ret = ::connect(socket_fd_, (struct sockaddr *) &stSockAddr, sizeof(stSockAddr));
00062 
00063       if (ret == 0)
00064       {
00065         connected_ = true;
00066         logDebug("Connected succeeded.");
00067       }
00068     }
00069   }
00070 }
00071 
00072 void LMS1xx::disconnect()
00073 {
00074   if (connected_)
00075   {
00076     close(socket_fd_);
00077     connected_ = false;
00078   }
00079 }
00080 
00081 bool LMS1xx::isConnected()
00082 {
00083   return connected_;
00084 }
00085 
00086 void LMS1xx::startMeas()
00087 {
00088   char buf[100];
00089   sprintf(buf, "%c%s%c", 0x02, "sMN LMCstartmeas", 0x03);
00090 
00091   write(socket_fd_, buf, strlen(buf));
00092 
00093   int len = read(socket_fd_, buf, 100);
00094   if (buf[0] != 0x02)
00095     logWarn("invalid packet recieved");
00096   buf[len] = 0;
00097   logDebug("RX: %s", buf);
00098 }
00099 
00100 void LMS1xx::stopMeas()
00101 {
00102   char buf[100];
00103   sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03);
00104 
00105   write(socket_fd_, buf, strlen(buf));
00106 
00107   int len = read(socket_fd_, buf, 100);
00108   if (buf[0] != 0x02)
00109     logWarn("invalid packet recieved");
00110   buf[len] = 0;
00111   logDebug("RX: %s", buf);
00112 }
00113 
00114 status_t LMS1xx::queryStatus()
00115 {
00116   char buf[100];
00117   sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03);
00118 
00119   write(socket_fd_, buf, strlen(buf));
00120 
00121   int len = read(socket_fd_, buf, 100);
00122   if (buf[0] != 0x02)
00123     logWarn("invalid packet recieved");
00124   buf[len] = 0;
00125   logDebug("RX: %s", buf);
00126 
00127   int ret;
00128   sscanf((buf + 10), "%d", &ret);
00129 
00130   return (status_t) ret;
00131 }
00132 
00133 void LMS1xx::login()
00134 {
00135   char buf[100];
00136   int result;
00137   sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
00138 
00139   fd_set readset;
00140   struct timeval timeout;
00141 
00142 
00143   do   //loop until data is available to read
00144   {
00145     timeout.tv_sec = 1;
00146     timeout.tv_usec = 0;
00147 
00148     write(socket_fd_, buf, strlen(buf));
00149 
00150     FD_ZERO(&readset);
00151     FD_SET(socket_fd_, &readset);
00152     result = select(socket_fd_ + 1, &readset, NULL, NULL, &timeout);
00153 
00154   }
00155   while (result <= 0);
00156 
00157   int len = read(socket_fd_, buf, 100);
00158   if (buf[0] != 0x02)
00159     logWarn("invalid packet recieved");
00160   buf[len] = 0;
00161   logDebug("RX: %s", buf);
00162 }
00163 
00164 scanCfg LMS1xx::getScanCfg() const
00165 {
00166   scanCfg cfg;
00167   char buf[100];
00168   sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
00169 
00170   write(socket_fd_, buf, strlen(buf));
00171 
00172   int len = read(socket_fd_, buf, 100);
00173   if (buf[0] != 0x02)
00174     logWarn("invalid packet recieved");
00175   buf[len] = 0;
00176   logDebug("RX: %s", buf);
00177 
00178   sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency,
00179          &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle);
00180   return cfg;
00181 }
00182 
00183 void LMS1xx::setScanCfg(const scanCfg &cfg)
00184 {
00185   char buf[100];
00186   sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg",
00187           cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle,
00188           cfg.stopAngle, 0x03);
00189 
00190   write(socket_fd_, buf, strlen(buf));
00191 
00192   int len = read(socket_fd_, buf, 100);
00193 
00194   buf[len - 1] = 0;
00195 }
00196 
00197 void LMS1xx::setScanDataCfg(const scanDataCfg &cfg)
00198 {
00199   char buf[100];
00200   sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02,
00201           "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0,
00202           cfg.resolution, cfg.encoder, cfg.position ? 1 : 0,
00203           cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03);
00204   logDebug("TX: %s", buf);
00205   write(socket_fd_, buf, strlen(buf));
00206 
00207   int len = read(socket_fd_, buf, 100);
00208   buf[len - 1] = 0;
00209 }
00210 
00211 scanOutputRange LMS1xx::getScanOutputRange() const
00212 {
00213   scanOutputRange outputRange;
00214   char buf[100];
00215   sprintf(buf, "%c%s%c", 0x02, "sRN LMPoutputRange", 0x03);
00216 
00217   write(socket_fd_, buf, strlen(buf));
00218 
00219   int len = read(socket_fd_, buf, 100);
00220 
00221   sscanf(buf + 1, "%*s %*s %*d %X %X %X", &outputRange.angleResolution,
00222          &outputRange.startAngle, &outputRange.stopAngle);
00223   return outputRange;
00224 }
00225 
00226 void LMS1xx::scanContinous(int start)
00227 {
00228   char buf[100];
00229   sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03);
00230 
00231   write(socket_fd_, buf, strlen(buf));
00232 
00233   int len = read(socket_fd_, buf, 100);
00234 
00235   if (buf[0] != 0x02)
00236     logError("invalid packet recieved");
00237 
00238   buf[len] = 0;
00239   logDebug("RX: %s", buf);
00240 }
00241 
00242 bool LMS1xx::getScanData(scanData* scan_data)
00243 {
00244   fd_set rfds;
00245   FD_ZERO(&rfds);
00246   FD_SET(socket_fd_, &rfds);
00247 
00248   // Block a total of up to 100ms waiting for more data from the laser.
00249   while (1)
00250   {
00251     // Would be great to depend on linux's behaviour of updating the timeval, but unfortunately
00252     // that's non-POSIX (doesn't work on OS X, for example).
00253     struct timeval tv;
00254     tv.tv_sec = 0;
00255     tv.tv_usec = 100000;
00256 
00257     logDebug("entering select()", tv.tv_usec);
00258     int retval = select(socket_fd_ + 1, &rfds, NULL, NULL, &tv);
00259     logDebug("returned %d from select()", retval);
00260     if (retval)
00261     {
00262       buffer_.readFrom(socket_fd_);
00263 
00264       // Will return pointer if a complete message exists in the buffer,
00265       // otherwise will return null.
00266       char* buffer_data = buffer_.getNextBuffer();
00267 
00268       if (buffer_data)
00269       {
00270         parseScanData(buffer_data, scan_data);
00271         buffer_.popLastBuffer();
00272         return true;
00273       }
00274     }
00275     else
00276     {
00277       // Select timed out or there was an fd error.
00278       return false;
00279     }
00280   }
00281 }
00282 
00283 
00284 void LMS1xx::parseScanData(char* buffer, scanData* data)
00285 {
00286   char* tok = strtok(buffer, " "); //Type of command
00287   tok = strtok(NULL, " "); //Command
00288   tok = strtok(NULL, " "); //VersionNumber
00289   tok = strtok(NULL, " "); //DeviceNumber
00290   tok = strtok(NULL, " "); //Serial number
00291   tok = strtok(NULL, " "); //DeviceStatus
00292   tok = strtok(NULL, " "); //MessageCounter
00293   tok = strtok(NULL, " "); //ScanCounter
00294   tok = strtok(NULL, " "); //PowerUpDuration
00295   tok = strtok(NULL, " "); //TransmissionDuration
00296   tok = strtok(NULL, " "); //InputStatus
00297   tok = strtok(NULL, " "); //OutputStatus
00298   tok = strtok(NULL, " "); //ReservedByteA
00299   tok = strtok(NULL, " "); //ScanningFrequency
00300   tok = strtok(NULL, " "); //MeasurementFrequency
00301   tok = strtok(NULL, " ");
00302   tok = strtok(NULL, " ");
00303   tok = strtok(NULL, " ");
00304   tok = strtok(NULL, " "); //NumberEncoders
00305   int NumberEncoders;
00306   sscanf(tok, "%d", &NumberEncoders);
00307   for (int i = 0; i < NumberEncoders; i++)
00308   {
00309     tok = strtok(NULL, " "); //EncoderPosition
00310     tok = strtok(NULL, " "); //EncoderSpeed
00311   }
00312 
00313   tok = strtok(NULL, " "); //NumberChannels16Bit
00314   int NumberChannels16Bit;
00315   sscanf(tok, "%d", &NumberChannels16Bit);
00316   logDebug("NumberChannels16Bit : %d", NumberChannels16Bit);
00317 
00318   for (int i = 0; i < NumberChannels16Bit; i++)
00319   {
00320     int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2
00321     char content[6];
00322     tok = strtok(NULL, " "); //MeasuredDataContent
00323     sscanf(tok, "%s", content);
00324     if (!strcmp(content, "DIST1"))
00325     {
00326       type = 0;
00327     }
00328     else if (!strcmp(content, "DIST2"))
00329     {
00330       type = 1;
00331     }
00332     else if (!strcmp(content, "RSSI1"))
00333     {
00334       type = 2;
00335     }
00336     else if (!strcmp(content, "RSSI2"))
00337     {
00338       type = 3;
00339     }
00340     tok = strtok(NULL, " "); //ScalingFactor
00341     tok = strtok(NULL, " "); //ScalingOffset
00342     tok = strtok(NULL, " "); //Starting angle
00343     tok = strtok(NULL, " "); //Angular step width
00344     tok = strtok(NULL, " "); //NumberData
00345     int NumberData;
00346     sscanf(tok, "%X", &NumberData);
00347     logDebug("NumberData : %d", NumberData);
00348 
00349     if (type == 0)
00350     {
00351       data->dist_len1 = NumberData;
00352     }
00353     else if (type == 1)
00354     {
00355       data->dist_len2 = NumberData;
00356     }
00357     else if (type == 2)
00358     {
00359       data->rssi_len1 = NumberData;
00360     }
00361     else if (type == 3)
00362     {
00363       data->rssi_len2 = NumberData;
00364     }
00365 
00366     for (int i = 0; i < NumberData; i++)
00367     {
00368       int dat;
00369       tok = strtok(NULL, " "); //data
00370       sscanf(tok, "%X", &dat);
00371 
00372       if (type == 0)
00373       {
00374         data->dist1[i] = dat;
00375       }
00376       else if (type == 1)
00377       {
00378         data->dist2[i] = dat;
00379       }
00380       else if (type == 2)
00381       {
00382         data->rssi1[i] = dat;
00383       }
00384       else if (type == 3)
00385       {
00386         data->rssi2[i] = dat;
00387       }
00388 
00389     }
00390   }
00391 
00392   tok = strtok(NULL, " "); //NumberChannels8Bit
00393   int NumberChannels8Bit;
00394   sscanf(tok, "%d", &NumberChannels8Bit);
00395   logDebug("NumberChannels8Bit : %d\n", NumberChannels8Bit);
00396 
00397   for (int i = 0; i < NumberChannels8Bit; i++)
00398   {
00399     int type = -1;
00400     char content[6];
00401     tok = strtok(NULL, " "); //MeasuredDataContent
00402     sscanf(tok, "%s", content);
00403     if (!strcmp(content, "DIST1"))
00404     {
00405       type = 0;
00406     }
00407     else if (!strcmp(content, "DIST2"))
00408     {
00409       type = 1;
00410     }
00411     else if (!strcmp(content, "RSSI1"))
00412     {
00413       type = 2;
00414     }
00415     else if (!strcmp(content, "RSSI2"))
00416     {
00417       type = 3;
00418     }
00419     tok = strtok(NULL, " "); //ScalingFactor
00420     tok = strtok(NULL, " "); //ScalingOffset
00421     tok = strtok(NULL, " "); //Starting angle
00422     tok = strtok(NULL, " "); //Angular step width
00423     tok = strtok(NULL, " "); //NumberData
00424     int NumberData;
00425     sscanf(tok, "%X", &NumberData);
00426     logDebug("NumberData : %d\n", NumberData);
00427 
00428     if (type == 0)
00429     {
00430       data->dist_len1 = NumberData;
00431     }
00432     else if (type == 1)
00433     {
00434       data->dist_len2 = NumberData;
00435     }
00436     else if (type == 2)
00437     {
00438       data->rssi_len1 = NumberData;
00439     }
00440     else if (type == 3)
00441     {
00442       data->rssi_len2 = NumberData;
00443     }
00444     for (int i = 0; i < NumberData; i++)
00445     {
00446       int dat;
00447       tok = strtok(NULL, " "); //data
00448       sscanf(tok, "%X", &dat);
00449 
00450       if (type == 0)
00451       {
00452         data->dist1[i] = dat;
00453       }
00454       else if (type == 1)
00455       {
00456         data->dist2[i] = dat;
00457       }
00458       else if (type == 2)
00459       {
00460         data->rssi1[i] = dat;
00461       }
00462       else if (type == 3)
00463       {
00464         data->rssi2[i] = dat;
00465       }
00466     }
00467   }
00468 }
00469 
00470 void LMS1xx::saveConfig()
00471 {
00472   char buf[100];
00473   sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
00474 
00475   write(socket_fd_, buf, strlen(buf));
00476 
00477   int len = read(socket_fd_, buf, 100);
00478 
00479   if (buf[0] != 0x02)
00480     logWarn("invalid packet recieved");
00481   buf[len] = 0;
00482   logDebug("RX: %s", buf);
00483 }
00484 
00485 void LMS1xx::startDevice()
00486 {
00487   char buf[100];
00488   sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
00489 
00490   write(socket_fd_, buf, strlen(buf));
00491 
00492   int len = read(socket_fd_, buf, 100);
00493 
00494   if (buf[0] != 0x02)
00495     logWarn("invalid packet recieved");
00496   buf[len] = 0;
00497   logDebug("RX: %s", buf);
00498 }


lms1xx
Author(s): Konrad Banachowicz
autogenerated on Thu Jun 6 2019 18:52:14