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