modularization/src/tool/PerceptionShow/mainwindow.cpp

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

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>

#include <iostream>
#include "cameraobjectarray.pb.h"

#ifdef arch_agx
#include "opencv2/imgcodecs/legacy/constants_c.h"
#include <opencv2/imgproc/types_c.h>

#endif


#define VIEW_WIDTH 2000
#define VIEW_HEIGHT 2000
//ConsumerProducerQueue<cv::Mat> *imageBuffer = new ConsumerProducerQueue<cv::Mat>(1, true);
std::string gstrperceptitle[] = {"image00","lidar","radar","lidarobj","fusion","imgdet"};
//std::string gstrperceptitle[] = {"image00","lidar","radar","fusion","lidarobj","imgdet"};
//std::string gstrmemname[] = {"image00","lidarpc_center","radar","radafds","fusion"};
std::string gstrmemname[] = {"image00","lidarpc_center","radar","fusion"};
MainWindow * gw;
//iv::vision::cameraobjectarray cameraobjvec;
//void Listencamera(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
//{
//    /*
//    iv::vision::cameraobjectarray cameraobjvec;
//    std::string in;
//    in.append(strdata,nSize);
//    cameraobjvec.ParseFromString(in);
//    qDebug("obj size is %d ",cameraobjvec.obj_size());
//    */

//    //iv::vision::cameraobjectarray cameraobjvec;
////    if(false == cameraobjvec.ParseFromArray(strdata, nSize))
////    {
////        std::cout<<"ccccccccccc Listencamera fail."<<std::endl;
////        return;
////    }
////    else{
////        //std::cout<<"camera byte size:  "<<cameraobjvec.ByteSize()<<std::endl;
////    }

//    /*
//    std::cout<<"camera obj size:  "<<cameraobjvec.obj_size()<<std::endl;
//    for(int i=0; i<cameraobjvec.obj_size(); i++)
//    {
//        std::cout<<"i: "<<i<<std::endl;
//        std::cout<<"output x: "<<cameraobjvec.obj(i).outpointx()<<std::endl;
//        std::cout<<"putput y: "<<cameraobjvec.obj(i).outpointy()<<std::endl;
//    }
//    */

//}

void Listenpics0(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1000)return;
    iv::vision::rawpic pic;
    if(false == pic.ParseFromArray(strdata,nSize))
    {
        std::cout<<"picview Listenpic fail."<<std::endl;
        return;
    }
    gw->UpdatePic(pic);

}

void Listenpicdet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1000)return;
    iv::vision::rawpic pic;
    if(false == pic.ParseFromArray(strdata,nSize))
    {
        std::cout<<"picview Listenpic fail."<<std::endl;
        return;
    }
    gw->UpdatePicDet(pic);

}

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);


}

void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    if(nSize<1)return;
    iv::radar::radarobjectarray xobj;


    if(false == xobj.ParseFromArray(strdata,nSize))
    {
        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
        return;
    }
    std::cout<<"----------------"<<xobj.obj_size()<<std::endl;
//    xt = QDateTime::currentMSecsSinceEpoch();
//    qDebug("latence = %ld ",xt-pic.time());
    gw->UpdateRADAR(xobj);


}

void Listenlidarcnndectect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

//    std::vector<iv::lidar::object>  lidarobjvec;
//    strtolidarobj(lidarobjvec,strdata,nSize);

    iv::lidar::objectarray lidarobjvec;
    std::string in;
    in.append(strdata,nSize);
    lidarobjvec.ParseFromString(in);
    qDebug("obj size is %d ",lidarobjvec.obj_size());
//    if(lidarobjvec.obj_size() > 1)
//    {
//        qDebug("type is %s",lidarobjvec.obj(0).type_name().data());
//    }
    gw->UpdateLidarObj(lidarobjvec);
}

void ListenFusion(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)return;
    iv::fusion::fusionobjectarray xfusionobjarray;
    if(false == xfusionobjarray.ParseFromArray(strdata,nSize))
    {
        std::cout<<"PecerptionShow Listenenfusion fail."<<std::endl;
        return;
    }
    std::cout<<xfusionobjarray.ByteSize()<<"fusion-----------"<<std::endl;
//    xt = QDateTime::currentMSecsSinceEpoch();
    gw->UpdateFusionObj(xfusionobjarray);
}

void ListenCamera_f(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)return;
    iv::vision::cameraobjectarray xCameraobjarray;
    if(false == xCameraobjarray.ParseFromArray(strdata,nSize))
    {
        std::cout<<"PecerptionShow Listenenfusion fail."<<std::endl;
        return;
    }
    std::cout<<xCameraobjarray.ByteSize()<<"camera-----------"<<std::endl;
//    xt = QDateTime::currentMSecsSinceEpoch();
//    gw->UpdateCamerObj(xCameraobjarray);
}

void ListenRadar_f(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)return;
    iv::radar::radarobjectarray xRadarobjarray;
    if(false == xRadarobjarray.ParseFromArray(strdata,nSize))
    {
        std::cout<<"PecerptionShow Listenenfusion fail."<<std::endl;
        return;
    }
    std::cout<<xRadarobjarray.ByteSize()<<"radar---------"<<std::endl;
//    xt = QDateTime::currentMSecsSinceEpoch();
//    gw->UpdateRadarObj(xRadarobjarray);
}

void ListenLidar_f(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1)return;
    iv::lidar::objectarray xLidarobjarray;
    if(false == xLidarobjarray.ParseFromArray(strdata,nSize))
    {

        std::cout<<"PecerptionShow Listenenfusion fail."<<std::endl;
        return;
    }
    std::cout<<xLidarobjarray.ByteSize()<<"lidar--------"<<std::endl;
//    xt = QDateTime::currentMSecsSinceEpoch();
//    gw->UpdateRadarObj(xRadarobjarray);
}


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

    gw = this;

    mpLidarView = new IVLidarObjView();
    mpRadarView = new IVRadarView();
    mpPicView = new IVPicView();
    mpPCDView = new IVPCDView();
    mpFusionView = new IVFusionView();
    mpPicDetView = new IVPicView();

    mpIVView[0] = mpPicView;
    mpIVView[1] = mpPCDView;
    mpIVView[2] = mpRadarView;
    mpIVView[3] = mpLidarView;
    mpIVView[4] = mpFusionView;
    mpIVView[5] = mpPicDetView;

    int i;
    for(i=0;i<MAX_SEL;i++)mpIVView[i]->start();
//    mpLidarView->start();
//    mpRadarView->start();
//    mpPicView->start();
//    mpPCDView->start();



    CreateView();

    void * pa;

    pa = iv::modulecomm::RegisterRecv("image00",Listenpics0);

//    pa = iv::modulecomm::RegisterRecv("lidarpc_center",ListenPointCloud);
    pa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);

    pa = iv::modulecomm::RegisterRecv("esr_front",Listenesrfront);

//   pa = iv::modulecomm::RegisterRecv("can0",Listenesrfront);

    pa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndectect);

    pa = iv::modulecomm::RegisterRecv("fusion",ListenFusion);

    pa = iv::modulecomm::RegisterRecv("f_camera",ListenCamera_f);

    pa = iv::modulecomm::RegisterRecv("f_radar",ListenRadar_f);

    pa = iv::modulecomm::RegisterRecv("f_lidar",ListenLidar_f);

    pa = iv::modulecomm::RegisterRecv("camera_detect",Listenpicdet);

    QTimer * timer = new QTimer(this);
    connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
    timer->start(10);

    setWindowTitle("Perception Show");
}

MainWindow::~MainWindow()
{
    QTime xTime;
    xTime.start();

    int i;
    for(i=0;i<MAX_SEL;i++)mpIVView[i]->requestInterruption();

    while(xTime.elapsed()<1000)
    {
        int j;
        int nStop = 0;
        for(j=0;j<MAX_SEL;j++)
        {
            if(mpIVView[j]->isFinished())nStop++;
        }
        if(nStop == MAX_SEL)break;
    }
    delete ui;
}

void MainWindow::CreateView()
{
    int i;
    for(i=0;i<4;i++)
    {
        myview[i] = new MyView(ui->centralWidget);
        myview[i]->setObjectName(QStringLiteral("graphicsView"));
        myview[i]->setGeometry(QRect(30, 30, 600, 600));

        myview[i]->setCacheMode(myview[i]->CacheBackground);

        scene[i] = new QGraphicsScene;
    }

    mpGroup = new QGroupBox(ui->centralWidget);
    CreateRightView(mpGroup);
}

void MainWindow::CreateRightView(QGroupBox *pGroup)
{
    int i;
    for(i=0;i<4;i++)
    {
        QString strname;
        strname = QString("view");
        strname = strname + QString::number(i);
        QLabel * pLabel = new QLabel(pGroup);
        pLabel->setText(strname);
        pLabel->setGeometry(10,30+i*50,90,30);
        mcbviewsel[i] = new QComboBox(pGroup);
        mcbviewsel[i]->setGeometry(100,30+i*50,150,30);
        int j;
        for(j=0;j<MAX_SEL;j++)
        {
            mcbviewsel[i]->addItem(gstrperceptitle[j].data());

        }
        mnSel[i] = i;
        mcbviewsel[i]->setCurrentIndex(mnSel[i]);
        connect(mcbviewsel[i],SIGNAL(currentIndexChanged(int)),this,SLOT(onComboSel(int)));
    }

    int nXPos = 10;
    int nYPos = 230;
    QSlider * pSL = new QSlider(Qt::Horizontal,pGroup);
    pSL->setGeometry(nXPos,nYPos,200,30);
    mpSlider = pSL;
    pSL->setMinimum(1);
    pSL->setMaximum(199);
    pSL->setValue(100);

   nXPos = nXPos + 220;
   QPushButton * pPB = new QPushButton(pGroup);
   pPB->setGeometry(nXPos,nYPos,80,30);
   pPB->setText("Set");
   connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onChangeWin()));

   nXPos = 10;
   nYPos = nYPos + 50;
   QCheckBox * pCheck = new QCheckBox(pGroup);
   pCheck->setText("Show Lidar");
   pCheck->setGeometry(nXPos,nYPos,150,30);
   mpCheck[0] = pCheck;

   nXPos = 10;
   nYPos = nYPos + 50;
   pCheck = new QCheckBox(pGroup);
   pCheck->setText("Show Radar");
   pCheck->setGeometry(nXPos,nYPos,150,30);
   mpCheck[1] = pCheck;

   nXPos = 10;
   nYPos = nYPos + 50;
   pCheck = new QCheckBox(pGroup);
   pCheck->setText("Show Camera");
   pCheck->setGeometry(nXPos,nYPos,150,30);
   mpCheck[2] = pCheck;

   for(i=0;i<3;i++)mpCheck[i]->setChecked(false);

//    connect(pSL,SIGNAL(valueChanged())),this,SLOT(onChangeWind());
}

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::onChangeWin()
{
    QSize sizemain = ui->centralWidget->size();

    mnFix = mpSlider->value();

    AdjustWPos(sizemain);
}

void MainWindow::AdjustWPos(QSize sizemain)
{
    int i;
    int width = (sizemain.width() - 300)/2;
    if(width < 0)width = 20;
    int hgt = (sizemain.height()-20)/2;
//    myview[0]->setGeometry(0,10,width,hgt );
//    myview[1]->setGeometry(width+10,10,width,hgt );
//    myview[2]->setGeometry(0,hgt+20,width,hgt );
//    myview[3]->setGeometry(width+10,hgt+20,width,hgt );

    int fx = mnFix;
    if((fx>199)||(fx<1))fx = 100;
    myview[0]->setGeometry(0,10,width*fx/100,hgt*fx/100 );
    myview[1]->setGeometry(width*fx/100+10,10,width*(200-fx)/100,hgt*fx/100 );
    myview[2]->setGeometry(0,hgt*fx/100+20,width*fx/100,hgt*(200-fx)/100 );
    myview[3]->setGeometry(width*fx/100+10,hgt*fx/100+20,width*(200-fx)/100,hgt*(200-fx)/100 );

    mpGroup->setGeometry(width*2 + 20,10,sizemain.width()-width*2-20,sizemain.height()-20);

 //   mgplidar->setGeometry(sizemain.width()-280,30,260,200);
}

void MainWindow::onComboSel(int index)
{
    QComboBox * pcb = (QComboBox * )sender();

    int i = -1;
    for(i=0;i<4;i++)
    {
        if(pcb == mcbviewsel[i])break;
    }

    if(i == -1)return;

    mnSel[i] = pcb->currentIndex();

}

void MainWindow::UpdatePic(iv::vision::rawpic pic)
{
    mpPicView->SetPic(pic);
}

void MainWindow::UpdatePicDet(iv::vision::rawpic pic)
{
    mpPicDetView->SetPic(pic);
}

void MainWindow::paintEvent(QPaintEvent *)
{
    int i;
    for(i=0;i<4;i++)SelectView(i);
}

void MainWindow::SelectView(int viewindex)
{
    switch (mnSel[viewindex]) {
    case 0:
        painterPic(viewindex);
        break;
    case 1:
        painterPCD(viewindex);
        break;
    case 2:
        painterRADAR(viewindex);
        break;
    case 3:
        painterLidarObj(viewindex);
        break;
    case 4:
        painterFusion(viewindex);
        break;
    case 5:
        painterPicDet(viewindex);
        break;
    default:
        break;
    }
}

void MainWindow::painterPic(int viewindex)
{

    if(!mpPicView->IsHaveNew())
    {
        return;
    }

    QImage image = mpPicView->GetImage();

    scene[viewindex]->clear();
    scene[viewindex]->addPixmap(QPixmap::fromImage(image));
    myview[viewindex]->setScene(scene[viewindex]);
    myview[viewindex]->show();

}

void MainWindow::painterPicDet(int viewindex)
{

    if(!mpPicDetView->IsHaveNew())
    {
        return;
    }

    QImage image = mpPicDetView->GetImage();

    scene[viewindex]->clear();
    scene[viewindex]->addPixmap(QPixmap::fromImage(image));
    myview[viewindex]->setScene(scene[viewindex]);
    myview[viewindex]->show();

}

void MainWindow::painterPCD(int viewindex)
{
    if(!mpPCDView->IsHaveNew())
    {
        return;
    }
    QImage image = mpPCDView->GetImage();
    scene[viewindex]->clear();
    scene[viewindex]->addPixmap(QPixmap::fromImage(image));
    myview[viewindex]->setScene(scene[viewindex]);
    myview[viewindex]->show();

}

void MainWindow::painterRADAR(int viewindex)
{
    if(!mpRadarView->IsHaveNew())
    {
        return;
    }
    QImage image = mpRadarView->GetImage();
    scene[viewindex]->clear();
    scene[viewindex]->addPixmap(QPixmap::fromImage(image));
    myview[viewindex]->setScene(scene[viewindex]);
    myview[viewindex]->show();
}

void MainWindow::painterLidarObj(int viewindex)
{

    if(!mpLidarView->IsHaveNew())
    {
        return;
    }
    QImage image = mpLidarView->GetImage();
    scene[viewindex]->clear();
    scene[viewindex]->addPixmap(QPixmap::fromImage(image));
    myview[viewindex]->setScene(scene[viewindex]);
    myview[viewindex]->show();

}

void MainWindow::painterFusion(int viewindex)
{

    if(!mpFusionView->IsHaveNew())
    {
        return;
    }
    QImage image = mpFusionView->GetImage();
    scene[viewindex]->clear();
    scene[viewindex]->addPixmap(QPixmap::fromImage(image));
    myview[viewindex]->setScene(scene[viewindex]);
    myview[viewindex]->show();

}


void MainWindow::onTimer()
{
    update();
}

void MainWindow::UpdatePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr pc)
{
    mpPCDView->SetPointCloud(pc);
}

void MainWindow::UpdateRADAR(iv::radar::radarobjectarray radarobj)
{
    mpRadarView->SetRADAR(radarobj);
}

void MainWindow::UpdateLidarObj(iv::lidar::objectarray  lidarobjvec)
{
    mpLidarView->SetLidarObj(lidarobjvec);
}

void MainWindow::UpdateFusionObj(iv::fusion::fusionobjectarray xfusionobjarray)
{
    mpFusionView->SetFusionObj(xfusionobjarray);
}