modularization/src/tool/bqev_pcdview/mainwindow.cpp

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

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

MainWindow * gw;


char gstr_memname[256];


double GetThresh(double x,double y)
{
    double dis = sqrt(x*x + y*y);
    if(dis<20)return 0.4;
    if(dis<40)return 0.6;
    if(dis<60)return 0.8;
 //   if((dis>=10)&&(dis<=30))return 0.5;
    return 1.0;
}

inline double CalcDis(double x0,double y0,double x1,double y1)
{
    return sqrt((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0));
}

int CountNearPoint(std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs,std::vector<iv::ObstacleBasic>::iterator its,double thresh)
{
    std::vector<iv::ObstacleBasic>::iterator it = lidar_obs->begin();
    iv::ObstacleBasic xp = *it;

    int nRtn = 1;
    while(it!= lidar_obs->end())
    {
        if(its != it)
        {
            iv::ObstacleBasic xt = *it;
            if(CalcDis(xp.nomal_x,xp.nomal_y,(*it).nomal_x,(*it).nomal_y)<=thresh)nRtn++;

        }
        else
        {
  //          std::cout<<"equal"<<std::endl;
        }
        it++;
    }
 //   std::cout<<"nRtn = "<<nRtn<<std::endl;
    return nRtn;
}



void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize <=16)return;
    unsigned int * pHeadSize = (unsigned int *)strdata;
    if(*pHeadSize > nSize)
    {
        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());
    int nNameSize;
    nNameSize = *pHeadSize - 4-4-8;
    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
    point_cloud->header.frame_id = strName;
    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
    int i;
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZI xp;
        xp.x = p->x;
        xp.y = p->y;
        xp.z = p->z;
        xp.intensity = p->intensity;
        point_cloud->push_back(xp);
        p++;

  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
    }

 //   return;


    gw->UpdatePointCloud(point_cloud);
 //   DBSCAN_PC(point_cloud);


}




MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    gw = this;

    mpcdview = new IVPCDView();
    mpcdview->start();

    mTime.start();
    mnlidarcount = 0;
    mnonepcd = 0;
    mbSaveOne = false;
    mbSave = false;
    mnsave = 0;

    mbOpen = false;

    mstrSavePath = QCoreApplication::applicationDirPath();

    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloudtemp(
                new pcl::PointCloud<pcl::PointXYZI>());

    mpoint_cloud = point_cloudtemp;

    myview = new MyView(this);
    myview->setObjectName(QStringLiteral("graphicsView"));
    myview->setGeometry(QRect(30, 30, 600, 600));

    image = new QImage(2000, 2000, QImage::Format_RGB32);//画布的初始化大小设为300*300，使用32位颜色
    myview->setCacheMode(myview->CacheBackground);

    painter = new QPainter(image);
    painter->end();

    scene = new QGraphicsScene;

    CreateView();

//    myview->scale(0.3,0.3);
//    myview->show();

    mpa = iv::modulecomm::RegisterRecv(gstr_memname,ListenPointCloud);

    connect(&mTimer,SIGNAL(timeout()),this,SLOT(onTimer()));
#ifdef Q_OS_WIN32
    mTimer.start(1000);
#else
    mTimer.start(10);
#endif

    setWindowTitle("bqev-pcdview");

}

MainWindow::~MainWindow()
{
    QTime time;
    time.start();
    mpcdview->requestInterruption();
    while(time.elapsed()<1000)
    {
        if(mpcdview->isFinished())
        {
            break;
        }
    }
    delete ui;
}



void MainWindow::UpdatePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
    if(mbSaveOne)
    {
        QString strpath = mstrSavePath;
        strpath = strpath+"/";
        strpath = strpath + QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz") + "-1.pcd";

        if(0 == pcl::io::savePCDFile(strpath.toLatin1().data(),*pc))
        {
            mnonepcd++;

            mbSaveOne = false;
        }
   //     pcl::io::loadPCDFile<pcl::PointXYZRGBA>("table_scene_lms400.pcd", *cloud
    }

    if(mbSave)
    {
        QString strpath = mstrSavePath;
        strpath = strpath+"/";
        strpath = strpath + QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz") + ".pcd";

        if(0 == pcl::io::savePCDFile(strpath.toLatin1().data(),*pc))
        {
            mnsave++;

        }
    }

    mpcdview->SetPointCloud(pc);
//    mMutex.lock();
//    pc.swap(mpoint_cloud);
//    mMutex.unlock();
//    mnlidarcount++;




//    if(!pc->empty())update();
}


void MainWindow::on_checkBox_clicked()
{

}


void MainWindow::onTimer()
{
    update();
    mpLE_lidarcount->setText(QString::number(mnlidarcount));
    mpLE_savepcd->setText(QString::number(mnsave));
    mpLE_onepcd->setText(QString::number(mnonepcd));


}

//刷新
void MainWindow::paintEvent(QPaintEvent *)
{

//    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
//                new pcl::PointCloud<pcl::PointXYZI>());


//    int nz = 0;

//    mMutex.lock();
//    point_cloud.swap(mpoint_cloud);
//    mMutex.unlock();


//    if(point_cloud->width <1)return;


//    painter->begin(image);

//    image->fill(QColor(255, 255, 255));//对画布进行填充
////    std::vector<iv::GPSData> navigation_data = brain->navigation_data;
//    painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式，好看一点

//    painter->translate(1000,1000);

//    painter->setPen(Qt::black);

//    painter->drawLine(-1000,0,1000,0);
//    painter->drawLine(0,-1000,0,1000);

//    int i;
//    for(i=-9;i<=9;i++)
//    {
//        painter->drawLine(i*100,0,i*100,-20);
////        painter->drawText(i*100-20,-50,40,20,Qt::AlignHCenter | Qt::AlignTop,QString::number(i*10));
//    }

////     std::cout<<"enter paint."<<std::endl;

//    painter->save();
//    painter->rotate(90);
//    for(i=-9;i<=9;i++)
//    {
//        painter->drawLine(i*100,0,i*100,-20);
////        painter->drawText(i*100-20,-50,40,20,Qt::AlignHCenter | Qt::AlignTop,QString::number(i*(-10)));
//    }
//    painter->restore();


//    if(point_cloud != 0)
//    {
//    for(i=0;i<point_cloud->width;i++)
//    {
//        pcl::PointXYZI pxyzi = point_cloud->at(i);
//        int x,y;

// //       std::cout<<" x is "<<pxyzi.x<<" y is "<<pxyzi.y<<std::endl;
//        x =(int) (pxyzi.x*10.0);
//        y = (int)(pxyzi.y*(-10.0));
//        if((fabs(x)<5) &&(fabs(y)<5))
//        {
//            nz++;
//        }
////        std::cout<<" x is "<<x<<" y is "<<y<<std::endl;
//        int inten = pxyzi.intensity;
//        inten = inten/2;
//        QColor color;
//        color.setHsl(inten,255,120);
//        painter->setPen(color);
//        painter->drawPoint(x,y);
//    }
//    }



//    qDebug("nz = %d",nz);

////    QColor x;
////    x.setHsl(200,255,120);
////    painter->setPen(x);
////    painter->drawText(-300,-300,40,40,Qt::AlignHCenter | Qt::AlignTop,"hello");
//    painter->end();

    QTime xTime;
    if(!mpcdview->IsHaveNew())
    {
 //       qDebug("no pcd");
        return;
    }
    xTime.start();
    QImage imagex = mpcdview->GetImage();
    scene->clear();
    scene->addPixmap(QPixmap::fromImage(imagex));
    myview->setScene(scene);
    myview->show();

    qDebug("show time is %d ",xTime.elapsed());



}

void MainWindow::resizeEvent(QResizeEvent *event)
{
    qDebug("resize");
    QSize sizemain = ui->centralWidget->size();
    qDebug("size x = %d y=%d",sizemain.width(),sizemain.height());

    AdjustWPos(sizemain);

}

void MainWindow::AdjustWPos(QSize sizemain)
{
    myview->setGeometry(0,30,sizemain.width()-300,sizemain.height());
    mgplidar->setGeometry(sizemain.width()-280,30,260,200);
}

void MainWindow::CreateStatusView(QGridLayout *gl)
{
    gl->setSpacing(10);
    int iRow = 0;

    QLabel * pl = new QLabel(this);
    pl->setText("Count");
    pl->setFixedWidth(80);

    QLineEdit * ple = new QLineEdit(this);
    ple->setFixedWidth(100);

    gl->addWidget(pl,iRow,0);
    gl->addWidget(ple,iRow,1);
    iRow++;

    mpLE_lidarcount = ple;

    QPushButton * pb2 = new QPushButton(this);
    pb2->setText("Save One");
    pb2->setFixedWidth(80);

    QLineEdit * ple2 = new QLineEdit(this);
    ple2->setFixedWidth(100);

    gl->addWidget(pb2,iRow,0);
    gl->addWidget(ple2,iRow,1);
    iRow++;

    mpPB_saveonepcd = pb2;
    mpLE_onepcd = ple2;
    connect(mpPB_saveonepcd,SIGNAL(clicked(bool)),this,SLOT(onSaveOnePCD()));

    QPushButton * pb3 = new QPushButton(this);
    pb3->setText("Start Save");
    pb3->setFixedWidth(80);

    QLineEdit * ple3 = new QLineEdit(this);
    ple3->setFixedWidth(100);

    gl->addWidget(pb3,iRow,0);
    gl->addWidget(ple3,iRow,1);
    iRow++;

    mpPB_savepcd = pb3;
    mpLE_savepcd = ple3;
    connect(mpPB_savepcd,SIGNAL(clicked(bool)),this,SLOT(onSavePCD()));

    QPushButton * pb4 = new QPushButton(this);
    pb4->setText("Set Folder");
    pb4->setFixedWidth(80);

    QLineEdit * ple4 = new QLineEdit(this);
    ple4->setFixedWidth(100);

    gl->addWidget(pb4,iRow,0);
    gl->addWidget(ple4,iRow,1);
    iRow++;

    mpLE_savefolder = ple4;
    connect(pb4,SIGNAL(clicked(bool)),this,SLOT(onSelectFolder()));

    QSpacerItem * xpsi2 = new QSpacerItem(200,1000,QSizePolicy::Maximum);

    gl->addItem(xpsi2,iRow,0);
}

void MainWindow::CreateView()
{
    QGroupBox * gp1 = new QGroupBox(ui->centralWidget);
    gp1->setTitle(QStringLiteral("LIDAR Status"));
    gp1->setGeometry(QRect(10,100,400,600));
    QGridLayout * gll1 = new QGridLayout(ui->centralWidget);
    gp1->setLayout(gll1);

    CreateStatusView(gll1);

    mgplidar = gp1;
}

void MainWindow::onSaveOnePCD()
{
    mbSaveOne = true;
}

void MainWindow::onSavePCD()
{
    if(mbSave)
    {
        mbSave = false;
        mpPB_savepcd->setText("Start Save");
    }
    else
    {
        mbSave = true;
        mpPB_savepcd->setText("Stop Save");
    }
}

void MainWindow::onSelectFolder()
{
    QString str = QFileDialog::getExistingDirectory(this, tr("Set PCD Save Directory"), mstrSavePath, QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
    if(!str.isEmpty())
    {
        mstrSavePath = str;
        mpLE_savefolder->setText(mstrSavePath);

    }
    else
    {
        qDebug("not change dir.");
    }
}