CIbeoLuxETH.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, 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 <mrpt/hwdrivers/CIbeoLuxETH.h>  // Precompiled headers

#include <bitset>
#include <thread>

#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<CObservation3DRangeScan>();
                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
}