Debian Codegraph @

libs/hwdrivers/src/CLMS100eth.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/CLMS100eth.h>
13 #include <mrpt/system/string_utils.h>
14
15 #include <chrono>
16 #include <cstring>
17 #include <iostream>
18 #include <sstream>
19 #include <thread>
20
21 #define APPERTURE 4.712385 // in radian <=> 270°
22
23 using namespace mrpt;
24 using namespace mrpt::system;
25 using namespace mrpt::hwdrivers;
26 using namespace mrpt::poses;
27 using namespace mrpt::obs;
28 using namespace std;
29
30 IMPLEMENTS_GENERIC_SENSOR(CLMS100Eth, mrpt::hwdrivers)
31
32 CLMS100Eth::CLMS100Eth(string _ip, unsigned int _port) :
33 m_ip(_ip),
34 m_port(_port),
35 m_client(),
36 m_cmd(),
37 m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
38 m_beamApperture(.25 * M_PI / 180.0)
39 {
40 setVerbosityLevel(mrpt::system::LVL_DEBUG);
41 }
42
43 CLMS100Eth::~CLMS100Eth()
44 {
45 if (m_connected) m_client.close();
46 }
47
48 void CLMS100Eth::initialize()
49 {
50 if (!checkIsConnected())
51 {
52 THROW_EXCEPTION(
53 "Can't connect to LMS100 Ethernet Sensor check your configuration "
54 "file.");
55 }
56 turnOn();
57 }
58
59 void CLMS100Eth::loadConfig_sensorSpecific(
60 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
61 {
62 C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
63 float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
64
65 pose_x = configSource.read_float(iniSection, "pose_x", 0, false);
66 pose_y = configSource.read_float(iniSection, "pose_y", 0, false);
67 pose_z = configSource.read_float(iniSection, "pose_z", 0, false);
68 pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, false);
69 pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, false);
70 pose_roll = configSource.read_float(iniSection, "pose_roll", 0, false);
71 m_ip = configSource.read_string(iniSection, "ip_address", "192.168.0.1", false);
72 m_port = configSource.read_int(iniSection, "TCP_port", 2111, false);
73 m_process_rate = configSource.read_int(iniSection, string("process_rate"), 10, false);
74 m_sensorLabel = configSource.read_string(iniSection, "sensorLabel", "SICK", false);
75 m_sensorPose =
76 CPose3D(pose_x, pose_y, pose_z, DEG2RAD(pose_yaw), DEG2RAD(pose_pitch), DEG2RAD(pose_roll));
77 }
78
79 bool CLMS100Eth::checkIsConnected()
80 {
81 if (m_connected)
82 return true;
83 else
84 {
85 try
86 {
87 m_client.connect(m_ip, m_port);
88 }
89 catch (const std::exception& e)
90 {
91 MRPT_LOG_ERROR_FMT("[CLMS100ETH] ERROR TRYING TO OPEN Ethernet DEVICE:\n%s", e.what());
92 return false;
93 }
94 }
95 m_connected = true;
96 return true;
97 }
98
99 bool CLMS100Eth::turnOff()
100 {
101 if (m_client.isConnected()) m_client.close();
102 m_connected = false;
103 m_turnedOn = false;
104 return true;
105 }
106
107 bool CLMS100Eth::turnOn()
108 {
109 /** From the LMS100 datasheet : :
110 * * Login sMN SetAccessMode 03 F4724744F4724744
111 * * Set Scanarea and Resolution
112 * * sMN mLMPsetscancfg
113 * * SWN LMDscandatacfg 01 00 0 1 0 00 00 0 0 0 0 1
114 * * SMN mEEwriteall Je ne le fais pas, car ca écrit en mémoire non
115 * volatile...
116 * * Request scan : sRN LMDscandata OR sEN LMDscandata
117 */
118 if (checkIsConnected())
119 {
120 try
121 {
122 {
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); // 18
128
129 msgIn[read - 1] = 0;
130 MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
131 MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
132
133 if (!read) return false;
134 }
135 {
136 char msg[] = {"sMN mLMPsetscancfg +2500 +1 +2500 -450000 +2250000"};
137 char msgIn[100];
138 sendCommand(msg);
139
140 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
141
142 msgIn[read - 1] = 0;
143 MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
144 MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
145
146 if (!read) return false;
147 }
148 {
149 char msg[] = {"sWN LMDscandatacfg 01 00 0 1 0 00 00 0 0 0 0 +1"};
150 char msgIn[100];
151 sendCommand(msg);
152
153 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
154
155 msgIn[read - 1] = 0;
156 MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
157 MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
158
159 if (!read) return false;
160 }
161 {
162 char msg[] = {"sMN LMCstartmeas"};
163 char msgIn[100];
164 sendCommand(msg);
165 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
166
167 msgIn[read - 1] = 0;
168 MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
169 if (!read) return false;
170 }
171 {
172 char msgIn[100];
173 char msg[] = {"sRN STlms"};
174 do
175 {
176 sendCommand(msg);
177 size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
178 std::this_thread::sleep_for(10000ms);
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 }
188 catch (const std::exception& e)
189 {
190 MRPT_LOG_ERROR_FMT("%s", e.what());
191 return false;
192 }
193 }
194 else
195 {
196 return false;
197 }
198 return true;
199 }
200
201 void CLMS100Eth::sendCommand(const char* cmd)
202 {
203 generateCmd(cmd);
204 if (!m_cmd.empty()) // one never knows...
205 m_client.writeAsync(&m_cmd[0], m_cmd.size());
206 }
207
208 /** Add the start and end character.
209 */
210 void CLMS100Eth::generateCmd(const char* cmd)
211 {
212 if (strlen(cmd) > 995)
213 {
214 MRPT_LOG_ERROR("Error: command is too long.");
215 return;
216 }
217 m_cmd = mrpt::format("%c%s%c", 0x02, cmd, 0x03);
218 }
219
220 bool CLMS100Eth::decodeScan(char* buff, CObservation2DRangeScan& outObservation)
221 {
222 char* next;
223 unsigned int idx = 0;
224 unsigned int scanCount = 0;
225 char* tmp;
226 // double factor;
227
228 next = strtok(buff, " ", &tmp);
229
230 while (next && scanCount == 0)
231 {
232 // cout << "Interpreting : " << next << endl;
233 switch (++idx)
234 {
235 case 1:
236 if (strncmp(&next[1], "sRA", 3) != 0 && strncmp(&next[1], "sSN", 3) != 0) 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 {
248 MRPT_LOG_ERROR_FMT("Contamination error on LMS100: '%s'", next);
249 }
250 else
251 {
252 MRPT_LOG_DEBUG("STATUS Ok.\n");
253 }
254 break;
255 case 21:
256 if (strcmp(next, "DIST1") != 0)
257 {
258 THROW_EXCEPTION("LMS100 is not configured to send distances.");
259 return false;
260 }
261 MRPT_LOG_DEBUG("Distance : OK\n");
262 break;
263 case 22:
264 // factor = strtod(next, nullptr);
265 break;
266 case 26:
267 scanCount = strtoul(next, nullptr, 16);
268 MRPT_LOG_DEBUG_FMT("Scan Count : %d\n", scanCount);
269 break;
270 default:
271 break;
272 }
273 next = strtok(nullptr, " ", &tmp);
274 }
275 outObservation.aperture = (float)APPERTURE;
276 outObservation.rightToLeft = false;
277 outObservation.stdError = 0.012f;
278 outObservation.sensorPose = m_sensorPose;
279 outObservation.beamAperture = m_beamApperture;
280 outObservation.maxRange = m_maxRange;
281 outObservation.timestamp = mrpt::Clock::now();
282 outObservation.sensorLabel = m_sensorLabel;
283
284 outObservation.resizeScan(scanCount);
285 unsigned int i;
286 for (i = 0; i < scanCount && next; i++, next = strtok(nullptr, " ", &tmp))
287 {
288 outObservation.setScanRange(i, double(strtoul(next, nullptr, 16)) / 1000.0);
289 outObservation.setScanRangeValidity(
290 i, outObservation.getScanRange(i) <= outObservation.maxRange);
291 }
292 outObservation.resizeScan(i);
293 return i >= scanCount;
294 }
295
296 void CLMS100Eth::doProcessSimple(
297 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
313 // dans le vent" << endl;
314
315 m_client.readAsync(buffIn, sizeof(buffIn), 40, 40);
316
317 if (decodeScan(buffIn, outObservation))
318 {
319 // Do filter:
320 C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
321 C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
322 // Do show preview:
323 C2DRangeFinderAbstract::processPreview(outObservation);
324
325 outThereIsObservation = true;
326 hardwareError = false;
327 }
328 else
329 {
330 hardwareError = true;
331 outThereIsObservation = false;
332 MRPT_LOG_ERROR("doProcessSimple failed\n");
333 }
334 }
335
336 /*-------------------------------------------------------------*/
337 void CLMS100Eth::doProcess()
338 {
339 CObservation2DRangeScan::Ptr obs = std::make_shared<CObservation2DRangeScan>();
340 try
341 {
342 bool isThereObservation, hwError;
343 doProcessSimple(isThereObservation, *obs, hwError);
344 if (hwError)
345 m_state = ssError;
346 else
347 m_state = ssWorking;
348 // if at least one data have been sensed :
349 if (isThereObservation)
350 {
351 appendObservation(obs);
352 }
353 }
354 catch (...)
355 {
356 m_state = ssError;
357 THROW_EXCEPTION("No observation received from the Phidget board!");
358 }
359 }
360
361 /** A method to set the sensor pose on the robot.
362 */
363 void CLMS100Eth::setSensorPose(const CPose3D& _pose) { m_sensorPose = _pose; }