Debian Codegraph @

libs/hwdrivers/src/CSICKTim561Eth_2050101.cpp

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