MRPT  1.9.9
CLMS100eth.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
14 
15 #include <cstring>
16 #include <iostream>
17 #include <sstream>
18 
19 #define APPERTURE 4.712385 // in radian <=> 270°
20 
21 using namespace mrpt;
22 using namespace mrpt::system;
23 using namespace mrpt::hwdrivers;
24 using namespace mrpt::poses;
25 using namespace mrpt::obs;
26 using namespace std;
27 
29 
30 CLMS100Eth::CLMS100Eth(string _ip, unsigned int _port)
31  : m_ip(_ip),
32  m_port(_port),
33  m_client(),
34  m_cmd(),
35  m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
36  m_beamApperture(.25 * M_PI / 180.0)
37 {
38  setVerbosityLevel(mrpt::system::LVL_DEBUG);
39 }
40 
41 CLMS100Eth::~CLMS100Eth()
42 {
43  if (m_connected) m_client.close();
44  // delete m_client;
45  // delete m_sensorPose;
46 }
47 
48 void CLMS100Eth::initialize()
49 {
50  if (!checkIsConnected())
51  {
53  "Can't connect to LMS100 Ethernet Sensor check your configuration "
54  "file.");
55  }
56  turnOn();
57 }
58 
59 void CLMS100Eth::loadConfig_sensorSpecific(
60  const mrpt::config::CConfigFileBase& configSource,
61  const std::string& iniSection)
62 {
63  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
64  float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
65 
66  pose_x = configSource.read_float(iniSection, "pose_x", 0, false);
67  pose_y = configSource.read_float(iniSection, "pose_y", 0, false);
68  pose_z = configSource.read_float(iniSection, "pose_z", 0, false);
69  pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, false);
70  pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, false);
71  pose_roll = configSource.read_float(iniSection, "pose_roll", 0, false);
72  m_ip = configSource.read_string(
73  iniSection, "ip_address", "192.168.0.1", false);
74  m_port = configSource.read_int(iniSection, "TCP_port", 2111, false);
75  m_process_rate =
76  configSource.read_int(iniSection, string("process_rate"), 10, false);
77  m_sensorLabel =
78  configSource.read_string(iniSection, "sensorLabel", "SICK", false);
79  m_sensorPose = CPose3D(
80  pose_x, pose_y, pose_z, DEG2RAD(pose_yaw), DEG2RAD(pose_pitch),
81  DEG2RAD(pose_roll));
82 }
83 
84 bool CLMS100Eth::checkIsConnected()
85 {
86  if (m_connected)
87  return true;
88  else
89  {
90  try
91  {
92  m_client.connect(m_ip, m_port);
93  }
94  catch (const std::exception& e)
95  {
97  "[CLMS100ETH] ERROR TRYING TO OPEN Ethernet DEVICE:\n%s",
98  e.what());
99  return false;
100  }
101  }
102  m_connected = true;
103  return true;
104 }
105 
106 bool CLMS100Eth::turnOff()
107 {
108  if (m_client.isConnected()) m_client.close();
109  m_connected = false;
110  m_turnedOn = false;
111  return true;
112 }
113 
114 bool CLMS100Eth::turnOn()
115 {
116  /** From the LMS100 datasheet : :
117  * * Login sMN SetAccessMode 03 F4724744F4724744
118  * * Set Scanarea and Resolution
119  * * sMN mLMPsetscancfg
120  * * SWN LMDscandatacfg 01 00 0 1 0 00 00 0 0 0 0 1
121  * * SMN mEEwriteall Je ne le fais pas, car ca écrit en mémoire non
122  * volatile...
123  * * Request scan : sRN LMDscandata OR sEN LMDscandata
124  */
125  if (checkIsConnected())
126  {
127  try
128  {
129  {
130  char msg[] = {"sMN SetAccessMode 03 F4724744"};
131  char msgIn[100];
132  sendCommand(msg);
133 
134  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000); // 18
135 
136  msgIn[read - 1] = 0;
137  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
138  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
139 
140  if (!read) return false;
141  }
142  {
143  char msg[] = {
144  "sMN mLMPsetscancfg +2500 +1 +2500 -450000 +2250000"};
145  char msgIn[100];
146  sendCommand(msg);
147 
148  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
149 
150  msgIn[read - 1] = 0;
151  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
152  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
153 
154  if (!read) return false;
155  }
156  {
157  char msg[] = {
158  "sWN LMDscandatacfg 01 00 0 1 0 00 00 0 0 0 0 +1"};
159  char msgIn[100];
160  sendCommand(msg);
161 
162  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
163 
164  msgIn[read - 1] = 0;
165  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
166  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
167 
168  if (!read) return false;
169  }
170  {
171  char msg[] = {"sMN LMCstartmeas"};
172  char msgIn[100];
173  sendCommand(msg);
174  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
175 
176  msgIn[read - 1] = 0;
177  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
178  if (!read) return false;
179  }
180  {
181  char msgIn[100];
182  char msg[] = {"sRN STlms"};
183  do
184  {
185  sendCommand(msg);
186  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
187  std::this_thread::sleep_for(10000ms);
188 
189  msgIn[read - 1] = 0;
190  MRPT_LOG_DEBUG_FMT("message : %s\n", &msgIn[1]);
191  MRPT_LOG_DEBUG_FMT("%c\n", msgIn[11]);
192  if (!read) return false;
193  } while (msgIn[11] != '7');
194  }
195  m_turnedOn = true;
196  }
197  catch (const std::exception& e)
198  {
199  MRPT_LOG_ERROR_FMT("%s", e.what());
200  return false;
201  }
202  }
203  else
204  {
205  return false;
206  }
207  return true;
208 }
209 
210 void CLMS100Eth::sendCommand(const char* cmd)
211 {
212  generateCmd(cmd);
213  if (!m_cmd.empty()) // one never knows...
214  m_client.writeAsync(&m_cmd[0], m_cmd.size());
215 }
216 
217 /** Add the start and end character.
218  */
219 void CLMS100Eth::generateCmd(const char* cmd)
220 {
221  if (strlen(cmd) > 995)
222  {
223  MRPT_LOG_ERROR("Error: command is too long.");
224  return;
225  }
226  m_cmd = format("%c%s%c", 0x02, cmd, 0x03);
227 }
228 
229 bool CLMS100Eth::decodeScan(char* buff, CObservation2DRangeScan& outObservation)
230 {
231  char* next;
232  unsigned int idx = 0;
233  unsigned int scanCount = 0;
234  char* tmp;
235  // double factor;
236 
237  next = strtok(buff, " ", &tmp);
238 
239  while (next && scanCount == 0)
240  {
241  // cout << "Interpreting : " << next << endl;
242  switch (++idx)
243  {
244  case 1:
245  if (strncmp(&next[1], "sRA", 3) && strncmp(&next[1], "sSN", 3))
246  return false;
247  break;
248  case 2:
249  if (strcmp(next, "LMDscandata")) return false;
250  break;
251  case 6:
252  if (!strcmp(next, "1"))
253  {
254  MRPT_LOG_ERROR_FMT("STATUS error on LMS100: '%s'", next);
255  }
256  else if (!strcmp(next, "4"))
257  {
259  "Contamination error on LMS100: '%s'", next);
260  }
261  else
262  {
263  MRPT_LOG_DEBUG("STATUS Ok.\n");
264  }
265  break;
266  case 21:
267  if (strcmp(next, "DIST1"))
268  {
270  "LMS100 is not configured to send distances.");
271  return false;
272  }
273  MRPT_LOG_DEBUG("Distance : OK\n");
274  break;
275  case 22:
276  // factor = strtod(next, nullptr);
277  break;
278  case 26:
279  scanCount = strtoul(next, nullptr, 16);
280  MRPT_LOG_DEBUG_FMT("Scan Count : %d\n", scanCount);
281  break;
282  default:
283  break;
284  }
285  next = strtok(nullptr, " ", &tmp);
286  }
287  outObservation.aperture = (float)APPERTURE;
288  outObservation.rightToLeft = false;
289  outObservation.stdError = 0.012f;
290  outObservation.sensorPose = m_sensorPose;
291  outObservation.beamAperture = m_beamApperture;
292  outObservation.maxRange = m_maxRange;
293  outObservation.timestamp = mrpt::system::getCurrentTime();
294  outObservation.sensorLabel = m_sensorLabel;
295 
296  outObservation.resizeScan(scanCount);
297  unsigned int i;
298  for (i = 0; i < scanCount && next; i++, next = strtok(nullptr, " ", &tmp))
299  {
300  outObservation.setScanRange(
301  i, double(strtoul(next, nullptr, 16)) / 1000.0);
302  outObservation.setScanRangeValidity(
303  i, outObservation.getScanRange(i) <= outObservation.maxRange);
304  }
305  outObservation.resizeScan(i);
306  return i >= scanCount;
307 }
308 
309 void CLMS100Eth::doProcessSimple(
310  bool& outThereIsObservation, CObservation2DRangeScan& outObservation,
311  bool& hardwareError)
312 {
313  if (!m_turnedOn)
314  {
315  hardwareError = true;
316  outThereIsObservation = false;
317  return;
318  }
319  hardwareError = false;
320 
321  char msg[] = {"sRN LMDscandata"};
322  sendCommand(msg);
323  char buffIn[16 * 1024];
324  // size_t read = m_client.readAsync(buffIn, sizeof(buffIn), 100, 100);
325  // cout << "read :" << read << endl;
326  // while(m_client.readAsync(buffIn, sizeof(buffIn), 100, 100)) cout << "Lit
327  // dans le vent" << endl;
328 
329  m_client.readAsync(buffIn, sizeof(buffIn), 40, 40);
330 
331  if (decodeScan(buffIn, outObservation))
332  {
333  // Do filter:
334  C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
335  C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
336  // Do show preview:
337  C2DRangeFinderAbstract::processPreview(outObservation);
338 
339  outThereIsObservation = true;
340  hardwareError = false;
341  }
342  else
343  {
344  hardwareError = true;
345  outThereIsObservation = false;
346  MRPT_LOG_ERROR("doProcessSimple failed\n");
347  }
348 }
349 
350 /*-------------------------------------------------------------*/
351 void CLMS100Eth::doProcess()
352 {
354  std::make_shared<CObservation2DRangeScan>();
355  try
356  {
357  bool isThereObservation, hwError;
358  doProcessSimple(isThereObservation, *obs, hwError);
359  if (hwError)
360  m_state = ssError;
361  else
362  m_state = ssWorking;
363  // if at least one data have been sensed :
364  if (isThereObservation)
365  {
366  appendObservation(obs);
367  }
368  }
369  catch (...)
370  {
371  m_state = ssError;
372  THROW_EXCEPTION("No observation received from the Phidget board!");
373  }
374 }
375 
376 /** A method to set the sensor pose on the robot.
377  */
378 void CLMS100Eth::setSensorPose(const CPose3D& _pose) { m_sensorPose = _pose; }
This "software driver" implements the communication protocol for interfacing a SICK LMS100 laser scan...
Definition: CLMS100eth.h:72
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
float beamAperture
The aperture of each beam, in radians, used to insert "thick" rays in the occupancy grid...
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
void setScanRange(const size_t i, const float val)
double DEG2RAD(const double x)
Degrees to radians.
Contains classes for various device interfaces.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
float maxRange
The maximum range allowed by the device, in meters (e.g.
This class allows loading and storing values and vectors of different types from a configuration text...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
char * strtok(char *str, const char *strDelimit, char **context) noexcept
An OS-independent method for tokenizing a string.
This namespace contains representation of robot actions and observations.
GLsizei const GLchar ** string
Definition: glext.h:4116
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
#define APPERTURE
Definition: CLMS100eth.cpp:19
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
#define MRPT_LOG_ERROR(_STRING)
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8a2d96f36 Mon Jul 15 00:01:58 2019 +0200 at lun jul 15 00:10:13 CEST 2019