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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020