LMS1xx.cpp
Go to the documentation of this file.
1 /*
2  * LMS1xx.cpp
3  *
4  * Created on: 09-08-2010
5  * Author: Konrad Banachowicz
6  ***************************************************************************
7  * This library is free software; you can redistribute it and/or *
8  * modify it under the terms of the GNU Lesser General Public *
9  * License as published by the Free Software Foundation; either *
10  * version 2.1 of the License, or (at your option) any later version. *
11  * *
12  * This library is distributed in the hope that it will be useful, *
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
15  * Lesser General Public License for more details. *
16  * *
17  * You should have received a copy of the GNU Lesser General Public *
18  * License along with this library; if not, write to the Free Software *
19  * Foundation, Inc., 59 Temple Place, *
20  * Suite 330, Boston, MA 02111-1307 USA *
21  * *
22  ***************************************************************************/
23 
24 #include <sys/time.h>
25 #include <sys/types.h>
26 #include <sys/socket.h>
27 #include <netinet/in.h>
28 #include <arpa/inet.h>
29 #include <cstdio>
30 #include <cstdlib>
31 #include <cstring>
32 #include <errno.h>
33 #include <fcntl.h>
34 #include <unistd.h>
35 
36 #include "LMS1xx/LMS1xx.h"
37 #include "console_bridge/console.h"
38 
39 LMS1xx::LMS1xx() : connected_(false)
40 {
41 }
42 
43 LMS1xx::~LMS1xx()
44 {
45 }
46 
47 void LMS1xx::connect(std::string host, int port)
48 {
49  if (!connected_)
50  {
51  logDebug("Creating non-blocking socket.");
52  socket_fd_ = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
53  if (socket_fd_)
54  {
55  struct sockaddr_in stSockAddr;
56  stSockAddr.sin_family = PF_INET;
57  stSockAddr.sin_port = htons(port);
58  inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr);
59 
60  logDebug("Connecting socket to laser.");
61  int ret = ::connect(socket_fd_, (struct sockaddr *) &stSockAddr, sizeof(stSockAddr));
62 
63  if (ret == 0)
64  {
65  connected_ = true;
66  logDebug("Connected succeeded.");
67  }
68  }
69  }
70 }
71 
72 void LMS1xx::disconnect()
73 {
74  if (connected_)
75  {
76  close(socket_fd_);
77  connected_ = false;
78  }
79 }
80 
81 bool LMS1xx::isConnected()
82 {
83  return connected_;
84 }
85 
86 void LMS1xx::startMeas()
87 {
88  char buf[100];
89  sprintf(buf, "%c%s%c", 0x02, "sMN LMCstartmeas", 0x03);
90 
91  write(socket_fd_, buf, strlen(buf));
92 
93  int len = read(socket_fd_, buf, 100);
94  if (buf[0] != 0x02)
95  logWarn("invalid packet recieved");
96  buf[len] = 0;
97  logDebug("RX: %s", buf);
98 }
99 
100 void LMS1xx::stopMeas()
101 {
102  char buf[100];
103  sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03);
104 
105  write(socket_fd_, buf, strlen(buf));
106 
107  int len = read(socket_fd_, buf, 100);
108  if (buf[0] != 0x02)
109  logWarn("invalid packet recieved");
110  buf[len] = 0;
111  logDebug("RX: %s", buf);
112 }
113 
114 status_t LMS1xx::queryStatus()
115 {
116  char buf[100];
117  sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03);
118 
119  write(socket_fd_, buf, strlen(buf));
120 
121  int len = read(socket_fd_, buf, 100);
122  if (buf[0] != 0x02)
123  logWarn("invalid packet recieved");
124  buf[len] = 0;
125  logDebug("RX: %s", buf);
126 
127  int ret;
128  sscanf((buf + 10), "%d", &ret);
129 
130  return (status_t) ret;
131 }
132 
133 void LMS1xx::login()
134 {
135  char buf[100];
136  int result;
137  sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
138 
139  fd_set readset;
140  struct timeval timeout;
141 
142 
143  do //loop until data is available to read
144  {
145  timeout.tv_sec = 1;
146  timeout.tv_usec = 0;
147 
148  write(socket_fd_, buf, strlen(buf));
149 
150  FD_ZERO(&readset);
151  FD_SET(socket_fd_, &readset);
152  result = select(socket_fd_ + 1, &readset, NULL, NULL, &timeout);
153 
154  }
155  while (result <= 0);
156 
157  int len = read(socket_fd_, buf, 100);
158  if (buf[0] != 0x02)
159  logWarn("invalid packet recieved");
160  buf[len] = 0;
161  logDebug("RX: %s", buf);
162 }
163 
164 scanCfg LMS1xx::getScanCfg() const
165 {
166  scanCfg cfg;
167  char buf[100];
168  sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
169 
170  write(socket_fd_, buf, strlen(buf));
171 
172  int len = read(socket_fd_, buf, 100);
173  if (buf[0] != 0x02)
174  logWarn("invalid packet recieved");
175  buf[len] = 0;
176  logDebug("RX: %s", buf);
177 
178  sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency,
179  &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle);
180  return cfg;
181 }
182 
183 void LMS1xx::setScanCfg(const scanCfg &cfg)
184 {
185  char buf[100];
186  sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg",
187  cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle,
188  cfg.stopAngle, 0x03);
189 
190  write(socket_fd_, buf, strlen(buf));
191 
192  int len = read(socket_fd_, buf, 100);
193 
194  buf[len - 1] = 0;
195 }
196 
197 void LMS1xx::setScanDataCfg(const scanDataCfg &cfg)
198 {
199  char buf[100];
200  sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02,
201  "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0,
202  cfg.resolution, cfg.encoder, cfg.position ? 1 : 0,
203  cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03);
204  logDebug("TX: %s", buf);
205  write(socket_fd_, buf, strlen(buf));
206 
207  int len = read(socket_fd_, buf, 100);
208  buf[len - 1] = 0;
209 }
210 
211 scanOutputRange LMS1xx::getScanOutputRange() const
212 {
213  scanOutputRange outputRange;
214  char buf[100];
215  sprintf(buf, "%c%s%c", 0x02, "sRN LMPoutputRange", 0x03);
216 
217  write(socket_fd_, buf, strlen(buf));
218 
219  int len = read(socket_fd_, buf, 100);
220 
221  sscanf(buf + 1, "%*s %*s %*d %X %X %X", &outputRange.angleResolution,
222  &outputRange.startAngle, &outputRange.stopAngle);
223  return outputRange;
224 }
225 
226 void LMS1xx::scanContinous(int start)
227 {
228  char buf[100];
229  sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03);
230 
231  write(socket_fd_, buf, strlen(buf));
232 
233  int len = read(socket_fd_, buf, 100);
234 
235  if (buf[0] != 0x02)
236  logError("invalid packet recieved");
237 
238  buf[len] = 0;
239  logDebug("RX: %s", buf);
240 }
241 
242 bool LMS1xx::getScanData(scanData* scan_data)
243 {
244  fd_set rfds;
245  FD_ZERO(&rfds);
246  FD_SET(socket_fd_, &rfds);
247 
248  // Block a total of up to 100ms waiting for more data from the laser.
249  while (1)
250  {
251  // Would be great to depend on linux's behaviour of updating the timeval, but unfortunately
252  // that's non-POSIX (doesn't work on OS X, for example).
253  struct timeval tv;
254  tv.tv_sec = 0;
255  tv.tv_usec = 100000;
256 
257  logDebug("entering select()", tv.tv_usec);
258  int retval = select(socket_fd_ + 1, &rfds, NULL, NULL, &tv);
259  logDebug("returned %d from select()", retval);
260  if (retval)
261  {
262  buffer_.readFrom(socket_fd_);
263 
264  // Will return pointer if a complete message exists in the buffer,
265  // otherwise will return null.
266  char* buffer_data = buffer_.getNextBuffer();
267 
268  if (buffer_data)
269  {
270  parseScanData(buffer_data, scan_data);
271  buffer_.popLastBuffer();
272  return true;
273  }
274  }
275  else
276  {
277  // Select timed out or there was an fd error.
278  return false;
279  }
280  }
281 }
282 
283 
284 void LMS1xx::parseScanData(char* buffer, scanData* data)
285 {
286  char* tok = strtok(buffer, " "); //Type of command
287  tok = strtok(NULL, " "); //Command
288  tok = strtok(NULL, " "); //VersionNumber
289  tok = strtok(NULL, " "); //DeviceNumber
290  tok = strtok(NULL, " "); //Serial number
291  tok = strtok(NULL, " "); //DeviceStatus
292  tok = strtok(NULL, " "); //MessageCounter
293  tok = strtok(NULL, " "); //ScanCounter
294  tok = strtok(NULL, " "); //PowerUpDuration
295  tok = strtok(NULL, " "); //TransmissionDuration
296  tok = strtok(NULL, " "); //InputStatus
297  tok = strtok(NULL, " "); //OutputStatus
298  tok = strtok(NULL, " "); //ReservedByteA
299  tok = strtok(NULL, " "); //ScanningFrequency
300  tok = strtok(NULL, " "); //MeasurementFrequency
301  tok = strtok(NULL, " ");
302  tok = strtok(NULL, " ");
303  tok = strtok(NULL, " ");
304  tok = strtok(NULL, " "); //NumberEncoders
305  int NumberEncoders;
306  sscanf(tok, "%d", &NumberEncoders);
307  for (int i = 0; i < NumberEncoders; i++)
308  {
309  tok = strtok(NULL, " "); //EncoderPosition
310  tok = strtok(NULL, " "); //EncoderSpeed
311  }
312 
313  tok = strtok(NULL, " "); //NumberChannels16Bit
314  int NumberChannels16Bit;
315  sscanf(tok, "%d", &NumberChannels16Bit);
316  logDebug("NumberChannels16Bit : %d", NumberChannels16Bit);
317 
318  for (int i = 0; i < NumberChannels16Bit; i++)
319  {
320  int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2
321  char content[6];
322  tok = strtok(NULL, " "); //MeasuredDataContent
323  sscanf(tok, "%s", content);
324  if (!strcmp(content, "DIST1"))
325  {
326  type = 0;
327  }
328  else if (!strcmp(content, "DIST2"))
329  {
330  type = 1;
331  }
332  else if (!strcmp(content, "RSSI1"))
333  {
334  type = 2;
335  }
336  else if (!strcmp(content, "RSSI2"))
337  {
338  type = 3;
339  }
340  tok = strtok(NULL, " "); //ScalingFactor
341  tok = strtok(NULL, " "); //ScalingOffset
342  tok = strtok(NULL, " "); //Starting angle
343  tok = strtok(NULL, " "); //Angular step width
344  tok = strtok(NULL, " "); //NumberData
345  int NumberData;
346  sscanf(tok, "%X", &NumberData);
347  logDebug("NumberData : %d", NumberData);
348 
349  if (type == 0)
350  {
351  data->dist_len1 = NumberData;
352  }
353  else if (type == 1)
354  {
355  data->dist_len2 = NumberData;
356  }
357  else if (type == 2)
358  {
359  data->rssi_len1 = NumberData;
360  }
361  else if (type == 3)
362  {
363  data->rssi_len2 = NumberData;
364  }
365 
366  for (int i = 0; i < NumberData; i++)
367  {
368  int dat;
369  tok = strtok(NULL, " "); //data
370  sscanf(tok, "%X", &dat);
371 
372  if (type == 0)
373  {
374  data->dist1[i] = dat;
375  }
376  else if (type == 1)
377  {
378  data->dist2[i] = dat;
379  }
380  else if (type == 2)
381  {
382  data->rssi1[i] = dat;
383  }
384  else if (type == 3)
385  {
386  data->rssi2[i] = dat;
387  }
388 
389  }
390  }
391 
392  tok = strtok(NULL, " "); //NumberChannels8Bit
393  int NumberChannels8Bit;
394  sscanf(tok, "%d", &NumberChannels8Bit);
395  logDebug("NumberChannels8Bit : %d\n", NumberChannels8Bit);
396 
397  for (int i = 0; i < NumberChannels8Bit; i++)
398  {
399  int type = -1;
400  char content[6];
401  tok = strtok(NULL, " "); //MeasuredDataContent
402  sscanf(tok, "%s", content);
403  if (!strcmp(content, "DIST1"))
404  {
405  type = 0;
406  }
407  else if (!strcmp(content, "DIST2"))
408  {
409  type = 1;
410  }
411  else if (!strcmp(content, "RSSI1"))
412  {
413  type = 2;
414  }
415  else if (!strcmp(content, "RSSI2"))
416  {
417  type = 3;
418  }
419  tok = strtok(NULL, " "); //ScalingFactor
420  tok = strtok(NULL, " "); //ScalingOffset
421  tok = strtok(NULL, " "); //Starting angle
422  tok = strtok(NULL, " "); //Angular step width
423  tok = strtok(NULL, " "); //NumberData
424  int NumberData;
425  sscanf(tok, "%X", &NumberData);
426  logDebug("NumberData : %d\n", NumberData);
427 
428  if (type == 0)
429  {
430  data->dist_len1 = NumberData;
431  }
432  else if (type == 1)
433  {
434  data->dist_len2 = NumberData;
435  }
436  else if (type == 2)
437  {
438  data->rssi_len1 = NumberData;
439  }
440  else if (type == 3)
441  {
442  data->rssi_len2 = NumberData;
443  }
444  for (int i = 0; i < NumberData; i++)
445  {
446  int dat;
447  tok = strtok(NULL, " "); //data
448  sscanf(tok, "%X", &dat);
449 
450  if (type == 0)
451  {
452  data->dist1[i] = dat;
453  }
454  else if (type == 1)
455  {
456  data->dist2[i] = dat;
457  }
458  else if (type == 2)
459  {
460  data->rssi1[i] = dat;
461  }
462  else if (type == 3)
463  {
464  data->rssi2[i] = dat;
465  }
466  }
467  }
468 }
469 
470 void LMS1xx::saveConfig()
471 {
472  char buf[100];
473  sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
474 
475  write(socket_fd_, buf, strlen(buf));
476 
477  int len = read(socket_fd_, buf, 100);
478 
479  if (buf[0] != 0x02)
480  logWarn("invalid packet recieved");
481  buf[len] = 0;
482  logDebug("RX: %s", buf);
483 }
484 
485 void LMS1xx::startDevice()
486 {
487  char buf[100];
488  sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
489 
490  write(socket_fd_, buf, strlen(buf));
491 
492  int len = read(socket_fd_, buf, 100);
493 
494  if (buf[0] != 0x02)
495  logWarn("invalid packet recieved");
496  buf[len] = 0;
497  logDebug("RX: %s", buf);
498 }
LMSBuffer::getNextBuffer
char * getNextBuffer()
Definition: lms_buffer.h:58
scanData::rssi2
uint16_t rssi2[1082]
Remission values for the second reflected pulse.
Definition: lms_structs.h:203
scanOutputRange::startAngle
int startAngle
Start angle. 1/10000 degree.
Definition: lms_structs.h:139
scanData
Structure containing single scan message.
Definition: lms_structs.h:154
LMS1xx::setScanDataCfg
void setScanDataCfg(const scanDataCfg &cfg)
Set scan data configuration. Set format of scan message returned by device.
Definition: LMS1xx.cpp:197
LMS1xx::getScanCfg
scanCfg getScanCfg() const
Get current scan configuration. Get scan configuration :
Definition: LMS1xx.cpp:164
LMS1xx::queryStatus
status_t queryStatus()
Get current status of LMS1xx device.
Definition: LMS1xx.cpp:114
LMSBuffer::readFrom
void readFrom(int fd)
Definition: lms_buffer.h:42
LMS1xx::getScanOutputRange
scanOutputRange getScanOutputRange() const
Get current output range configuration. Get output range configuration :
Definition: LMS1xx.cpp:211
scanData::dist1
uint16_t dist1[1082]
Radial distance for the first reflected pulse.
Definition: lms_structs.h:167
LMS1xx::login
void login()
Log into LMS1xx unit. Increase privilege level, giving ability to change device configuration.
Definition: LMS1xx.cpp:133
scanDataCfg::resolution
int resolution
Remission resolution. Defines whether the remission values are output with 8-bit or 16bit resolution...
Definition: lms_structs.h:87
scanDataCfg::remission
bool remission
Remission. Defines whether remission values are output.
Definition: lms_structs.h:81
scanDataCfg::outputInterval
int outputInterval
Output interval. Defines which scan is output.
Definition: lms_structs.h:118
scanDataCfg
Structure containing scan data configuration.
Definition: lms_structs.h:68
scanOutputRange::angleResolution
int angleResolution
Scanning resolution. 1/10000 degree.
Definition: lms_structs.h:133
scanOutputRange::stopAngle
int stopAngle
Stop angle. 1/10000 degree.
Definition: lms_structs.h:145
scanDataCfg::encoder
int encoder
Encoders channels. Defines which output channel is activated.
Definition: lms_structs.h:93
scanData::rssi_len2
int rssi_len2
Number of samples in rssi2.
Definition: lms_structs.h:197
LMS1xx::stopMeas
void stopMeas()
Stop measurements. After receiving this command LMS1xx unit stop spinning laser and measuring...
Definition: LMS1xx.cpp:100
scanDataCfg::timestamp
bool timestamp
Definition: lms_structs.h:107
scanCfg::scaningFrequency
int scaningFrequency
Scanning frequency. 1/100 Hz.
Definition: lms_structs.h:41
LMS1xx::setScanCfg
void setScanCfg(const scanCfg &cfg)
Set scan configuration. Get scan configuration :
Definition: LMS1xx.cpp:183
scanCfg
Structure containing scan configuration.
Definition: lms_structs.h:35
LMS1xx::getScanData
bool getScanData(scanData *scan_data)
Receive single scan message.
Definition: LMS1xx.cpp:242
scanData::dist_len2
int dist_len2
Number of samples in dist2.
Definition: lms_structs.h:173
LMS1xx::startDevice
void startDevice()
The device is returned to the measurement mode after configuration.
Definition: LMS1xx.cpp:485
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:226
scanDataCfg::outputChannel
int outputChannel
Output channels. Defines which output channel is activated.
Definition: lms_structs.h:75
scanCfg::angleResolution
int angleResolution
Scanning resolution. 1/10000 degree.
Definition: lms_structs.h:47
scanData::dist_len1
int dist_len1
Number of samples in dist1.
Definition: lms_structs.h:161
LMS1xx::buffer_
LMSBuffer buffer_
Definition: LMS1xx.h:173
LMS1xx::startMeas
void startMeas()
Start measurements. After receiving this command LMS1xx unit starts spinning laser and measuring...
Definition: LMS1xx.cpp:86
LMS1xx::isConnected
bool isConnected()
Get status of connection.
Definition: LMS1xx.cpp:81
LMS1xx::disconnect
void disconnect()
Disconnect from LMS1xx device.
Definition: LMS1xx.cpp:72
LMS1xx::connect
void connect(std::string host, int port=2111)
Connect to LMS1xx.
Definition: LMS1xx.cpp:47
scanCfg::stopAngle
int stopAngle
Stop angle. 1/10000 degree.
Definition: lms_structs.h:59
LMS1xx::socket_fd_
int socket_fd_
Definition: LMS1xx.h:174
scanCfg::startAngle
int startAngle
Start angle. 1/10000 degree.
Definition: lms_structs.h:53
LMS1xx::LMS1xx
LMS1xx()
Definition: LMS1xx.cpp:39
outputRange
Structure containing scan output range configuration.
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:470
scanDataCfg::position
bool position
Position. Defines whether position values are output.
Definition: lms_structs.h:99
LMS1xx::~LMS1xx
virtual ~LMS1xx()
Definition: LMS1xx.cpp:43
status_t
status_t
Definition: LMS1xx.h:32
scanData::rssi1
uint16_t rssi1[1082]
Remission values for the first reflected pulse.
Definition: lms_structs.h:191
LMS1xx::parseScanData
static void parseScanData(char *buf, scanData *data)
Receive single scan message.
Definition: LMS1xx.cpp:284
LMS1xx::connected_
bool connected_
Definition: LMS1xx.h:172
scanData::rssi_len1
int rssi_len1
Number of samples in rssi1.
Definition: lms_structs.h:185
LMSBuffer::popLastBuffer
void popLastBuffer()
Definition: lms_buffer.h:99
scanDataCfg::deviceName
bool deviceName
Device name. Determines whether the device name is to be output.
Definition: lms_structs.h:105
scanData::dist2
uint16_t dist2[1082]
Radial distance for the second reflected pulse.
Definition: lms_structs.h:179

lms1xx
Author(s): Konrad Banachowicz
autogenerated on Wed Jan 22 2020 03:36:59