基于QT实现西克sick激光LMS系列单线激光数据读取及显示

奥_特_快

3048人浏览 · 2020-08-22 18:19:56

奥_特_快 · 2020-08-22 18:19:56 发布

这是很久之前项目使用的单线激光雷达，lms151/141系列，激光照片如下图所示：

根据官方介绍资料如下

根据Technical_information_Telegram_Listing_Ranging_sensors_LMS1xx_LMS5xx_TiM5xx_MRS1000_MRS6000_NAV310_LD_OEM15xx_LD_LRS36xx_LMS4000_en_IM0045927.PDF手册介绍，

激光协议解释如下所示：

在连接激光雷达的时候，一定将个人电脑跟激光雷达配置在一个IP网段比如：sensor是192.168.0.1，那么个人电脑就可以配置为192.168.0.10

通过网口读取数据如下图所示：

如果使用RS232通信，定义如下：

通过QT建立工程，代码如下所示：

#include "mainwindow.h"
#include "ui_mainwindow.h"

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    InitTcpSocket();
    InitTimer();
    InitSlots();
}
//初始化TCP相关
void MainWindow::InitTcpSocket()
{
    tcpSocket = new QTcpSocket(this);
    tcpSocket->abort();
    tcpSocket->connectToHost("192.168.0.1",2111);
    //初始化
    for(int i=0;i<721;i++)
    {
        LidarInfo lidarN;
        lidarN.angle=i*0.25;
        qLidarList.append(lidarN);
    }
}

//初始化Timer
void MainWindow::InitTimer()
{
    //计数Timers
    cTimers = new QTimer(this);
    cTimers->setInterval(20);
}
//初始化槽相关
void MainWindow::InitSlots()
{
    QObject::connect(tcpSocket,SIGNAL(readyRead()),this,SLOT(revDataSlot()));
    QObject::connect(tcpSocket,SIGNAL(error(QAbstractSocket::SocketError)),this,SLOT(displayErrorSlot(QAbstractSocket::SocketError)));
    QObject::connect(ui->pushButton_con,SIGNAL(clicked()),this,SLOT(PushButtonConSlot()));
    QObject::connect(ui->pushButton_discon,SIGNAL(clicked()),this,SLOT(PushButtonDisConSlot()));
    //激光雷达处理数据的槽
    QObject::connect(this, SIGNAL(ProcessLidarData()), this, SLOT(ProcessLidarDataSlot()));
    //cTimer的槽
    QObject::connect(cTimers,SIGNAL(timeout()),this,SLOT(NextcTimerSlot()));

    QObject::connect(ui->pushButton,SIGNAL(clicked()),this,SLOT(PushButton1Slot()));
    QObject::connect(ui->pushButton_2,SIGNAL(clicked()),this,SLOT(PushButton2Slot()));
    QObject::connect(ui->pushButton_3,SIGNAL(clicked()),this,SLOT(PushButton3Slot()));
    QObject::connect(ui->pushButton_4,SIGNAL(clicked()),this,SLOT(PushButton4Slot()));
    QObject::connect(ui->pushButton_5,SIGNAL(clicked()),this,SLOT(PushButton5Slot()));
    QObject::connect(ui->pushButton_6,SIGNAL(clicked()),this,SLOT(PushButton6Slot()));
    QObject::connect(ui->pushButton_7,SIGNAL(clicked()),this,SLOT(PushButton7Slot()));
}

//接收数据的槽
void MainWindow::revDataSlot()
{
    QString rData = tcpSocket->readAll();

    qDebug()<<"rData"<<rData;

    if(!rData.isEmpty())
    {
        qLidarData.append(rData);
        if(qLidarData.contains("sSN"))
        {
            QString str=qLidarData.split("sSN").at(0);
            qLidarItems.append(str);
            emit ProcessLidarDataSlot();
            qLidarData = qLidarData.right(qLidarData.length()-qLidarData.indexOf("sSN")-1);
        }
    }
}
//处理激光数据的槽
void MainWindow::ProcessLidarDataSlot()
{
    foreach(QString ba, qLidarItems)
    {
        if(!ba.isEmpty())
        {
            if(ba.indexOf("LMDscandata")!=-1)
            {
                QStringList qsList=ba.split(' ');
                if(qsList.size()>715)
                {
                    //25  all
                    int size=HtoD(QString(qsList.at(25)).toLatin1());
                    for(int i=0;i<size;i++)
                    {
                        LidarInfo lidarN;
                        lidarN.angle=i*0.25;
                        lidarN.measureDis=HtoD(QString(qsList.at(26+i)).toLatin1());
                        qLidarList.replace(i,lidarN);
                    }
                }
            }
        }
        //移除数据
        qLidarItems.removeFirst();
    }
}
//时间处理槽
void MainWindow::NextcTimerSlot()
{
    if(qLidarList.size()>0)
    {
        int size=qLidarList.size();

        for(int i=0;i<size;i++)
        {
            LidarInfo lidarinfo=  qLidarList.at(i);  

            if(lidarinfo.angle==0)
            {
               ui->label_0000->setText(QString::number(lidarinfo.measureDis));
            }

            if(lidarinfo.angle==22.5)
            {
               ui->label_0225->setText(QString::number(lidarinfo.measureDis));
            }

            if(lidarinfo.angle==45)
            {
               ui->label_0450->setText(QString::number(lidarinfo.measureDis));
            }

            if(lidarinfo.angle==67.5)
            {
               ui->label_0675->setText(QString::number(lidarinfo.measureDis));
            }

            if(lidarinfo.angle==90)
            {
               ui->label_0900->setText(QString::number(lidarinfo.measureDis));
            }

            if(lidarinfo.angle==112.5)
            {
               ui->label_1125->setText(QString::number(lidarinfo.measureDis));
            }

            if(lidarinfo.angle==135)
            {
               ui->label_1350->setText(QString::number(lidarinfo.measureDis));
            }

            if(lidarinfo.angle==157.5)
            {
               ui->label_1575->setText(QString::number(lidarinfo.measureDis));
            }

            if(lidarinfo.angle==180)
            {
               ui->label_1800->setText(QString::number(lidarinfo.measureDis));
            }
        }
    }
}

void MainWindow::displayErrorSlot(QAbstractSocket::SocketError)
{
    qDebug()<<"err-->"<<tcpSocket->errorString();
       tcpSocket->close();
}

void MainWindow::PushButtonConSlot()
{
    QString sData="sEN LMDscandata 1";
    //QString sData="sRN LMDscandata";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
    if(!cTimers->isActive())
       cTimers->start();
}

void MainWindow::PushButtonDisConSlot()
{
    QString sData="sEN LMDscandata 0";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
    if(cTimers->isActive())
        cTimers->stop();
}


void MainWindow::PushButton1Slot()
{
    QString sData="sMN SetAccessMode 03 F4724744";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
}

void MainWindow::PushButton2Slot()
{
    QString sData="sMN mLMPsetscancfg +2500 +1 +2500 -450000 +2250000";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
}

void MainWindow::PushButton3Slot()
{
    //sWN LMDscandatacfg 01 00 1 1 0 00 00 0 0 0 0 +1<ETX>
    QString sData="sWN LMDscandatacfg 01 00 0 0 0 00 00 0 0 0 0 +1";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
}

void MainWindow::PushButton4Slot()
{
    QString sData="sWN LMPoutputRange 1 9C4 0 1B7740";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
}

void MainWindow::PushButton5Slot()
{
    QString sData="sMN mEEwriteall";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
}

void MainWindow::PushButton6Slot()
{
    QString sData="sMN Run";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
}

void MainWindow::PushButton7Slot()
{
    QString sData="sRN LMDscandata";
    QByteArray ba=sData.toLatin1();
    this->tcpSocket->write(ba);
}

int MainWindow::HtoD(QByteArray s)
{
    int i;
    int n;
    n = 0;

    for(i = 0; i<s.size(); i++)
    {
        if(s.at(i) >= '0' && s.at(i) <= '9')
            n = n * 16 + s.at(i) - '0';

        if(s.at(i) >= 'a' && s.at(i) <= 'f')
            n = n * 16 + s.at(i) - 'a' + 10;

        if(s.at(i) >= 'A' && s.at(i) <= 'F')
            n = n * 16 + s.at(i) - 'A' + 10;
    }
    return n;
}


//补充代码

typedef struct _LidarInfo
{
  double angle;//角度
  int measureDis;//测量距离
}LidarInfo;

private:
    Ui::MainWindow *ui;
    QTcpSocket *tcpSocket;
    QString qLidarData;
    QList<QString> qLidarItems;//激光雷达集合
    QList<LidarInfo> qLidarList;

最终效果如下所示：