Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
11 #include "hwdrivers-precomp.h" // Precompiled headers
12 
13 #include <mrpt/hwdrivers/CIbeoLuxETH.h> // Precompiled headers
14 
15 #include <bitset>
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::utils;
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 
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_maxRange(200.0),
42  m_beamApperture(.25*M_PI/180.0),
43  vwinkel(0.0)
44 {
45 }
46 
47 CIbeoLuxETH::~CIbeoLuxETH()
48 {
49  m_run = false;
50  mrpt::system::joinThread(dataCollectionThread);
51  // Wait a little for the thread to come down
52  // Don't ask why, it just works
53  //TODO: Try without the delay
54  sleep(10);
55 }
56 
57 void CIbeoLuxETH::dataCollection()
58 {
59  unsigned char state = SearchForAF;
60  unsigned char msgIn[1], Header[20], ScanListHeader[44], ScanPointData[10];
61  unsigned int datatype, /*scannumber,*/ numScanpoints, angleTicks, SPlayer, 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) state = SearchForC0; else state = SearchForAF;
89  break;
90  case SearchForC0:
91  m_client.readAsync(msgIn, 1, 100, 10);
92  if (msgIn[0] == 0xC0) state = SearchForC2; else state = SearchForAF;
93  break;
94  case SearchForC2:
95  m_client.readAsync(msgIn, 1, 100, 10);
96  if (msgIn[0] == 0xC2) state = PacketFound; else state = SearchForAF;
97  break;
98  case PacketFound:
99  m_client.readAsync(Header, 20, 100, 10);
100  datatype = Header[10] * 0x100 + Header[11];
101  switch(datatype)
102  {
103  case 0x2030:
104  // do nothing
105  state = SearchForAF;
106  break;
107  case 0x2221:
108  // do nothing
109  state = SearchForAF;
110  break;
111  case 0x2805:
112  // do nothing
113  state = SearchForAF;
114  break;
115  case 0x2020:
116  // do nothing
117  state = SearchForAF;
118  break;
119  case 0x2202:
120  state = SaveData;
121  break;
122  default:
123  std::cerr << "UNKNOWN packet of type " << hex << datatype << " received!!\n";
124  state = SearchForAF;
125  }
126  break;
127  case SaveData:
128  // Create new observation object pointer
129  CObservation3DRangeScanPtr newObs = CObservation3DRangeScan::Create();
130  newObs->hasPoints3D = true;
131  newObs->maxRange = 200.00;
132 
133  m_client.readAsync(ScanListHeader, 44, 10, 10);
134  /*scannumber =
135  ScanListHeader[1] * 0x100 + ScanListHeader[0]; */
136  numScanpoints = ScanListHeader[29] * 0x100 + ScanListHeader[28];
137  angleTicks = ScanListHeader[23] * 0x100 + ScanListHeader[22];
138 
139  for (unsigned int i = 0; i < numScanpoints; ++i)
140  {
141  bool dropPacket = false;
142 
143  m_client.readAsync(ScanPointData, 10, 10, 10);
144  SPlayer = ScanPointData[0] & 0x0F; // two lower bits denote layer
145  // SPecho = ScanPointData[0] >> 4; // two higher bits denote echo
146  SPHangle = (char)ScanPointData[3] * 0x100 + ScanPointData[2]; // signed INT16 here
147  SPdistance = ScanPointData[5] * 0x100 + ScanPointData[4];
148 
149  // Sanity checks
150  if (SPlayer > 4)
151  {
152  dropPacket = true;
153  //std::cerr << "Invalid layer: " << SPlayer << " should be element of [0,3] Scanpoint dropped.\n";
154  }
155  if ((SPHangle < -((int)angleTicks)/2) || (SPHangle > (int)angleTicks/2))
156  {
157  dropPacket = true;
158  //std::cerr << "Invalid horizontal angle: " << (int)-angleTicks/2 << "< " << SPHangle << " <" << angleTicks/2 << " Scanpoint dropped.\n";
159  }
160  if ((SPdistance < 30) || (SPdistance > 20000))
161  {
162  dropPacket = true;
163  //std::cerr << "Invalid distance: 30< " << SPdistance << " <20000 Scanpoint dropped.\n";
164  }
165 
166 
167  if (!dropPacket)
168  {
169  //TODO: Process point information correctly
170  CPoint3D cartesianPoint = convertToCartesian(
171  convertLayerToRad(SPlayer), // vertikal coord of scanner
172  convertTicksToHRad(SPHangle, angleTicks), // horizontal coord of scanner
173  SPdistance);
174 
175  // write scanpoint data to observation object
176  newObs->points3D_x.push_back(cartesianPoint.x());
177  newObs->points3D_y.push_back(cartesianPoint.y());
178  newObs->points3D_z.push_back(cartesianPoint.z());
179  }
180  } // for
181 
182  // return observation to framework
183  appendObservation( newObs );
184 
185  state = SearchForAF;
186  break; // SaveData
187  } // Switch
188  } // While
189 
190  // Send stop command
191  makeCommandHeader(msg);
192  makeStopCommand(msg);
193  m_client.writeAsync(&msg[0], 28);
194 
195  m_client.close();
196 } // dataCollection
197 
198 CPoint3D CIbeoLuxETH::convertToCartesian(float vrad, float hrad, float distance)
199 {
200  float x, y, z;
201  float rho, phi, theta;
202 
203  // Convert from laserscanner coordinate system to spherical coordinates
204  rho = distance/100; // cm to meter
205  phi = -hrad+(M_PI/2); // start with 0 pointing straight up
206  theta = vrad+M_PI; // 0 is straight ahead, going clockwise for 2 Pi
207 
208  x = rho * sin (phi) * cos (theta);
209  y = rho * sin (phi) * sin (theta);
210  z = rho * cos (phi);
211 
212  CPoint3D point(x, y, z);
213  return point;
214 }
215 
216 double CIbeoLuxETH::convertTicksToHRad(int hticks, int hticksPerRotation)
217 {
218  return M_PI*2 * hticks / hticksPerRotation;
219 }
220 
221 double CIbeoLuxETH::convertLayerToRad(int scanlayer)
222 {
223  double vangle;
224 
225  switch(scanlayer)
226  {
227  case 0:
228  vangle = -0.02094395103;
229  break;
230  case 1:
231  vangle = -0.006981317009;
232  break;
233  case 2:
234  vangle = 0.006981317009;
235  break;
236  case 3:
237  vangle = 0.02094395103;
238  break;
239  default:
240  vangle = 0;
241  std::cerr << "Layer: " << scanlayer << "! Returning " << vangle << " as angle.\n";
242  break;
243  }
244 
245  return vangle;
246 }
247 
248 void CIbeoLuxETH::loadConfig_sensorSpecific( const mrpt::utils::CConfigFileBase &configSource,
249  const std::string &iniSection )
250 {
251  float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
252  bool faillNotFound = false;
253  pose_x = configSource.read_float(iniSection,"pose_x",0,faillNotFound);
254  pose_y = configSource.read_float(iniSection,"pose_y",0,faillNotFound);
255  pose_z = configSource.read_float(iniSection,"pose_z",0,faillNotFound);
256  pose_yaw = configSource.read_float(iniSection,"pose_yaw",0,faillNotFound);
257  pose_pitch = configSource.read_float(iniSection,"pose_pitch",0,faillNotFound);
258  pose_roll = configSource.read_float(iniSection,"pose_roll",0,faillNotFound);
259 
260  m_sensorPose = CPose3D( pose_x, pose_y, pose_z,
261  DEG2RAD( pose_yaw ),DEG2RAD( pose_pitch ), DEG2RAD( pose_roll ));
262 
263 }
264 
265 void CIbeoLuxETH::makeCommandHeader(unsigned char* buffer)
266 {
267  // Header - all big endian
268  buffer[0] = 0xAF; // magic word
269  buffer[1] = 0xFE;
270  buffer[2] = 0xC0;
271  buffer[3] = 0xC2;
272  buffer[4] = 0x00; // Size of previous message, here just left to null
273  buffer[5] = 0x00;
274  buffer[6] = 0x00;
275  buffer[7] = 0x00;
276  buffer[8] = 0x00; // Size of data block
277  buffer[9] = 0x00;
278  buffer[10] = 0x00;
279  buffer[11] = 0x00; // to be set by the command function
280  buffer[12] = 0x00; // Reserved + source Id
281  buffer[13] = 0x78; // source ID of 0x78 as observed
282  buffer[14] = 0x20; // Data Type - 2010 = command
283  buffer[15] = 0x10;
284  buffer[16] = 0x00; // 4* ntpp time (s) + 4* fractions of a second
285  buffer[17] = 0x00;
286  buffer[18] = 0x00;
287  buffer[19] = 0x00;
288  buffer[20] = 0x00;
289  buffer[21] = 0x00;
290  buffer[22] = 0x00;
291  buffer[23] = 0x00;
292 }
293 
294 void CIbeoLuxETH::makeStartCommand(unsigned char* buffer)
295 {
296  // Header - all big endian
297  buffer[11] = 0x04; // Size of data block
298  // Data Block - all little endian
299  buffer[24] = 0x20; // Start Measure 0x0020
300  buffer[25] = 0x00;
301  buffer[26] = 0x00; // Reserved, but obligatory
302  buffer[27] = 0x00;
303 }
304 
305 void CIbeoLuxETH::makeStopCommand(unsigned char* buffer)
306 {
307  // Header - all big endian
308  buffer[11] = 0x04; // Size of data block
309  // Data Block - all little endian
310  buffer[24] = 0x21; // Stop Measure 0x0021
311  buffer[25] = 0x00;
312  buffer[26] = 0x00; // Reserved, but obligatory
313  buffer[27] = 0x00;
314 }
315 
316 void CIbeoLuxETH::makeTypeCommand(unsigned char* buffer)
317 {
318  // Header - all big endian
319  buffer[11] = 0x08; // Size of data block
320  // Data Block - big endian (for filter command!)
321  buffer[24] = 0x00; // Command Type - 0005 = set datatype filter
322  buffer[25] = 0x05;
323  buffer[26] = 0x00; // Data type filter length
324  buffer[27] = 0x02;
325  buffer[28] = 0x22; // start value
326  buffer[29] = 0x00;
327  buffer[30] = 0x22; // end value
328  buffer[31] = 0x10;
329 }
330 
331 /** This method can or cannot be implemented in the derived class, depending on the need for it.
332 * \exception This method must throw an exception with a descriptive message if some critical error is found.
333 */
334 void CIbeoLuxETH::initialize()
335 {
336  m_run = true;
337 
338  // Start a thread to collect and interpret scandata
339  //std::cout << "Start dataCollectionThread\n";
340 
341  // boost threads:
342  //boost::thread dataCollectionThread(boost::bind(&CIbeoLuxETH::dataCollection,this));
343 
344  dataCollectionThread = createThreadFromObjectMethod(this, &CIbeoLuxETH::dataCollection);
345 }
346 
347 void CIbeoLuxETH::doProcess()
348 {
349  // nothing is done here
350  // data is collected in the dataCollection thread
351 }
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
const unsigned char PacketFound
Definition: CIbeoLuxETH.cpp:32
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
const unsigned char SaveData
Definition: CIbeoLuxETH.cpp:33
#define M_PI
Definition: bits.h:78
This "software driver" implements the communication protocol for interfacing a Ibeo Lux laser scanner...
Definition: CIbeoLuxETH.h:40
const unsigned char SearchForC2
Definition: CIbeoLuxETH.cpp:31
This class allows loading and storing values and vectors of different types from a configuration text...
const unsigned char SearchForC0
Definition: CIbeoLuxETH.cpp:30
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
GLuint buffer
Definition: glew.h:1585
#define DEG2RAD
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
A class used to store a 3D point.
Definition: CPoint3D.h:32
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
TThreadHandle createThreadFromObjectMethod(CLASS *obj, void(CLASS::*func)(PARAM), PARAM param)
Creates a new thread running a non-static method (so it will have access to "this") from another meth...
Definition: threads.h:216
GLdouble GLdouble z
Definition: glew.h:1464
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
const unsigned char SearchForAF
Definition: CIbeoLuxETH.cpp:28
const unsigned char SearchForFE
Definition: CIbeoLuxETH.cpp:29
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1511
void BASE_IMPEXP joinThread(const TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:190



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018