How to use RPLidar with QT C++?

[KroMignon]

@Geeva said in How to use RPLidar with QT C++?:

I am not getting proper sync_quality data. How can I get, what modification I have to do?

I think this is just a cast problem, sync_quality and sync_qualitylevel are defined as unsigned char, cout will output corresponding ASCII char. Try this:

	         cout << "quality  "<< int(sync_quality) << endl;
	        cout << "distance "<< distance_q2 << endl;
	        cout << "angle "<< angle_q6_checkbit << endl;
	        cout << "quality level "<< int(sync_qualitylevel) << endl;

[Geeva]

@KroMignon Ya, It is working. Now in laser data thread, I am getting the proper data. While trying to take the data from laser data thread to motion thread, I am not getting any data.

cout data in Laser data Thread output:

Taking data from Laser data thread to motion thread (cout data in motion thread):

        cout << "distance "<< LaserdataThread::distance_q2 << endl;
        cout << "angle "<< UltrasonicThread::angle_q6_checkbit << endl;
        cout << "quality level "<< int(UltrasonicThread::sync_qualitylevel) << endl;

why it is happening? what is causing a problem?

[KroMignon]

@Geeva said in How to use RPLidar with QT C++?:

why it is happening? what is causing a problem?

You can't do that!!! This is not a proper way to share value between different threads!

distance_q2, angle_q6_checkbit, etc. are NOT atomic variable, so reading and writing them from different thread is not consistant!!
Read/write are not atomic, that means, read/writing those variable could be interrupted by thread context switch, so you are not sure you will read good/real values!

Try to declare then as atomic:

#include <QAtomicInteger>
...
public:
    
    static QAtomicInteger<_u16> angle_q6_checkbit;
    static QAtomicInteger<_u16> distance_q2;
    static QAtomicInteger<_u8> sync_quality;
    static QAtomicInteger<_u8> sync_qualitylevel;

[Geeva]

@KroMignon I have tried to share values similar way but I am getting an error.

Modification in Header:

public:

    static QAtomicInteger<_u16> angle_q6_checkbit;
    static QAtomicInteger<_u16> distance_q2;
    static QAtomicInteger<_u8> sync_quality;
    static QAtomicInteger<_u8> sync_qualitylevel;
    
//public:
    
//    static _u16 angle_q6_checkbit;
//    static _u16 distance_q2;
//    static _u8 sync_quality;
//    static _u8 sync_qualitylevel;

Error:

/usr/include/arm-linux-gnueabihf/qt5/QtCore/qbasicatomic.h:116: error: invalid application of 'sizeof' to incomplete type 'QStaticAssertFailure<false>'

[KroMignon]

@Geeva strange, can't figure out what the problem is :(

One alternative could be to declare variables as QAtomicInt , I think variable size is not important in you application:

#include <QAtomicInteger>
...
public:
    static QAtomicInt angle_q6_checkbit;
    static QAtomicInt distance_q2;
    static QAtomicInt sync_quality;
    static QAtomicInt sync_qualitylevel;

If I have correctly read your code, this should also work, and you don't have to cast value when passing it to cout.

[Geeva]

@KroMignon No, still same problem. I think better I can post the program, it will help you to understand, and get a proper solution because still, I am not getting data, the same output I am getting.

Thread.h

public:
    static QAtomicInt angle_q6_checkbit;
    static QAtomicInt distance_q2;
    static QAtomicInt sync_quality;
    static QAtomicInt sync_qualitylevel;

struct RPLidarMeasurement
{
   float distance;
   float angle;
   uint8_t quality;
   bool  startBit;
};

struct rplidarData {
 double theta;
 double dist;
};

const RPLidarMeasurement & getCurrentPoint()
{
    return _currentMeasurement;
}

    void run();
    void run2();

     bool Stop_Ultra;
     static int Mode;
     bool Close_Thread;

     SensorDistance *sd1;

     static float centerDist;

signals:

     void NumberChangedCenter(float);

public slots:

protected:
    u_result _sendCommand(_u8 cmd, const void * payload, size_t payloadsize);
    u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout);

protected:
//    HardwareSerial * _bined_serialdev;
    RPLidarMeasurement _currentMeasurement;

};

Lidar data Thread (Thread.cpp):

#include "thread.h"
#include <QApplication>
#include <QAtomicInteger>

#include "rplidar.h" //RPLIDAR standard sdk
#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif

#ifdef _WIN32
#include <Windows.h>
#define delay(x)   ::Sleep(x)
#else
#include <unistd.h>

static inline void delay(_word_size_t ms){
    while (ms>=1000){
        usleep(1000*1000);
        ms-=1000;
    };
    if (ms!=0)
        usleep(ms*1000);
}
#endif

using namespace std;

 QAtomicInt Thread::angle_q6_checkbit;
 QAtomicInt Thread::distance_q2;
 QAtomicInt Thread::sync_quality;
 QAtomicInt Thread::sync_qualitylevel;

Thread::Thread(QObject *parent) :
    QThread(parent)
{
    cout<< "Thread Started"<<endl;
}

Thread::~Thread()
{
    cout<<"Thread Stopped"<<endl;
}

using namespace rp::standalone::rplidar;

bool checkRPLIDARHealth(RPlidarDriver * drv)
{
    u_result     op_result;
    rplidar_response_device_health_t healthinfo;

    op_result = drv->getHealth(healthinfo);
    if (IS_OK(op_result)) { 
        printf("RPLidar health status : %d\n", healthinfo.status);
        if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
            fprintf(stderr, "Error, rplidar internal error detected. Please reboot the device to retry.\n");
            return false;
        } else {
            return true;
        }

    } else {
        fprintf(stderr, "Error, cannot retrieve the lidar health code: %x\n", op_result);
        return false;
    }
}

#include <signal.h>

bool ctrl_c_pressed;
void ctrlc(int)
{
    ctrl_c_pressed = true;
}

using namespace std;

void UltrasonicThread::run()
{
    RPlidarDriver * drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);

    sd1 = new SensorDistance();

    if(sd1->Interrupt_error())
    {
        while(true)
        {
           if(Thread::Mode == 1)
           {
               if(!this->Stop_Ultra)
                {
                    run2();
                }

               else if(this->Stop_Ultra)
               {
                   drv->stop();
                   drv->stopMotor();
                   // done!
                   RPlidarDriver::DisposeDriver(drv);
                   drv = NULL;
                   break;
               }
           }

            if(this->Close_Thread)
            {
                 break;
            }
        }
   }
  delete sd1;
}

void Thread::run2()
{
    const char * opt_com_path = NULL;
    _u32         baudrateArray[2] = {115200, 256000};
    _u32         opt_com_baudrate = 0;
    u_result     op_result;

    bool useArgcBaudrate = false;

    printf("Ultra simple LIDAR data grabber for RPLIDAR.\n"
           "Version: "RPLIDAR_SDK_VERSION"\n");

    // read serial port from the command line...
    if (!opt_com_path) {
#ifdef _WIN32
        // use default com port
        opt_com_path = "\\\\.\\com3";
#else
        opt_com_path = "/dev/ttyUSB0";
#endif
    }

    // create the driver instance
    RPlidarDriver * drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
    if (!drv) {
        fprintf(stderr, "insufficent memory, exit\n");
        exit(-2);
    }

    rplidar_response_device_info_t devinfo;
    bool connectSuccess = false;

    if(useArgcBaudrate)
    {
        if(!drv)
            drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
        if (IS_OK(drv->connect(opt_com_path, opt_com_baudrate)))
        {
            op_result = drv->getDeviceInfo(devinfo);

            if (IS_OK(op_result))
            {
                connectSuccess = true;
            }
            else
            {
                delete drv;
                drv = NULL;
            }
        }
    }

    else
    {
        size_t baudRateArraySize = (sizeof(baudrateArray))/ (sizeof(baudrateArray[0]));
        for(size_t i = 0; i < baudRateArraySize; ++i)
        {
            if(!drv)
                drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
            if(IS_OK(drv->connect(opt_com_path, baudrateArray[i])))
            {
                op_result = drv->getDeviceInfo(devinfo);

                if (IS_OK(op_result))
                {
                    connectSuccess = true;
                    break;
                }
                else
                {
                    delete drv;
                    drv = NULL;
                }
            }
        }
    }

    if (!connectSuccess) {

        fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
            , opt_com_path);
    }

        printf("RPLIDAR S/N: ");

        for (int pos = 0; pos < 16 ;++pos) {
            printf("%02X", devinfo.serialnum[pos]);
        }

        printf("\n"
                "Firmware Ver: %d.%02d\n"
                "Hardware Rev: %d\n"
                , devinfo.firmware_version>>8
                , devinfo.firmware_version & 0xFF
                , (int)devinfo.hardware_version);

        if (!checkRPLIDARHealth(drv)) {
        }

        signal(SIGINT, ctrlc);

        drv->startMotor();

drv->startScan(0,1);

    while (1) {
rplidar_response_measurement_node_t nodes[8192];
size_t   count = _countof(nodes);

op_result = drv->grabScanData(nodes, count);

if (IS_OK(op_result)) {
    drv->ascendScanData(nodes, count);

    for (int pos = 0; pos < (int)count ; ++pos) {
        UltrasonicThread::sync_quality=nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT;
        UltrasonicThread::angle_q6_checkbit=(nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
        UltrasonicThread::distance_q2=nodes[pos].distance_q2/4.0f;
        UltrasonicThread::sync_qualitylevel=nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;

        printf("%s theta: %03.2f Dist: %08.2f Q: %d \n",

               (nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":"  ",
               (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f,
                nodes[pos].distance_q2/4.0f,
                nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

       cout << "distance "<< distance_q2 << endl;
       cout << "angle "<< angle_q6_checkbit << endl;
       cout << "quality level "<< int(sync_qualitylevel) << endl;

    }
}

if(this->Stop_Ultra)
{
    qDebug ()<< "stop lidar";

    drv->stop();
    drv->stopMotor();
    // done!
    RPlidarDriver::DisposeDriver(drv);
    drv = NULL;
    break;
}
    }
return ;
}

Motion Thread.cpp

void Motion::Synchronization(QString motiontype)
{
      if(motiontype == START)
       {
            cout << "angle "<<Thread::angle_q6_checkbit<< endl;
            cout << "distance "<<Thread::distance_q2<< endl;
            cout << "quality "<<Thread::sync_quality<< endl;

   if(269<Thread::angle_q6_checkbit && Thread::angle_q6_checkbit <271)
    {
        if(Thread::distance_q2>1200)
        {
            dist_left=1200;
        }
     else
      {
           dist_left=Thread::distance_q2;
      }
                cout<<"Motion +Stuff+="<< endl;
    }
  }
}

So, I want to take the lidar data into motion thread then I can apply with my logic stuff. This is what I want to do. So, here what wrong or how can I approach? Is there any other way?

[KroMignon]

@Geeva reply what do you mean with "same problem"? Does NOT compile?

[Geeva]

@KroMignon No output 0/0

[Geeva]

This post is deleted!

[Geeva]

@KroMignon I think problem because of for loop. I am getting the output data like this

theta: 356.73 Dist: 00000.00 Q: 0        // from Thread.cpp
theta: 357.36 Dist: 09147.00 Q: 47 
theta: 358.00 Dist: 08823.00 Q: 47 
theta: 358.64 Dist: 08964.00 Q: 47 
theta: 359.30 Dist: 09024.00 Q: 47 
theta: 359.92 Dist: 09140.00 Q: 47 
distance 9140                            // from motionthread.cpp
angle 359
quality level 47
distance 9140
angle 359
quality level 47
distance 9140
angle 359
quality level 47
distance 9140
angle 359
quality level 47
distance 9140
angle 359
quality level 47
distance 9140
angle 359
quality level 47
distance 9140
angle 359
quality level 47
distance 9140
angle 359
quality level 47
distance 9140
angle 359
quality level 47
distance 9140
angle 359
quality level 47
theta: 0.12 Dist: 00000.00 Q: 0   // from Thread.cpp
theta: 0.77 Dist: 00000.00 Q: 0 
theta: 1.42 Dist: 00000.00 Q: 0 
theta: 2.06 Dist: 00000.00 Q: 0 
theta: 2.70 Dist: 00000.00 Q: 0

Help would be greatly appreciated.

[vladstelmahovsky]

1st, you have to make sure it works with simplest demo apps, provided
then define, why exactly you want to reuse Qt and ask that questions here (f.ex. how to utilize Qt's feature <name of the feature> best for lidar?)
please, pay attention that baudrate of your lidar not compatible with RPi HW, so you have to use USB->Serial adapter, provided with the lidar

[jack1998]

This post is deleted!