lms1xx.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <sys/types.h>
19 #include <sys/socket.h>
20 #include <netinet/in.h>
21 #include <arpa/inet.h>
22 #include <cstdio>
23 #include <cstdlib>
24 #include <cstring>
25 #include <unistd.h>
26 #include <iostream>
27 #include <errno.h>
28 
29 #include "lms1xx.h"
30 
31 LMS1xx::LMS1xx() :
32  connected(false) {
33  debug = false;
34 }
35 
36 LMS1xx::~LMS1xx() {
37 
38 }
39 
40 void LMS1xx::connect(std::string host, int port) {
41  if (!connected) {
42  sockDesc = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
43  if (sockDesc) {
44  struct sockaddr_in stSockAddr;
45  int Res;
46  stSockAddr.sin_family = PF_INET;
47  stSockAddr.sin_port = htons(port);
48  Res = inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr);
49 
50  int ret = ::connect(sockDesc, (struct sockaddr *) &stSockAddr,
51  sizeof stSockAddr);
52  if (ret == 0) {
53  connected = true;
54  }
55  }
56  }
57 }
58 
59 void LMS1xx::disconnect() {
60  if (connected) {
61  close(sockDesc);
62  connected = false;
63  }
64 }
65 
66 bool LMS1xx::isConnected() {
67  return connected;
68 }
69 
70 void LMS1xx::startMeas() {
71  char buf[100];
72  sprintf(buf, "%c%s%c", 0x02, "sMN LMCstartmeas", 0x03);
73 
74  ssize_t res = write(sockDesc, buf, strlen(buf));
75 
76  int len = read(sockDesc, buf, 100);
77  // if (buf[0] != 0x02)
78  // std::cout << "invalid packet recieved" << std::endl;
79  // if (debug) {
80  // buf[len] = 0;
81  // std::cout << buf << std::endl;
82  // }
83 }
84 
85 void LMS1xx::stopMeas() {
86  char buf[100];
87  sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03);
88 
89  ssize_t res = write(sockDesc, buf, strlen(buf));
90 
91  int len = read(sockDesc, buf, 100);
92  // if (buf[0] != 0x02)
93  // std::cout << "invalid packet recieved" << std::endl;
94  // if (debug) {
95  // buf[len] = 0;
96  // std::cout << buf << std::endl;
97  // }
98 }
99 
100 status_t LMS1xx::queryStatus() {
101  char buf[100];
102  sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03);
103 
104  ssize_t res = write(sockDesc, buf, strlen(buf));
105 
106  int len = read(sockDesc, buf, 100);
107  // if (buf[0] != 0x02)
108  // std::cout << "invalid packet recieved" << std::endl;
109  // if (debug) {
110  // buf[len] = 0;
111  // std::cout << buf << std::endl;
112  // }
113  int ret;
114  sscanf((buf + 10), "%d", &ret);
115 
116  return (status_t) ret;
117 }
118 
119 void LMS1xx::login() {
120  char buf[100];
121  sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
122 
123  ssize_t res = write(sockDesc, buf, strlen(buf));
124 
125  int len = read(sockDesc, buf, 100);
126  // if (buf[0] != 0x02)
127  // std::cout << "invalid packet recieved" << std::endl;
128  // if (debug) {
129  // buf[len] = 0;
130  // std::cout << buf << std::endl;
131  // }
132 }
133 
134 scanCfg LMS1xx::getScanCfg() const {
135  scanCfg cfg;
136  char buf[100];
137  sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
138 
139  ssize_t res = write(sockDesc, buf, strlen(buf));
140 
141  int len = read(sockDesc, buf, 100);
142  // if (buf[0] != 0x02)
143  // std::cout << "invalid packet recieved" << std::endl;
144  // if (debug) {
145  // buf[len] = 0;
146  // std::cout << buf << std::endl;
147  // }
148 
149  sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency,
150  &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle);
151  return cfg;
152 }
153 
154 void LMS1xx::setScanCfg(const scanCfg &cfg) {
155  char buf[100];
156  sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg",
157  cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle,
158  cfg.stopAngle, 0x03);
159 
160  ssize_t res = write(sockDesc, buf, strlen(buf));
161 
162  int len = read(sockDesc, buf, 100);
163 
164  buf[len - 1] = 0;
165 }
166 
167 void LMS1xx::setScanDataCfg(const scanDataCfg &cfg) {
168  char buf[100];
169  sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02,
170  "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0,
171  cfg.resolution, cfg.encoder, cfg.position ? 1 : 0,
172  cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03);
173  if(debug)
174  printf("%s\n", buf);
175  ssize_t res = write(sockDesc, buf, strlen(buf));
176 
177  int len = read(sockDesc, buf, 100);
178  buf[len - 1] = 0;
179 }
180 
181 void LMS1xx::scanContinous(int start) {
182  char buf[100];
183  sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03);
184 
185  ssize_t res = write(sockDesc, buf, strlen(buf));
186 
187  int len = read(sockDesc, buf, 100);
188 
189  if (buf[0] != 0x02)
190  printf("invalid packet recieved\n");
191 
192  if (debug) {
193  buf[len] = 0;
194  printf("%s\n", buf);
195  }
196 
197  if (start = 0) {
198  for (int i = 0; i < 10; i++)
199  ssize_t res = read(sockDesc, buf, 100);
200  }
201 }
202 
203 bool LMS1xx::getData(scanData& data) {
204  char buf[20000];
205  fd_set rfds;
206  struct timeval tv;
207  int retval, len = 0;
208  int bytes_read = 0;
209  int inc = 0;
210  int r_off = 0;
211 
212  do{
213  FD_ZERO(&rfds);
214  FD_SET(sockDesc, &rfds);
215  tv.tv_sec = 1;
216  tv.tv_usec = 0;
217  bytes_read = 0; inc = 0;
218  retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv);
219  if(retval)
220  {
221  bytes_read = read(sockDesc, buf, 1);
222  if(bytes_read > 1)
223  {
224  //std::cerr<<"Error in getData: received "<< bytes_read <<" bytes"<<std::endl;
225  inc = bytes_read;
226  inc --;
227  }
228  else if(bytes_read < 1)
229  {
230  //std::cerr<<"Error in getData: received "<< bytes_read <<" bytes"<<std::endl;
231  }
232  }
233  else
234  {
235  //std::cerr<<"select failed during find start: "<<strerror(errno)<<std::endl;
236  }
237 
238  }while(buf[0+inc] != 0x02);
239  len += bytes_read;
240 
241 
242  do {
243  bool bFoundEnd = false;
244  bool bFoundStart = false;
245  if(len >= 20000)
246  {
247  int i;
248  for(i = 0; i < len; i++)
249  {
250  if(buf[i] == 0x03)
251  {
252  len = i+1;
253  bFoundEnd = true;
254  //std::cerr<<"Found end!\n";
255  }
256  if(bFoundEnd && buf[i] == 0x02)
257  {
258  //std::cerr<<"Found start!\n";
259  bFoundStart = true;
260  }
261  }
262  if(!bFoundEnd)
263  {
264  char flushBuffer[100000];
265  int bytesFlushed = recv(sockDesc, flushBuffer, 100000, 0);
266  //std::cerr<<"Flushed read buffer"<<std::endl;
267  return false;
268  }
269  }
270  if(!bFoundEnd)
271  {
272  FD_ZERO(&rfds);
273  FD_SET(sockDesc, &rfds);
274 
275  tv.tv_sec = 0;
276  tv.tv_usec = 100000;
277  bytes_read = 0;
278  retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv);
279  if (retval)
280  {
281  bytes_read = read(sockDesc, buf + len, 20000 - len);
282  if(bytes_read < 1)
283  {
284  //std::cerr<<"Error in getData: received "<< bytes_read <<" bytes"<<std::endl;
285  }
286  else
287  len += bytes_read;
288  }
289  else
290  {
291  //std::cerr<<"select failed during get data: "<<strerror(errno)<<std::endl;
292  }
293  }
294  } while ((buf[len - 1] != 0x03));
295 
296  // if (debug)
297  // std::cout << "scan data recieved" << std::endl;
298  buf[len - 1] = 0;
299  char* tok = strtok(buf, " "); //Type of command
300  tok = strtok(NULL, " "); //Command
301  tok = strtok(NULL, " "); //VersionNumber
302  tok = strtok(NULL, " "); //DeviceNumber
303  tok = strtok(NULL, " "); //Serial number
304  tok = strtok(NULL, " "); //DeviceStatus
305  tok = strtok(NULL, " "); //MessageCounter
306  tok = strtok(NULL, " "); //ScanCounter
307  tok = strtok(NULL, " "); //PowerUpDuration
308  tok = strtok(NULL, " "); //TransmissionDuration
309  tok = strtok(NULL, " "); //InputStatus
310  tok = strtok(NULL, " "); //OutputStatus
311  tok = strtok(NULL, " "); //ReservedByteA
312  tok = strtok(NULL, " "); //ScanningFrequency
313  tok = strtok(NULL, " "); //MeasurementFrequency
314  tok = strtok(NULL, " ");
315  tok = strtok(NULL, " ");
316  tok = strtok(NULL, " ");
317  tok = strtok(NULL, " "); //NumberEncoders
318  int NumberEncoders;
319  sscanf(tok, "%d", &NumberEncoders);
320  for (int i = 0; i < NumberEncoders; i++) {
321  tok = strtok(NULL, " "); //EncoderPosition
322  tok = strtok(NULL, " "); //EncoderSpeed
323  }
324 
325  tok = strtok(NULL, " "); //NumberChannels16Bit
326  int NumberChannels16Bit;
327  sscanf(tok, "%d", &NumberChannels16Bit);
328  if (debug)
329  printf("NumberChannels16Bit : %d\n", NumberChannels16Bit);
330  for (int i = 0; i < NumberChannels16Bit; i++) {
331  int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2
332  char content[6];
333  tok = strtok(NULL, " "); //MeasuredDataContent
334  sscanf(tok, "%s", content);
335  if (!strcmp(content, "DIST1")) {
336  type = 0;
337  } else if (!strcmp(content, "DIST2")) {
338  type = 1;
339  } else if (!strcmp(content, "RSSI1")) {
340  type = 2;
341  } else if (!strcmp(content, "RSSI2")) {
342  type = 3;
343  }
344  tok = strtok(NULL, " "); //ScalingFactor
345  tok = strtok(NULL, " "); //ScalingOffset
346  tok = strtok(NULL, " "); //Starting angle
347  tok = strtok(NULL, " "); //Angular step width
348  tok = strtok(NULL, " "); //NumberData
349  int NumberData;
350  sscanf(tok, "%X", &NumberData);
351 
352  if (debug)
353  printf("NumberData : %d\n", NumberData);
354 
355  if (type == 0) {
356  data.dist_len1 = NumberData;
357  } else if (type == 1) {
358  data.dist_len2 = NumberData;
359  } else if (type == 2) {
360  data.rssi_len1 = NumberData;
361  } else if (type == 3) {
362  data.rssi_len2 = NumberData;
363  }
364 
365  for (int i = 0; i < NumberData; i++) {
366  int dat;
367  tok = strtok(NULL, " "); //data
368  sscanf(tok, "%X", &dat);
369 
370  if (type == 0) {
371  data.dist1[i] = dat;
372  } else if (type == 1) {
373  data.dist2[i] = dat;
374  } else if (type == 2) {
375  data.rssi1[i] = dat;
376  } else if (type == 3) {
377  data.rssi2[i] = dat;
378  }
379 
380  }
381  }
382 
383  tok = strtok(NULL, " "); //NumberChannels8Bit
384  int NumberChannels8Bit;
385  sscanf(tok, "%d", &NumberChannels8Bit);
386  if (debug)
387  printf("NumberChannels8Bit : %d\n", NumberChannels8Bit);
388  for (int i = 0; i < NumberChannels8Bit; i++) {
389  int type = -1;
390  char content[6];
391  tok = strtok(NULL, " "); //MeasuredDataContent
392  sscanf(tok, "%s", content);
393  if (!strcmp(content, "DIST1")) {
394  type = 0;
395  } else if (!strcmp(content, "DIST2")) {
396  type = 1;
397  } else if (!strcmp(content, "RSSI1")) {
398  type = 2;
399  } else if (!strcmp(content, "RSSI2")) {
400  type = 3;
401  }
402  tok = strtok(NULL, " "); //ScalingFactor
403  tok = strtok(NULL, " "); //ScalingOffset
404  tok = strtok(NULL, " "); //Starting angle
405  tok = strtok(NULL, " "); //Angular step width
406  tok = strtok(NULL, " "); //NumberData
407  int NumberData;
408  sscanf(tok, "%X", &NumberData);
409 
410  if (debug)
411  printf("NumberData : %d\n", NumberData);
412 
413  if (type == 0) {
414  data.dist_len1 = NumberData;
415  } else if (type == 1) {
416  data.dist_len2 = NumberData;
417  } else if (type == 2) {
418  data.rssi_len1 = NumberData;
419  } else if (type == 3) {
420  data.rssi_len2 = NumberData;
421  }
422  for (int i = 0; i < NumberData; i++) {
423  int dat;
424  tok = strtok(NULL, " "); //data
425  sscanf(tok, "%X", &dat);
426 
427  if (type == 0) {
428  data.dist1[i] = dat;
429  } else if (type == 1) {
430  data.dist2[i] = dat;
431  } else if (type == 2) {
432  data.rssi1[i] = dat;
433  } else if (type == 3) {
434  data.rssi2[i] = dat;
435  }
436  }
437  }
438  return true;
439 }
440 
441 void LMS1xx::saveConfig() {
442  char buf[100];
443  sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
444 
445  ssize_t res = write(sockDesc, buf, strlen(buf));
446 
447  int len = read(sockDesc, buf, 100);
448  // if (buf[0] != 0x02)
449  // std::cout << "invalid packet recieved" << std::endl;
450  // if (debug) {
451  // buf[len] = 0;
452  // std::cout << buf << std::endl;
453  // }
454 }
455 
456 void LMS1xx::startDevice() {
457  char buf[100];
458  sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
459 
460  ssize_t res = write(sockDesc, buf, strlen(buf));
461 
462  int len = read(sockDesc, buf, 100);
463  // if (buf[0] != 0x02)
464  // std::cout << "invalid packet recieved" << std::endl;
465  // if (debug) {
466  // buf[len] = 0;
467  // std::cout << buf << std::endl;
468  // }
469 }
LMS1xx::login
void login()
Log into LMS1xx unit. Increase privilege level, giving ability to change device configuration.
Definition: lms1xx.cpp:119
LMS1xx::startDevice
void startDevice()
The device is returned to the measurement mode after configuration.
Definition: lms1xx.cpp:456
LMS1xx::stopMeas
void stopMeas()
Stop measurements. After receiving this command LMS1xx unit stop spinning laser and measuring.
Definition: lms1xx.cpp:85
LMS1xx::~LMS1xx
virtual ~LMS1xx()
Definition: lms1xx.cpp:36
status_t
status_t
Definition: lms1xx.h:171
LMS1xx::sockDesc
int sockDesc
Definition: lms1xx.h:296
LMS1xx::saveConfig
void saveConfig()
Save data permanently. Parameters are saved in the EEPROM of the LMS and will also be available after...
Definition: lms1xx.cpp:441
LMS1xx::queryStatus
status_t queryStatus()
Get current status of LMS1xx device.
Definition: lms1xx.cpp:100
scanCfg
Structure containing scan configuration.
LMS1xx::setScanCfg
void setScanCfg(const scanCfg &cfg)
Set scan configuration. Get scan configuration :
Definition: lms1xx.cpp:154
LMS1xx::disconnect
void disconnect()
Disconnect from LMS1xx device.
Definition: lms1xx.cpp:59
LMS1xx::isConnected
bool isConnected()
Get status of connection.
Definition: lms1xx.cpp:66
LMS1xx::startMeas
void startMeas()
Start measurements. After receiving this command LMS1xx unit starts spinning laser and measuring.
Definition: lms1xx.cpp:70
LMS1xx::debug
bool debug
Definition: lms1xx.h:294
LMS1xx::LMS1xx
LMS1xx()
Definition: lms1xx.cpp:31
start
ROSCPP_DECL void start()
LMS1xx::connected
bool connected
Definition: lms1xx.h:293
lms1xx.h
LMS1xx::getData
bool getData(scanData &data)
Receive single scan message.
Definition: lms1xx.cpp:203
scanData
Structure containing single scan message.
scanDataCfg
Structure containing scan data configuration.
LMS1xx::setScanDataCfg
void setScanDataCfg(const scanDataCfg &cfg)
Set scan data configuration. Set format of scan message returned by device.
Definition: lms1xx.cpp:167
LMS1xx::getScanCfg
scanCfg getScanCfg() const
Get current scan configuration. Get scan configuration :
Definition: lms1xx.cpp:134
LMS1xx::scanContinous
void scanContinous(int start)
Start or stop continuous data acquisition. After reception of this command device start or stop conti...
Definition: lms1xx.cpp:181
LMS1xx::connect
void connect(std::string host, int port=2111)
Connect to LMS1xx.
Definition: lms1xx.cpp:40

cob_sick_lms1xx
Author(s): Konrad Banachowicz
autogenerated on Wed Nov 8 2023 03:47:47