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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020