MRPT  1.9.9
CSICKTim561Eth_2050101.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
11 #include "hwdrivers-precomp.h" // Precompiled headers
12 
15 
16 #include <cstring>
17 #include <iostream>
18 #include <sstream>
19 
20 #define APPERTURE 4.712385 // in radian <=> 270
21 
22 using namespace mrpt;
23 using namespace mrpt::system;
24 using namespace mrpt::hwdrivers;
25 using namespace mrpt::poses;
26 using namespace mrpt::obs;
27 using namespace std;
28 
29 // CODE
31 
32 /** Default Lidar IP: 192.168.0.1
33  * Default IP_port: 2111
34  * Maximum Range: 10 Meters
35  * Default SensorPose: Depend on robot(0, 270, 105)mm, orientation(0, 21.25, 0)
36  */
37 CSICKTim561Eth::CSICKTim561Eth(string _ip, unsigned int _port)
38  : m_ip(_ip),
39  m_port(_port),
40  m_client(),
41 
42  m_cmd(),
43 
44  m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
45 
46  m_beamApperture(.25 * M_PI / 180.0)
47 {
48  setVerbosityLevel(mrpt::system::LVL_DEBUG);
49 }
50 
51 CSICKTim561Eth::~CSICKTim561Eth()
52 {
53  if (m_connected) m_client.close();
54  // delete m_client;
55  // delete m_sensorPose;
56 }
57 
58 void CSICKTim561Eth::initialize()
59 {
60  if (!checkIsConnected())
61  {
63  "Cannot connect to SICK Tim561 Ethernet Sensor check your "
64  "configuration"
65  "file.");
66  }
67  turnOn();
68 }
69 
70 void CSICKTim561Eth::loadConfig_sensorSpecific(
71  const mrpt::config::CConfigFileBase& configSource,
72  const std::string& iniSection)
73 {
74  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
75  float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
76 
77  pose_x = configSource.read_float(iniSection, "pose_x", 0, false);
78  pose_y = configSource.read_float(iniSection, "pose_y", 0, false);
79  pose_z = configSource.read_float(iniSection, "pose_z", 0, false);
80  pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, false);
81  pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, false);
82  pose_roll = configSource.read_float(iniSection, "pose_roll", 0, false);
83  m_ip = configSource.read_string(
84  iniSection, "ip_address", "192.168.0.1", false);
85  m_port = configSource.read_int(iniSection, "TCP_port", 2111, false);
86  m_process_rate =
87  configSource.read_int(iniSection, string("process_rate"), 15, false);
88  m_sensorLabel =
89  configSource.read_string(iniSection, "sensorLabel", "SICK", false);
90  m_sensorPose = CPose3D(
91  pose_x, pose_y, pose_z, DEG2RAD(pose_yaw), DEG2RAD(pose_pitch),
92  DEG2RAD(pose_roll));
93 }
94 
95 bool CSICKTim561Eth::checkIsConnected()
96 {
97  if (m_connected)
98  {
99  return true;
100  }
101  else
102  {
103  try
104  {
105  m_client.connect(m_ip, m_port);
106  }
107  catch (const std::exception& e)
108  {
110  "[SICKTIM561ETH] ERROR TRYING TO OPEN Ethernet DEVICE:\n%s",
111  e.what());
112  return false;
113  }
114  }
115  m_connected = true;
116  return true;
117 }
118 
119 bool CSICKTim561Eth::rebootDev()
120 {
121  {
122  // Set Maintenance access mode to allow reboot to be sent
123  char msg[] = {"sMN SetAccessMode 03 F4724744"};
124  char msgIn[100];
125  sendCommand(msg);
126 
127  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
128  msgIn[read - 1] = 0;
129  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
130  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
131 
132  if (!read)
133  {
135  "SOPAS - Error setting access mode, unexpected response");
136  return false;
137  }
138  }
139  {
140  // Send reboot command
141  char msg[] = {"sMN mSCreboot"};
142  char msgIn[100];
143  sendCommand(msg);
144 
145  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
146  msgIn[read - 1] = 0;
147  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
148  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
149 
150  if (!read)
151  {
153  "SOPAS - Error rebootting scanner, unexpected response.");
154  return false;
155  }
156  }
157  return true;
158 }
159 
160 bool CSICKTim561Eth::turnOff()
161 {
162  if (m_client.isConnected()) m_client.close();
163  m_connected = false;
164  m_turnedOn = false;
165  return true;
166 }
167 
168 bool CSICKTim561Eth::turnOn()
169 {
170  /** From the SICK TIM561 datasheet:
171  * * 1. Login: "sMN SetAccessMode 03 F4724744"
172  * * -wait answer: "sAN SetAccessMode 1"
173  * * 2. Set Freq and Resolution: "sMN mLMPsetscancfg +1500 0 1800000"
174  * * -wait answer(0-180): "sAN mLMPsetscancfg 0 +1500 0 1800000"
175  * * 3. Start measurement: "sMN LMCstartmeas"
176  * * -wait answer(0/1 yes/no): "sAN LMCstartmeas 0"
177  * * 4. Stop measurement: "sMN LMCstopmeas"
178  * * -wait answer(0/1 yes/no): "sAN LMCstopmeas 0"
179  *
180  *
181  * * z. Configure scandata context: "sWN LMDscandatacfg"
182  * * -wait answer
183  *
184  * * x. Read Freq and Angular:
185  * * -Get params: "sRN LMPscancfg"
186  * * -Read params(Freq, Sect, Resolution): "sRA LMPscancfg 1500 0
187  * 1800000"
188  */
189  if (checkIsConnected())
190  {
191  try
192  {
193  /** Init scanner
194  */
195  {
196  // Read 'DeviceIdent'
197  char msg[] = {"sRIO"};
198  char msgIn[100];
199  sendCommand(msg);
200 
201  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
202  msgIn[read - 1] = 0;
203  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
204  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
205 
206  if (!read)
207  {
209  "SOPAS - Error reading variable 'DeviceIdent'.");
210  return false;
211  }
212  }
213 
214  {
215  // Read 'SerialNumber'
216  char msg[] = {"sRN SerialNumber"};
217  char msgIn[100];
218  sendCommand(msg);
219 
220  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
221  msgIn[read - 1] = 0;
222  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
223  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
224 
225  if (!read)
226  {
228  "SOPAS - Error reading variable 'SerialNumber'.");
229  return false;
230  }
231  }
232 
233  {
234  // Read 'FirmwareVersion'
235  char msg[] = {"sRN FirmwareVersion"};
236  char msgIn[100];
237  sendCommand(msg);
238 
239  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
240  msgIn[read - 1] = 0;
241  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
242  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
243 
244  if (!read)
245  {
247  "SOPAS - Error reading variable 'FirmwareVersion'.");
248  return false;
249  }
250  }
251 
252  {
253  // Read 'Device state'
254  char msg[] = {"sRN SCdevicestate"};
255  char msgIn[100];
256  sendCommand(msg);
257 
258  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
259  msgIn[read - 1] = 0;
260  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
261  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
262 
263  if (!read)
264  {
266  "SOPAS - Error reading variable 'devicestate'.");
267  return false;
268  }
269  }
270 
271  /** End Init Scanner
272  */
273  // {
274  // /** Login TIM561
275  // */
276  // char msg[] = {"sMN SetAccessMode 03 F4724744"};
277  // char msgIn[100];
278  // sendCommand(msg);
279 
280  // size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
281  // msgIn[read - 1] = 0;
282  // MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
283  // MRPT_LOG_DEBUG_FMT("message : %s\n",
284  // string(&msgIn[1]).c_str());
285 
286  // if (!read )
287  // {
288  // return false;
289  // }
290  // }
291  // while(true);
292  /** Set Freq and Resolution
293  */
294  // {
295  // char msg[] = {"sMN mLMPsetscancfg +1500 +1 0 1800000"};
296  // char msgIn[100];
297  // sendCommand(msg);
298 
299  // size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
300  // msgIn[read - 1] = 0;
301  // MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
302  // MRPT_LOG_DEBUG_FMT("message : %s\n",
303  // string(&msgIn[1]).c_str());
304 
305  // if (!read)
306  // {
307  // return false;
308  // }
309  // }
310 
311  /** Set scandatacfg
312  */
313  // {
314  // char msg[] = {"sWN LMDscandatacfg 01 00 0 1 0 00 00 0 0 0 0
315  // +1"}; char msgIn[100]; sendCommand(msg);
316 
317  // size_t read =m_client.readAsync(msgIn, 100, 1000, 1000);
318  // msgIn[read - 1] = 0;
319  // MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
320  // MRPT_LOG_DEBUG_FMT("message : %s\n",
321  // string(&msgIn[1]).c_str()); if (!read){
322  // return false;
323  // }
324  // }
325 
326  /** Set send data permanently
327  */
328  {
329  char msg[] = {"sEN LMDscandata 1"};
330  char msgIn[100];
331  sendCommand(msg);
332  size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
333  msgIn[read - 1] = 0;
334  MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
335  MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
336  if (!read)
337  {
338  MRPT_LOG_DEBUG("No LMSDATA");
339  return false;
340  }
341  }
342 
343  // {
344  // /** Start measurement
345  // */
346  // char msg[] = {"sMN LMCstartmeas"};
347  // char msgIn[100];
348  // sendCommand(msg);
349  // size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
350  // msgIn[read - 1] = 0;
351  // MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
352  // MRPT_LOG_DEBUG_FMT("message : %s\n",
353  // string(&msgIn[1]).c_str()); if (!read)
354  // {
355  // return false;
356  // }
357  // }
358  m_turnedOn = true;
359  }
360  catch (const std::exception& e)
361  {
362  MRPT_LOG_ERROR_FMT("%s", e.what());
363  return false;
364  }
365  }
366  else
367  {
368  return false;
369  }
370  return true;
371 }
372 
373 void CSICKTim561Eth::sendCommand(const char* cmd)
374 {
375  generateCmd(cmd);
376  if (!m_cmd.empty())
377  {
378  m_client.writeAsync(&m_cmd[0], m_cmd.size());
379  }
380 }
381 
382 void CSICKTim561Eth::generateCmd(const char* cmd)
383 {
384  if (strlen(cmd) > 995)
385  {
386  MRPT_LOG_ERROR("Error: command is too long.");
387  return;
388  }
389  // m_cmd = format("%c%s%c", 0x02, cmd, 0x03);
390  m_cmd = format("%c%s%c%c", 0x02, cmd, 0x03, 0);
391 }
392 
393 bool CSICKTim561Eth::decodeScan(
394  char* buff, CObservation2DRangeScan& outObservation)
395 {
396  char* next;
397  unsigned int idx = 0;
398  unsigned int scanCount = 0;
399  char* tmp;
400 
401  next = strtok(buff, " ", &tmp);
402 
403  while (next && scanCount == 0)
404  {
405  switch (++idx)
406  {
407  case 1:
408  // If no "sRA" and also no "sSN", return false reading
409  if (strncmp(&next[1], "sRA", 3) && strncmp(&next[1], "sSN", 3))
410  {
411  return false;
412  }
413  break;
414  case 2:
415  if (strcmp(next, "LMDscandata"))
416  {
417  return false;
418  }
419  break;
420  case 6:
421  if (strcmp(next, "1"))
422  {
423  MRPT_LOG_DEBUG("Laser is ready");
424  }
425  else if (strcmp(next, "0"))
426  {
427  MRPT_LOG_DEBUG("Laser is busy");
428  }
429  else
430  {
431  MRPT_LOG_DEBUG("Laser reports error");
432  rebootDev();
433  }
434  break;
435  case 21:
436  if (strcmp(next, "DIST1"))
437  {
439  "TIM561 is not configured to send distances");
440  return false;
441  }
442  MRPT_LOG_DEBUG("Distance : OK\n");
443  break;
444  case 26:
445  scanCount = strtoul(next, nullptr, 16);
446  MRPT_LOG_DEBUG_FMT("Scan Count : %d\n", scanCount);
447  break;
448  default:
449  break;
450  }
451  next = strtok(nullptr, " ", &tmp);
452  }
453  outObservation.aperture = (float)APPERTURE;
454  outObservation.rightToLeft = false;
455  outObservation.stdError = 0.012f;
456  outObservation.sensorPose = m_sensorPose;
457  outObservation.beamAperture = m_beamApperture;
458  outObservation.maxRange = m_maxRange;
459  outObservation.timestamp = mrpt::system::getCurrentTime();
460  outObservation.sensorLabel = m_sensorLabel;
461 
462  outObservation.resizeScan(scanCount);
463  unsigned int i;
464  for (i = 0; i < scanCount && next; i++, next = strtok(nullptr, " ", &tmp))
465  {
466  outObservation.setScanRange(
467  i, double(strtoul(next, nullptr, 16)) / 1000.0);
468  outObservation.setScanRangeValidity(
469  i, outObservation.getScanRange(i) <= outObservation.maxRange);
470  }
471  outObservation.resizeScan(i);
472  return i >= scanCount;
473 }
474 
475 void CSICKTim561Eth::doProcessSimple(
476  bool& outThereIsObservation, CObservation2DRangeScan& outObservation,
477  bool& hardwareError)
478 {
479  if (!m_turnedOn)
480  {
481  hardwareError = true;
482  outThereIsObservation = false;
483  return;
484  }
485  hardwareError = false;
486 
487  char msg[] = {"sRN LMDscandata"};
488  sendCommand(msg);
489  char buffIn[16 * 1024];
490 
491  m_client.readAsync(buffIn, sizeof(buffIn), 40, 40);
492 
493  if (decodeScan(buffIn, outObservation))
494  {
495  // Filter:
496  C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
497  C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
498  // Do show preview:
499  C2DRangeFinderAbstract::processPreview(outObservation);
500  MRPT_LOG_DEBUG("doProcessSimple Show");
501 
502  outThereIsObservation = true;
503  hardwareError = false;
504  }
505  else
506  {
507  hardwareError = true;
508  outThereIsObservation = false;
509  MRPT_LOG_ERROR("doProcessSimple failed\n");
510  }
511 }
512 
513 void CSICKTim561Eth::doProcess()
514 {
516  std::make_shared<CObservation2DRangeScan>();
517  try
518  {
519  bool isThereObservation, hwError;
520  doProcessSimple(isThereObservation, *obs, hwError);
521  if (hwError)
522  {
523  m_state = ssError;
524  MRPT_LOG_DEBUG("state Error");
525  }
526  else
527  {
528  m_state = ssWorking;
529  MRPT_LOG_DEBUG("state working");
530  }
531  // if at least one data have been sensed:
532  if (isThereObservation)
533  {
534  appendObservation(obs);
535  }
536  }
537  catch (...)
538  {
539  m_state = ssError;
540  THROW_EXCEPTION("No observation received from the Phidget board!");
541  }
542 }
543 
544 /** A method to set the sensor pose on the robot.
545  */
546 void CSICKTim561Eth::setSensorPose(const CPose3D& _pose)
547 {
548  m_sensorPose = _pose;
549 }
#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.
#define APPERTURE
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...
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