| 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 |
} |