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