Debian Codegraph @

libs/hwdrivers/src/CSickLaserUSB.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/CSickLaserUSB.h>
13 #include <mrpt/serialization/CArchive.h>
14 #include <mrpt/system/crc.h>
15 #include <mrpt/system/os.h>
16
17 #include <chrono>
18 #include <thread>
19
20 IMPLEMENTS_GENERIC_SENSOR(CSickLaserUSB, mrpt::hwdrivers)
21
22 using namespace std;
23 using namespace mrpt;
24 using namespace mrpt::comms;
25 using namespace mrpt::obs;
26 using namespace mrpt::hwdrivers;
27 using namespace mrpt::poses;
28
29 /*-------------------------------------------------------------
30 CSickLaserUSB
31 -------------------------------------------------------------*/
32 CSickLaserUSB::CSickLaserUSB() : m_serialNumber("LASER001")
33 {
34 MRPT_START
35 m_sensorLabel = "SICKLMS";
36 m_usbConnection = std::make_unique<CInterfaceFTDI>();
37 MRPT_END
38 }
39
40 /*-------------------------------------------------------------
41 doProcessSimple
42 -------------------------------------------------------------*/
43 void CSickLaserUSB::doProcessSimple(
44 bool& outThereIsObservation,
45 mrpt::obs::CObservation2DRangeScan& outObservation,
46 bool& hardwareError)
47 {
48 outThereIsObservation = false;
49 hardwareError = false;
50
51 if (!checkControllerIsConnected())
52 {
53 hardwareError = true;
54 return;
55 }
56
57 vector<float> ranges;
58 unsigned char LMS_stat;
59 uint32_t board_timestamp;
60 bool is_mm_mode;
61
62 m_state = ssWorking;
63
64 // Wait for a scan:
65 if (!waitContinuousSampleFrame(ranges, LMS_stat, board_timestamp, is_mm_mode)) return;
66
67 // Yes, we have a new scan:
68
69 // -----------------------------------------------
70 // Extract the observation:
71 // -----------------------------------------------
72 uint32_t AtUI = 0;
73 if (m_timeStartUI == 0)
74 {
75 m_timeStartUI = board_timestamp;
76 m_timeStartTT = mrpt::Clock::now();
77 }
78 else
79 AtUI = board_timestamp - m_timeStartUI;
80
81 /* Board time is ms, -50ms aprox delay for scan sending from the scanner*/
82 auto AtDO = std::chrono::milliseconds(AtUI - 50);
83 outObservation.timestamp = m_timeStartTT + AtDO;
84
85 outObservation.sensorLabel = m_sensorLabel; // Set label
86
87 // Extract the timestamp of the sensor:
88
89 // And the scan ranges:
90 outObservation.rightToLeft = true;
91 outObservation.aperture = M_PIf;
92 outObservation.maxRange = is_mm_mode ? 32.7 : 81.0;
93 outObservation.stdError = 0.003f;
94 outObservation.sensorPose = m_sensorPose;
95
96 outObservation.resizeScan(ranges.size());
97
98 for (size_t i = 0; i < ranges.size(); i++)
99 {
100 outObservation.setScanRange(i, ranges[i]);
101 outObservation.setScanRangeValidity(i, (ranges[i] <= outObservation.maxRange));
102 }
103
104 // Do filter:
105 C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
106 C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
107 // Do show preview:
108 C2DRangeFinderAbstract::processPreview(outObservation);
109
110 outThereIsObservation = true;
111 }
112
113 /*-------------------------------------------------------------
114 loadConfig_sensorSpecific
115 -------------------------------------------------------------*/
116 void CSickLaserUSB::loadConfig_sensorSpecific(
117 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
118 {
119 m_serialNumber = configSource.read_string(iniSection, "SICKUSB_serialNumber", m_serialNumber);
120 m_sensorPose = CPose3D(
121 configSource.read_float(iniSection, "pose_x", 0),
122 configSource.read_float(iniSection, "pose_y", 0),
123 configSource.read_float(iniSection, "pose_z", 0),
124 DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
125 DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
126 DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
127 // Parent options:
128 C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
129 }
130
131 /*-------------------------------------------------------------
132 turnOn
133 -------------------------------------------------------------*/
134 bool CSickLaserUSB::turnOn() { return true; }
135 /*-------------------------------------------------------------
136 turnOff
137 -------------------------------------------------------------*/
138 bool CSickLaserUSB::turnOff() { return true; }
139 /*-------------------------------------------------------------
140 checkControllerIsConnected
141 -------------------------------------------------------------*/
142 bool CSickLaserUSB::checkControllerIsConnected()
143 {
144 // If device is already open, thats ok:
145 if (m_usbConnection->isOpen()) return true;
146
147 // If it isn't, try to open it now:
148 try
149 {
150 m_usbConnection->OpenBySerialNumber(m_serialNumber);
151 m_usbConnection->ResetDevice();
152 std::this_thread::sleep_for(10ms);
153 m_usbConnection->SetTimeouts(10, 20); // read, write, in milliseconds
154 std::this_thread::sleep_for(10ms);
155 m_usbConnection->SetLatencyTimer(1); // 1ms, the minimum
156 std::this_thread::sleep_for(10ms);
157
158 MRPT_LOG_INFO_FMT(
159 "[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n", m_serialNumber.c_str());
160 return true;
161 }
162 catch (const std::exception& e)
163 {
164 MRPT_LOG_ERROR_FMT(
165 "[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s", m_serialNumber.c_str(),
166 e.what());
167 return false;
168 }
169 }
170
171 /*-------------------------------------------------------------
172 waitContinuousSampleFrame
173 -------------------------------------------------------------*/
174 bool CSickLaserUSB::waitContinuousSampleFrame(
175 vector<float>& out_ranges_meters,
176 unsigned char& LMS_status,
177 uint32_t& out_board_timestamp,
178 bool& is_mm_mode)
179 {
180 size_t nRead, nBytesToRead;
181 size_t nFrameBytes = 0;
182 size_t lenghtField;
183 unsigned char buf[2000];
184 buf[2] = buf[3] = 0;
185
186 while (nFrameBytes <
187 (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) + 5 /* for 32bit timestamp + end-flag */)
188 {
189 if (lenghtField > 800)
190 {
191 cout << "#";
192 nFrameBytes = 0; // No es cabecera de trama correcta
193 buf[2] = buf[3] = 0;
194 }
195
196 if (nFrameBytes < 4)
197 nBytesToRead = 1;
198 else
199 nBytesToRead = (5 /* for 32bit timestamp + end-flag */ + lenghtField) - nFrameBytes;
200
201 try
202 {
203 nRead = m_usbConnection->ReadSync(buf + nFrameBytes, nBytesToRead);
204 }
205 catch (const std::exception& e)
206 {
207 // Disconnected?
208 MRPT_LOG_ERROR_FMT(
209 "[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due "
210 "to comms error: %s\n",
211 e.what());
212 m_usbConnection->Close();
213 m_timeStartUI = 0;
214 return false;
215 }
216
217 if (nRead == 0 && nFrameBytes == 0) return false;
218
219 if (nRead > 0)
220 {
221 // Lectura OK:
222 // Era la primera?
223 if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) ||
224 (nFrameBytes == 1 && buf[1] == 0x80))
225 nFrameBytes += nRead;
226 else
227 {
228 nFrameBytes = 0; // No es cabecera de trama correcta
229 buf[2] = buf[3] = 0;
230 }
231 }
232 }
233
234 // Frame received
235 // --------------------------------------------------------------------------
236 // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2
237 // |
238 // --------------------------------------------------------------------------
239
240 // Trama completa:
241 // Checkear que el byte de comando es 0xB0:
242 if (buf[4] != 0xB0) return false;
243
244 // GET FRAME INFO
245 int info = buf[5] | (buf[6] << 8); // Little Endian
246 int n_points = info & 0x01FF;
247 is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm
248
249 out_ranges_meters.resize(n_points);
250
251 // Copiar rangos:
252 short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
253 float meters_scale = is_mm_mode ? 0.001f : 0.01f;
254
255 for (int i = 0; i < n_points; i++)
256 out_ranges_meters[i] = ((buf[7 + i * 2] | (buf[8 + i * 2] << 8)) & mask) * meters_scale;
257
258 // Status
259 LMS_status = buf[lenghtField - 3];
260
261 // End frame:
262 if (buf[nFrameBytes - 1] != 0x55)
263 {
264 MRPT_LOG_ERROR("[CSickLaserUSB::waitContinuousSampleFrame] bad end flag");
265 return false; // Bad CRC
266 }
267
268 // CRC:
269 const uint16_t CRC = mrpt::system::compute_CRC16(buf, lenghtField - 2);
270 const uint16_t CRC_packet = buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
271 if (CRC_packet != CRC)
272 {
273 MRPT_LOG_ERROR_FMT(
274 "[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u "
275 "nptns=%u: %i != %i\n",
276 unsigned(lenghtField), unsigned(n_points), CRC_packet, CRC);
277 return false; // Bad CRC
278 }
279
280 // Get USB board timestamp:
281 out_board_timestamp = (uint32_t(buf[nFrameBytes - 5]) << 24) |
282 (uint32_t(buf[nFrameBytes - 4]) << 16) |
283 (uint32_t(buf[nFrameBytes - 3]) << 8) | uint32_t(buf[nFrameBytes - 2]);
284
285 // All OK
286 return true;
287 }