/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "hwdrivers-precomp.h" // Precompiled headers // #include // Precompiled headers #include #include #include #define APPERTURE 4.712385 // in radian <=> 270° using namespace mrpt; using namespace mrpt::system; using namespace mrpt::poses; using namespace mrpt::hwdrivers; using namespace mrpt::obs; using namespace std; // TODO: Use enum instead const unsigned char SearchForAF = 0; const unsigned char SearchForFE = 1; const unsigned char SearchForC0 = 2; const unsigned char SearchForC2 = 3; const unsigned char PacketFound = 4; const unsigned char SaveData = 5; IMPLEMENTS_GENERIC_SENSOR(CIbeoLuxETH, mrpt::hwdrivers) CIbeoLuxETH::CIbeoLuxETH(string _ip, unsigned int _port) : m_ip(_ip), m_port(_port), m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), m_beamApperture(.25 * M_PI / 180.0) { } CIbeoLuxETH::~CIbeoLuxETH() { m_run = false; dataCollectionThread.join(); // Wait a little for the thread to come down // Don't ask why, it just works // TODO: Try without the delay std::this_thread::sleep_for(10ms); } void CIbeoLuxETH::dataCollection() { unsigned char state = SearchForAF; unsigned char msgIn[1], Header[20], ScanListHeader[44], ScanPointData[10]; unsigned int datatype, /*scannumber,*/ numScanpoints, angleTicks, SPlayer, SPdistance; // SPecho; int SPHangle; unsigned char msg[32]; // Start TCP-connection to laserscanner m_client.connect(m_ip, m_port); // Send filter command makeCommandHeader(msg); makeTypeCommand(msg); m_client.writeAsync(&msg[0], 32); // Send start command makeCommandHeader(msg); makeStartCommand(msg); m_client.writeAsync(&msg[0], 28); while (m_run) { switch (state) { case SearchForAF: m_client.readAsync(msgIn, 1, 100, 10); if (msgIn[0] == 0xAF) state = SearchForFE; break; case SearchForFE: m_client.readAsync(msgIn, 1, 100, 10); if (msgIn[0] == 0xFE) state = SearchForC0; else state = SearchForAF; break; case SearchForC0: m_client.readAsync(msgIn, 1, 100, 10); if (msgIn[0] == 0xC0) state = SearchForC2; else state = SearchForAF; break; case SearchForC2: m_client.readAsync(msgIn, 1, 100, 10); if (msgIn[0] == 0xC2) state = PacketFound; else state = SearchForAF; break; case PacketFound: m_client.readAsync(Header, 20, 100, 10); datatype = Header[10] * 0x100 + Header[11]; switch (datatype) { case 0x2030: // do nothing state = SearchForAF; break; case 0x2221: // do nothing state = SearchForAF; break; case 0x2805: // do nothing state = SearchForAF; break; case 0x2020: // do nothing state = SearchForAF; break; case 0x2202: state = SaveData; break; default: std::cerr << "UNKNOWN packet of type " << hex << datatype << " received!!\n"; state = SearchForAF; } break; case SaveData: // Create new observation object pointer CObservation3DRangeScan::Ptr newObs = std::make_shared(); newObs->hasPoints3D = true; newObs->maxRange = 200.00; m_client.readAsync(ScanListHeader, 44, 10, 10); /*scannumber = ScanListHeader[1] * 0x100 + ScanListHeader[0]; */ numScanpoints = ScanListHeader[29] * 0x100 + ScanListHeader[28]; angleTicks = ScanListHeader[23] * 0x100 + ScanListHeader[22]; for (unsigned int i = 0; i < numScanpoints; ++i) { bool dropPacket = false; m_client.readAsync(ScanPointData, 10, 10, 10); SPlayer = ScanPointData[0] & 0x0F; // two lower bits denote layer // SPecho = ScanPointData[0] >> 4; // two higher bits // denote echo SPHangle = (char)ScanPointData[3] * 0x100 + ScanPointData[2]; // signed INT16 here SPdistance = ScanPointData[5] * 0x100 + ScanPointData[4]; // Sanity checks if (SPlayer > 4) { dropPacket = true; // std::cerr << "Invalid layer: " << SPlayer << " should // be element of [0,3] Scanpoint dropped.\n"; } if ((SPHangle < -((int)angleTicks) / 2) || (SPHangle > (int)angleTicks / 2)) { dropPacket = true; // std::cerr << "Invalid horizontal angle: " << // (int)-angleTicks/2 << "< " << SPHangle << " <" << // angleTicks/2 << " Scanpoint dropped.\n"; } if ((SPdistance < 30) || (SPdistance > 20000)) { dropPacket = true; // std::cerr << "Invalid distance: 30< " << SPdistance // << " <20000 Scanpoint dropped.\n"; } if (!dropPacket) { // TODO: Process point information correctly CPoint3D cartesianPoint = convertToCartesian( convertLayerToRad(SPlayer), // vertikal coord of scanner convertTicksToHRad( SPHangle, angleTicks), // horizontal coord of scanner SPdistance); // write scanpoint data to observation object newObs->points3D_x.push_back(cartesianPoint.x()); newObs->points3D_y.push_back(cartesianPoint.y()); newObs->points3D_z.push_back(cartesianPoint.z()); } } // for // return observation to framework appendObservation(newObs); state = SearchForAF; break; // SaveData } // Switch } // While // Send stop command makeCommandHeader(msg); makeStopCommand(msg); m_client.writeAsync(&msg[0], 28); m_client.close(); } // dataCollection CPoint3D CIbeoLuxETH::convertToCartesian(float vrad, float hrad, float distance) { float x, y, z; float rho, phi, theta; // Convert from laserscanner coordinate system to spherical coordinates rho = distance / 100; // cm to meter phi = -hrad + (M_PI / 2); // start with 0 pointing straight up theta = vrad + M_PI; // 0 is straight ahead, going clockwise for 2 Pi x = rho * sin(phi) * cos(theta); y = rho * sin(phi) * sin(theta); z = rho * cos(phi); CPoint3D point(x, y, z); return point; } double CIbeoLuxETH::convertTicksToHRad(int hticks, int hticksPerRotation) { return M_PI * 2 * hticks / hticksPerRotation; } double CIbeoLuxETH::convertLayerToRad(int scanlayer) { double vangle; switch (scanlayer) { case 0: vangle = -0.02094395103; break; case 1: vangle = -0.006981317009; break; case 2: vangle = 0.006981317009; break; case 3: vangle = 0.02094395103; break; default: vangle = 0; std::cerr << "Layer: " << scanlayer << "! Returning " << vangle << " as angle.\n"; break; } return vangle; } void CIbeoLuxETH::loadConfig_sensorSpecific( const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection) { float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll; bool faillNotFound = false; pose_x = configSource.read_float(iniSection, "pose_x", 0, faillNotFound); pose_y = configSource.read_float(iniSection, "pose_y", 0, faillNotFound); pose_z = configSource.read_float(iniSection, "pose_z", 0, faillNotFound); pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, faillNotFound); pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, faillNotFound); pose_roll = configSource.read_float(iniSection, "pose_roll", 0, faillNotFound); m_sensorPose = CPose3D(pose_x, pose_y, pose_z, DEG2RAD(pose_yaw), DEG2RAD(pose_pitch), DEG2RAD(pose_roll)); } void CIbeoLuxETH::makeCommandHeader(unsigned char* buffer) { // Header - all big endian buffer[0] = 0xAF; // magic word buffer[1] = 0xFE; buffer[2] = 0xC0; buffer[3] = 0xC2; buffer[4] = 0x00; // Size of previous message, here just left to null buffer[5] = 0x00; buffer[6] = 0x00; buffer[7] = 0x00; buffer[8] = 0x00; // Size of data block buffer[9] = 0x00; buffer[10] = 0x00; buffer[11] = 0x00; // to be set by the command function buffer[12] = 0x00; // Reserved + source Id buffer[13] = 0x78; // source ID of 0x78 as observed buffer[14] = 0x20; // Data Type - 2010 = command buffer[15] = 0x10; buffer[16] = 0x00; // 4* ntpp time (s) + 4* fractions of a second buffer[17] = 0x00; buffer[18] = 0x00; buffer[19] = 0x00; buffer[20] = 0x00; buffer[21] = 0x00; buffer[22] = 0x00; buffer[23] = 0x00; } void CIbeoLuxETH::makeStartCommand(unsigned char* buffer) { // Header - all big endian buffer[11] = 0x04; // Size of data block // Data Block - all little endian buffer[24] = 0x20; // Start Measure 0x0020 buffer[25] = 0x00; buffer[26] = 0x00; // Reserved, but obligatory buffer[27] = 0x00; } void CIbeoLuxETH::makeStopCommand(unsigned char* buffer) { // Header - all big endian buffer[11] = 0x04; // Size of data block // Data Block - all little endian buffer[24] = 0x21; // Stop Measure 0x0021 buffer[25] = 0x00; buffer[26] = 0x00; // Reserved, but obligatory buffer[27] = 0x00; } void CIbeoLuxETH::makeTypeCommand(unsigned char* buffer) { // Header - all big endian buffer[11] = 0x08; // Size of data block // Data Block - big endian (for filter command!) buffer[24] = 0x00; // Command Type - 0005 = set datatype filter buffer[25] = 0x05; buffer[26] = 0x00; // Data type filter length buffer[27] = 0x02; buffer[28] = 0x22; // start value buffer[29] = 0x00; buffer[30] = 0x22; // end value buffer[31] = 0x10; } /** This method can or cannot be implemented in the derived class, depending on * the need for it. * \exception This method must throw an exception with a descriptive message if * some critical error is found. */ void CIbeoLuxETH::initialize() { m_run = true; // Start a thread to collect and interpret scandata // std::cout << "Start dataCollectionThread\n"; // boost threads: // boost::thread // dataCollectionThread(boost::bind(&CIbeoLuxETH::dataCollection,this)); dataCollectionThread = std::thread(&CIbeoLuxETH::dataCollection, this); } void CIbeoLuxETH::doProcess() { // nothing is done here // data is collected in the dataCollection thread }