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