17 #define APPERTURE 4.712385 // in radian <=> 270°
39 m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
40 m_beamApperture(.25 *
M_PI / 180.0)
44 CIbeoLuxETH::~CIbeoLuxETH()
47 dataCollectionThread.join();
51 std::this_thread::sleep_for(10ms);
54 void CIbeoLuxETH::dataCollection()
57 unsigned char msgIn[1], Header[20], ScanListHeader[44], ScanPointData[10];
58 unsigned int datatype, numScanpoints, angleTicks, SPlayer,
61 unsigned char msg[32];
64 m_client.connect(m_ip, m_port);
67 makeCommandHeader(msg);
69 m_client.writeAsync(&msg[0], 32);
72 makeCommandHeader(msg);
73 makeStartCommand(msg);
74 m_client.writeAsync(&msg[0], 28);
81 m_client.readAsync(msgIn, 1, 100, 10);
85 m_client.readAsync(msgIn, 1, 100, 10);
92 m_client.readAsync(msgIn, 1, 100, 10);
99 m_client.readAsync(msgIn, 1, 100, 10);
100 if (msgIn[0] == 0xC2)
106 m_client.readAsync(Header, 20, 100, 10);
107 datatype = Header[10] * 0x100 + Header[11];
130 std::cerr <<
"UNKNOWN packet of type " << hex
131 << datatype <<
" received!!\n";
138 std::make_shared<CObservation3DRangeScan>();
139 newObs->hasPoints3D =
true;
140 newObs->maxRange = 200.00;
142 m_client.readAsync(ScanListHeader, 44, 10, 10);
145 numScanpoints = ScanListHeader[29] * 0x100 + ScanListHeader[28];
146 angleTicks = ScanListHeader[23] * 0x100 + ScanListHeader[22];
148 for (
unsigned int i = 0; i < numScanpoints; ++i)
150 bool dropPacket =
false;
152 m_client.readAsync(ScanPointData, 10, 10, 10);
154 ScanPointData[0] & 0x0F;
157 SPHangle = (char)ScanPointData[3] * 0x100 +
159 SPdistance = ScanPointData[5] * 0x100 + ScanPointData[4];
168 if ((SPHangle < -((
int)angleTicks) / 2) ||
169 (SPHangle > (int)angleTicks / 2))
176 if ((SPdistance < 30) || (SPdistance > 20000))
186 CPoint3D cartesianPoint = convertToCartesian(
195 newObs->points3D_x.push_back(cartesianPoint.
x());
196 newObs->points3D_y.push_back(cartesianPoint.
y());
197 newObs->points3D_z.push_back(cartesianPoint.z());
202 appendObservation(newObs);
210 makeCommandHeader(msg);
211 makeStopCommand(msg);
212 m_client.writeAsync(&msg[0], 28);
220 float rho, phi, theta;
224 phi = -hrad + (
M_PI / 2);
227 x = rho * sin(phi) * cos(theta);
228 y = rho * sin(phi) * sin(theta);
235 double CIbeoLuxETH::convertTicksToHRad(
int hticks,
int hticksPerRotation)
237 return M_PI * 2 * hticks / hticksPerRotation;
240 double CIbeoLuxETH::convertLayerToRad(
int scanlayer)
247 vangle = -0.02094395103;
250 vangle = -0.006981317009;
253 vangle = 0.006981317009;
256 vangle = 0.02094395103;
260 std::cerr <<
"Layer: " << scanlayer <<
"! Returning " << vangle
268 void CIbeoLuxETH::loadConfig_sensorSpecific(
270 const std::string& iniSection)
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);
278 configSource.
read_float(iniSection,
"pose_yaw", 0, faillNotFound);
280 configSource.
read_float(iniSection,
"pose_pitch", 0, faillNotFound);
282 configSource.
read_float(iniSection,
"pose_roll", 0, faillNotFound);
289 void CIbeoLuxETH::makeCommandHeader(
unsigned char* buffer)
318 void CIbeoLuxETH::makeStartCommand(
unsigned char* buffer)
329 void CIbeoLuxETH::makeStopCommand(
unsigned char* buffer)
340 void CIbeoLuxETH::makeTypeCommand(
unsigned char* buffer)
371 dataCollectionThread = std::thread(&CIbeoLuxETH::dataCollection,
this);
374 void CIbeoLuxETH::doProcess()