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