Debian Codegraph @

libs/hwdrivers/src/CIbeoLuxETH.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/CIbeoLuxETH.h> // Precompiled headers
13
14 #include <bitset>
15 #include <chrono>
16 #include <thread>
17
18 #define APPERTURE 4.712385 // in radian <=> 270°
19
20 using namespace mrpt;
21 using namespace mrpt::system;
22 using namespace mrpt::poses;
23 using namespace mrpt::hwdrivers;
24 using namespace mrpt::obs;
25 using namespace std;
26
27 // TODO: Use enum instead
28 const unsigned char SearchForAF = 0;
29 const unsigned char SearchForFE = 1;
30 const unsigned char SearchForC0 = 2;
31 const unsigned char SearchForC2 = 3;
32 const unsigned char PacketFound = 4;
33 const unsigned char SaveData = 5;
34
35 IMPLEMENTS_GENERIC_SENSOR(CIbeoLuxETH, mrpt::hwdrivers)
36
37 CIbeoLuxETH::CIbeoLuxETH(string _ip, unsigned int _port) :
38 m_ip(_ip),
39 m_port(_port),
40 m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
41 m_beamApperture(.25 * M_PI / 180.0)
42 {
43 }
44
45 CIbeoLuxETH::~CIbeoLuxETH()
46 {
47 m_run = false;
48 dataCollectionThread.join();
49 // Wait a little for the thread to come down
50 // Don't ask why, it just works
51 // TODO: Try without the delay
52 std::this_thread::sleep_for(10ms);
53 }
54
55 void CIbeoLuxETH::dataCollection()
56 {
57 unsigned char state = SearchForAF;
58 unsigned char msgIn[1], Header[20], ScanListHeader[44], ScanPointData[10];
59 unsigned int datatype, /*scannumber,*/ numScanpoints, angleTicks, SPlayer,
60 SPdistance; // SPecho;
61 int SPHangle;
62 unsigned char msg[32];
63
64 // Start TCP-connection to laserscanner
65 m_client.connect(m_ip, m_port);
66
67 // Send filter command
68 makeCommandHeader(msg);
69 makeTypeCommand(msg);
70 m_client.writeAsync(&msg[0], 32);
71
72 // Send start command
73 makeCommandHeader(msg);
74 makeStartCommand(msg);
75 m_client.writeAsync(&msg[0], 28);
76
77 while (m_run)
78 {
79 switch (state)
80 {
81 case SearchForAF:
82 m_client.readAsync(msgIn, 1, 100, 10);
83 if (msgIn[0] == 0xAF) state = SearchForFE;
84 break;
85 case SearchForFE:
86 m_client.readAsync(msgIn, 1, 100, 10);
87 if (msgIn[0] == 0xFE)
88 state = SearchForC0;
89 else
90 state = SearchForAF;
91 break;
92 case SearchForC0:
93 m_client.readAsync(msgIn, 1, 100, 10);
94 if (msgIn[0] == 0xC0)
95 state = SearchForC2;
96 else
97 state = SearchForAF;
98 break;
99 case SearchForC2:
100 m_client.readAsync(msgIn, 1, 100, 10);
101 if (msgIn[0] == 0xC2)
102 state = PacketFound;
103 else
104 state = SearchForAF;
105 break;
106 case PacketFound:
107 m_client.readAsync(Header, 20, 100, 10);
108 datatype = Header[10] * 0x100 + Header[11];
109 switch (datatype)
110 {
111 case 0x2030:
112 // do nothing
113 state = SearchForAF;
114 break;
115 case 0x2221:
116 // do nothing
117 state = SearchForAF;
118 break;
119 case 0x2805:
120 // do nothing
121 state = SearchForAF;
122 break;
123 case 0x2020:
124 // do nothing
125 state = SearchForAF;
126 break;
127 case 0x2202:
128 state = SaveData;
129 break;
130 default:
131 std::cerr << "UNKNOWN packet of type " << hex << datatype << " received!!\n";
132 state = SearchForAF;
133 }
134 break;
135 case SaveData:
136 // Create new observation object pointer
137 CObservation3DRangeScan::Ptr newObs = std::make_shared<CObservation3DRangeScan>();
138 newObs->hasPoints3D = true;
139 newObs->maxRange = 200.00;
140
141 m_client.readAsync(ScanListHeader, 44, 10, 10);
142 /*scannumber =
143 ScanListHeader[1] * 0x100 + ScanListHeader[0]; */
144 numScanpoints = ScanListHeader[29] * 0x100 + ScanListHeader[28];
145 angleTicks = ScanListHeader[23] * 0x100 + ScanListHeader[22];
146
147 for (unsigned int i = 0; i < numScanpoints; ++i)
148 {
149 bool dropPacket = false;
150
151 m_client.readAsync(ScanPointData, 10, 10, 10);
152 SPlayer = ScanPointData[0] & 0x0F; // two lower bits denote layer
153 // SPecho = ScanPointData[0] >> 4; // two higher bits
154 // denote echo
155 SPHangle = (char)ScanPointData[3] * 0x100 + ScanPointData[2]; // signed INT16 here
156 SPdistance = ScanPointData[5] * 0x100 + ScanPointData[4];
157
158 // Sanity checks
159 if (SPlayer > 4)
160 {
161 dropPacket = true;
162 // std::cerr << "Invalid layer: " << SPlayer << " should
163 // be element of [0,3] Scanpoint dropped.\n";
164 }
165 if ((SPHangle < -((int)angleTicks) / 2) || (SPHangle > (int)angleTicks / 2))
166 {
167 dropPacket = true;
168 // std::cerr << "Invalid horizontal angle: " <<
169 // (int)-angleTicks/2 << "< " << SPHangle << " <" <<
170 // angleTicks/2 << " Scanpoint dropped.\n";
171 }
172 if ((SPdistance < 30) || (SPdistance > 20000))
173 {
174 dropPacket = true;
175 // std::cerr << "Invalid distance: 30< " << SPdistance
176 // << " <20000 Scanpoint dropped.\n";
177 }
178
179 if (!dropPacket)
180 {
181 // TODO: Process point information correctly
182 CPoint3D cartesianPoint = convertToCartesian(
183 convertLayerToRad(SPlayer), // vertikal coord of scanner
184 convertTicksToHRad(
185 SPHangle,
186 angleTicks), // horizontal coord of scanner
187 SPdistance);
188
189 // write scanpoint data to observation object
190 newObs->points3D_x.push_back(cartesianPoint.x());
191 newObs->points3D_y.push_back(cartesianPoint.y());
192 newObs->points3D_z.push_back(cartesianPoint.z());
193 }
194 } // for
195
196 // return observation to framework
197 appendObservation(newObs);
198
199 state = SearchForAF;
200 break; // SaveData
201 } // Switch
202 } // While
203
204 // Send stop command
205 makeCommandHeader(msg);
206 makeStopCommand(msg);
207 m_client.writeAsync(&msg[0], 28);
208
209 m_client.close();
210 } // dataCollection
211
212 CPoint3D CIbeoLuxETH::convertToCartesian(float vrad, float hrad, float distance)
213 {
214 float x, y, z;
215 float rho, phi, theta;
216
217 // Convert from laserscanner coordinate system to spherical coordinates
218 rho = distance / 100; // cm to meter
219 phi = -hrad + (M_PI / 2); // start with 0 pointing straight up
220 theta = vrad + M_PI; // 0 is straight ahead, going clockwise for 2 Pi
221
222 x = rho * sin(phi) * cos(theta);
223 y = rho * sin(phi) * sin(theta);
224 z = rho * cos(phi);
225
226 CPoint3D point(x, y, z);
227 return point;
228 }
229
230 double CIbeoLuxETH::convertTicksToHRad(int hticks, int hticksPerRotation)
231 {
232 return M_PI * 2 * hticks / hticksPerRotation;
233 }
234
235 double CIbeoLuxETH::convertLayerToRad(int scanlayer)
236 {
237 double vangle;
238
239 switch (scanlayer)
240 {
241 case 0:
242 vangle = -0.02094395103;
243 break;
244 case 1:
245 vangle = -0.006981317009;
246 break;
247 case 2:
248 vangle = 0.006981317009;
249 break;
250 case 3:
251 vangle = 0.02094395103;
252 break;
253 default:
254 vangle = 0;
255 std::cerr << "Layer: " << scanlayer << "! Returning " << vangle << " as angle.\n";
256 break;
257 }
258
259 return vangle;
260 }
261
262 void CIbeoLuxETH::loadConfig_sensorSpecific(
263 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
264 {
265 float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
266 bool faillNotFound = false;
267 pose_x = configSource.read_float(iniSection, "pose_x", 0, faillNotFound);
268 pose_y = configSource.read_float(iniSection, "pose_y", 0, faillNotFound);
269 pose_z = configSource.read_float(iniSection, "pose_z", 0, faillNotFound);
270 pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, faillNotFound);
271 pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, faillNotFound);
272 pose_roll = configSource.read_float(iniSection, "pose_roll", 0, faillNotFound);
273
274 m_sensorPose =
275 CPose3D(pose_x, pose_y, pose_z, DEG2RAD(pose_yaw), DEG2RAD(pose_pitch), DEG2RAD(pose_roll));
276 }
277
278 void CIbeoLuxETH::makeCommandHeader(unsigned char* buffer)
279 {
280 // Header - all big endian
281 buffer[0] = 0xAF; // magic word
282 buffer[1] = 0xFE;
283 buffer[2] = 0xC0;
284 buffer[3] = 0xC2;
285 buffer[4] = 0x00; // Size of previous message, here just left to null
286 buffer[5] = 0x00;
287 buffer[6] = 0x00;
288 buffer[7] = 0x00;
289 buffer[8] = 0x00; // Size of data block
290 buffer[9] = 0x00;
291 buffer[10] = 0x00;
292 buffer[11] = 0x00; // to be set by the command function
293 buffer[12] = 0x00; // Reserved + source Id
294 buffer[13] = 0x78; // source ID of 0x78 as observed
295 buffer[14] = 0x20; // Data Type - 2010 = command
296 buffer[15] = 0x10;
297 buffer[16] = 0x00; // 4* ntpp time (s) + 4* fractions of a second
298 buffer[17] = 0x00;
299 buffer[18] = 0x00;
300 buffer[19] = 0x00;
301 buffer[20] = 0x00;
302 buffer[21] = 0x00;
303 buffer[22] = 0x00;
304 buffer[23] = 0x00;
305 }
306
307 void CIbeoLuxETH::makeStartCommand(unsigned char* buffer)
308 {
309 // Header - all big endian
310 buffer[11] = 0x04; // Size of data block
311 // Data Block - all little endian
312 buffer[24] = 0x20; // Start Measure 0x0020
313 buffer[25] = 0x00;
314 buffer[26] = 0x00; // Reserved, but obligatory
315 buffer[27] = 0x00;
316 }
317
318 void CIbeoLuxETH::makeStopCommand(unsigned char* buffer)
319 {
320 // Header - all big endian
321 buffer[11] = 0x04; // Size of data block
322 // Data Block - all little endian
323 buffer[24] = 0x21; // Stop Measure 0x0021
324 buffer[25] = 0x00;
325 buffer[26] = 0x00; // Reserved, but obligatory
326 buffer[27] = 0x00;
327 }
328
329 void CIbeoLuxETH::makeTypeCommand(unsigned char* buffer)
330 {
331 // Header - all big endian
332 buffer[11] = 0x08; // Size of data block
333 // Data Block - big endian (for filter command!)
334 buffer[24] = 0x00; // Command Type - 0005 = set datatype filter
335 buffer[25] = 0x05;
336 buffer[26] = 0x00; // Data type filter length
337 buffer[27] = 0x02;
338 buffer[28] = 0x22; // start value
339 buffer[29] = 0x00;
340 buffer[30] = 0x22; // end value
341 buffer[31] = 0x10;
342 }
343
344 /** This method can or cannot be implemented in the derived class, depending on
345 * the need for it.
346 * \exception This method must throw an exception with a descriptive message if
347 * some critical error is found.
348 */
349 void CIbeoLuxETH::initialize()
350 {
351 m_run = true;
352
353 // Start a thread to collect and interpret scandata
354 // std::cout << "Start dataCollectionThread\n";
355
356 // boost threads:
357 // boost::thread
358 // dataCollectionThread(boost::bind(&CIbeoLuxETH::dataCollection,this));
359
360 dataCollectionThread = std::thread(&CIbeoLuxETH::dataCollection, this);
361 }
362
363 void CIbeoLuxETH::doProcess()
364 {
365 // nothing is done here
366 // data is collected in the dataCollection thread
367 }