modularization/src/tool/view_radar4d_3d/mainwindow.cpp

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

#include <iostream>

#include <QFileDialog>
#include <QTimer>

#include <functional>


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

    ui->actionNoClassification->setChecked(true);
    ui->actionNoise->setChecked(true);
    ui->actionGround->setChecked(true);
    ui->actionTraversableUnder->setChecked(true);
    ui->actionObstacle->setChecked(true);
    ui->actionInvalid->setChecked(true);

    ui->actionCar->setChecked(true);
    ui->actionTruck->setChecked(true);
    ui->actionMotorcycle->setChecked(true);
    ui->actionBicycle->setChecked(true);
    ui->actionPedestrian->setChecked(true);
    ui->actionAnimal->setChecked(true);
    ui->actionHazard->setChecked(true);
    ui->actionUnknown->setChecked(true);

    vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
    vtkSmartPointer<vtkGenericOpenGLRenderWindow> renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
    renderWindow->AddRenderer(renderer);
    view.reset(new pcl::visualization::PCLVisualizer(renderer,renderWindow,"viewer",false));



#if VTK890
    view->setupInteractor(ui->vtk->interactor(),ui->vtk->renderWindow());
#else
    view->setupInteractor(ui->vtk->GetInteractor(),ui->vtk->GetRenderWindow());
#endif

#if VTK890
  this->ui->vtk->setRenderWindow(renderWindow);
#else
  this->ui->vtk->SetRenderWindow(renderWindow);
#endif

    DrawAxis();

    // Create a sphere.

    int i;
    for(i=0;i<NUM_DET_MAX;i++){
      detSource[i]->SetBounds(-0.001,-0.001,-0.001,0.001,0.001,0.001);
      detSource[i]->Update();

      detmapper[i]->SetInputData(detSource[i]->GetOutput());

      // Create an actor.
      vtkNew<vtkActor> actor;
      vtkNew<vtkNamedColors> colors;
      actor->GetProperty()->SetColor(colors->GetColor3d("Yellow").GetData());
      actor->SetMapper(detmapper[i]);

      renderer->AddActor(actor);
     }

    for(i=0;i<NUM_OBJ_MAX;i++){
      sphereSource[i]->SetCenter(0,0,0);
      sphereSource[i]->SetRadius(0.001);
      sphereSource[i]->Update();

      mapper[i]->SetInputData(sphereSource[i]->GetOutput());

      // Create an actor.
      vtkNew<vtkActor> actor;
      vtkNew<vtkNamedColors> colors;
      actor->GetProperty()->SetColor(colors->GetColor3d("Red").GetData());
      actor->SetMapper(mapper[i]);

      renderer->AddActor(actor);
     }

    QTimer * timer1 = new QTimer();
    connect(timer1,SIGNAL(timeout()),this,SLOT(onTimer()));
//    timer1->start(100);

    connect(this,SIGNAL(detupdate()),this,SLOT(onDetUpdate()));

#ifdef USE_MODULECOMM
    ModuleFun fundetect = std::bind(&MainWindow::UpdateDet,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpadet = iv::modulecomm::RegisterRecvPlus("radar4ddet",fundetect);
    ModuleFun funojbect = std::bind(&MainWindow::UpdateObj,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpaobj = iv::modulecomm::RegisterRecvPlus("radar4dobj",funojbect);
#else
    mprecv = new ars548recv();
    mpthreaddet =  new std::thread(&MainWindow::threaddet,this);
    mpthreadobj =  new std::thread(&MainWindow::threadobj,this);
#endif

    setWindowTitle("radar4d 3d Viewer");

}

MainWindow::~MainWindow()
{
    mbThreadRun = false;
#ifdef USE_MODULECOMM
    iv::modulecomm::Unregister(mpaobj);
    iv::modulecomm::Unregister(mpadet);
#else
    mpthreadobj->join();
    mpthreaddet->join();
    delete mprecv;
#endif
    delete ui;
}

void MainWindow::DrawAxis()
{

    bool bGrid = false;

    pcl::PointXYZ p1(-300,0,0);
    pcl::PointXYZ p2(300,0,0);
    view->addLine(p1,p2,1.0f,1.0f,1.0f,"axis_x");

    p1.x = 0;p1.y = -300;
    p2.x=  0;p2.y = 300;
    view->addLine(p1,p2,1.0f,1.0f,1.0f,"axis_y");

    int i;

    if(bGrid)
    {
        for(i=-30;i<=30;i++)
        {
            if(i==0)continue;
            p1.x = -300;p1.y = i*10;
            p2.x = 300; p2.y = i*10;
            char strname[256];
            snprintf(strname,256,"axisgx_%d",i);
            view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);


            p1.x = i*10;p1.y = -300;
            p2.x = i*10; p2.y = 300;
            snprintf(strname,256,"axisgy_%d",i);
            view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
        }
    }
    else
    {
        for(i=-30;i<=30;i++)
        {
            if(i==0)continue;
            p1.x = 0;p1.y = i*10;
            p2.x = 1; p2.y = i*10;
            char strname[256];
            snprintf(strname,256,"axisgx_%d",i);
            view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);


            p1.x = i*10;p1.y = 0;
            p2.x = i*10; p2.y = 1;
            snprintf(strname,256,"axisgy_%d",i);
            view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
        }
    }




    for(i=-6;i<=6;i++)
    {
        char strname[256];
        char strtext[256];
        double orientation[3];
        orientation[0] = 1.0;orientation[1] = 0.0;orientation[2] = 0.0;
        p1.x = i*50;p1.y = 1;
        snprintf(strname,256,"textx_%d",i);
        snprintf(strtext,256,"%d",i*50);
        if(i%2 == 0)
            view->addText3D(strtext,p1,orientation,1.0,1.0,1.0,1.0,strname);
        else
            view->addText3D(strtext,p1,orientation,0.5,1.0,1.0,1.0,strname);

        if(i== 0)continue;

        orientation[0] = 1.0;orientation[1] =0.0;orientation[2] = 0.0;
        p1.x = 1;p1.y = i*50+1;
        snprintf(strname,256,"texty_%d",i);
        snprintf(strtext,256,"%d",i*50);
        if(i%2 == 0)
            view->addText3D(strtext,p1,orientation,1.0,1.0,1.0,1.0,strname);
        else
            view->addText3D(strtext,p1,orientation,0.5,1.0,1.0,1.0,strname);
    }

//    double base_size = 0.2;
//    double base_height = 0.2;
//    double sigx = 15;
//    double sigy = 15;
//    double sigz = 1.0;
//    view->addCube(sigx-base_size/2.0,sigx + base_size/2.0,sigy-base_size/2.0,sigy+base_size/2.0,sigz-base_height/2.0,sigz + base_height/2.0,1.0,0.0,0.0,"cube1");

    view->setCameraPosition(0,0,100,0,0,0,0,1,0);
    view->resetCamera();    //视角
//    view->addText("300",300,5,10,1.0f,0.0f,0.0f,"txt1");


}

void MainWindow::resizeEvent(QResizeEvent *event)
{

    (void)event;
    QSize sizemain = ui->centralwidget->size();

    ui->vtk->setGeometry(0,0,sizemain.width()-0,sizemain.height()-0);



}


void MainWindow::onTimer()
{

    int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
    static bool bSetCam = false;
    if(bSetCam == false)
    {
        bSetCam = true;
        view->setCameraPosition(0,30,130,0,30,0,0,1,0);
//        view->resetCamera();    //视角
    }
    static int nindex = 0;
    nindex++;
    if(nindex>1000)nindex = 0;
    double base_size = 0.5;
    double base_height = 0.5;
    double sigx = 15;
    double sigy = 15 + nindex * 0.01;
    double sigz = 1.0;
    (void)base_size;
    (void)base_height;
    (void)sigx;
    (void)sigz;
//    view->removeShape("cube1");
//    view->addCube(sigx-base_size/2.0,sigx + base_size/2.0,sigy-base_size/2.0,sigy+base_size/2.0,sigz-base_height/2.0,sigz + base_height/2.0,1.0,0.0,0.0,"cube1");



    int i;
    for(i=0;i<100;i++){
    sphereSource[i]->SetCenter(i, sigy, 0.0);
    sphereSource[i]->Update();
    }


    // Create a mapper.

//    mapper->SetInputData(sphereSource->GetOutput());

#if VTK890
    ui->vtk->renderWindow()->Render();
#else
    ui->vtk->GetRenderWindow()->Render();
#endif

    int64_t nend = std::chrono::system_clock::now().time_since_epoch().count();
    std::cout<<" update use : "<<(nend -nstart)/1e6<<std::endl;
}

void MainWindow::onDetUpdate()
{
    static int64_t nlastup = 0;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
    int64_t ndiff = (nnow - nlastup)/1e6;
    if(abs(ndiff)<10)
    {
//        nlastup = nnow;
        return;
    }

    bool bdetupdate = false;
    bool bobjupdate = false;

    int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
    iv::radar::radar4ddetectarray xdetarray;
    mmutexdet.lock();
    if(mbdetupdate){
        xdetarray.CopyFrom(mdetarray);
        mbdetupdate = false;
        bdetupdate = true;
    }
    mmutexdet.unlock();

    iv::radar::radar4dobjectarray xobjarray;
    mmutexobj.lock();
    if(mbobjupdate){
        xobjarray.CopyFrom(mobjarray);
        mbobjupdate = false;
        bobjupdate = true;
    }
    mmutexobj.unlock();

//    static int nskip = 0;
//    nskip++;
//    if(nskip <2)
//    {
//        return;
//    }
//    nskip = 0;



    static bool bSetCam = false;
    if(bSetCam == false)
    {
        bSetCam = true;
        view->setCameraPosition(0,30,130,0,30,0,0,1,0);
//        view->resetCamera();    //视角
    }

    int i;




    double base_size = 0.2;
    double obj_size = 0.5;
    double sigx = 15;
    double sigy = 15 ;
    double sigz = 1.0;
    int nsize;

    if(bdetupdate){
        nsize = static_cast<int>(xdetarray.mdet_size());

        if(nsize > NUM_DET_MAX){
            nsize = NUM_DET_MAX;
        }

        for(i=0;i<nsize;i++)
        {
            iv::radar::radar4ddetect * pdet = xdetarray.mutable_mdet(i);


            if((pdet->detection_radial_distance()<0.001) || (pdet->detection_classification() != iv::radar::radar4ddetect::Obstacle))
            {
                detSource[i]->SetBounds(1000.0,1000.1,-0.001,0.001,0.001,0.001);
                detSource[i]->Update();
                continue;
            }
            bool bshow = false;
            if(ui->actionAll->isChecked())bshow = true;
            if((ui->actionNoClassification->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::NoClassification))bshow = true;
            if((ui->actionNoise->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Noise))bshow = true;
            if((ui->actionGround->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Ground))bshow = true;
            if((ui->actionTraversableUnder->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::TraversableUnder))bshow = true;
            if((ui->actionObstacle->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Obstacle))bshow = true;
            if((ui->actionInvalid->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Invalid))bshow = true;
            if(bshow == true){
                sigx = pdet->detection_radial_distance() * cos(pdet->unaligned_detection_elevation_angle()) * cos(pdet->unaligned_detection_azimuth_angle() + M_PI/2.0);
                sigy = pdet->detection_radial_distance() * cos(pdet->unaligned_detection_elevation_angle()) * sin(pdet->unaligned_detection_azimuth_angle() + M_PI/2.0);
                sigz = pdet->detection_radial_distance() * sin(pdet->unaligned_detection_elevation_angle());
                detSource[i]->SetBounds(sigx-base_size/2.0,sigx+base_size/2.0,sigy - base_size/2.0,sigy + base_size/2.0, sigz - base_size/2.0,
                                        sigz + base_size/2.0);
                detSource[i]->Update();
            }
            else {
                detSource[i]->SetBounds(1000.0,1000.1,-0.001,0.001,0.001,0.001);
                detSource[i]->Update();
            }
        }
    }

    if(bobjupdate){
        nsize = static_cast<int>(xobjarray.mobj_size());

        if(nsize > NUM_OBJ_MAX){
            nsize = NUM_OBJ_MAX;
        }

        for(i=0;i<nsize;i++)
        {
            iv::radar::radar4dobject * pobj = xobjarray.mutable_mobj(i);

//            std::cout<<" i: "<<i<<" unkown: "<<pobj->u_classification_unknown()<<" car: "<<pobj->u_classification_car()
//                       <<" truck: "<<pobj->u_classification_truck()
//                         <<" motocycle: "<<pobj->u_classification_motorcycle()
//                           <<" bycycle: "<<pobj->u_classification_bicycle()
//                             <<" ped: "<<pobj->u_classification_pedestrian()
//                     <<" animal: "<<pobj->u_classification_animal()
//                     <<" hazard: "<<pobj->u_classification_hazard()
//                    <<"exist: "<<pobj->u_existence_probability()<<std::endl;
            sigx = pobj->u_position_y() * (-1);
            sigy = pobj->u_position_x();
            sigz = pobj->u_position_z();
            bool bShow = false;
            unsigned int nMinProb = 30;
            if(ui->actionObjectAll->isChecked())bShow = true;
            if((ui->actionCar->isChecked())&&(pobj->u_classification_car()>=nMinProb))bShow = true;
            if((ui->actionTruck->isChecked())&&(pobj->u_classification_truck()>=nMinProb))bShow = true;
            if((ui->actionMotorcycle->isChecked())&&(pobj->u_classification_motorcycle()>=nMinProb))bShow = true;
            if((ui->actionBicycle->isChecked())&&(pobj->u_classification_bicycle()>=nMinProb))bShow = true;
            if((ui->actionPedestrian->isChecked())&&(pobj->u_classification_pedestrian()>=nMinProb))bShow = true;
            if((ui->actionAnimal->isChecked())&&(pobj->u_classification_animal()>=nMinProb))bShow = true;
            if((ui->actionHazard->isChecked())&&(pobj->u_classification_hazard()>=nMinProb))bShow = true;
            if((ui->actionUnknown->isChecked())&&(pobj->u_classification_unknown()>=nMinProb))bShow = true;
            if((pobj->u_existence_probability() >0)&&bShow){

                sphereSource[i]->SetCenter(sigx,sigy,sigz);
                sphereSource[i]->SetRadius(obj_size/2.0);
            }
            else{
                sphereSource[i]->SetCenter(0.0,0.0,0.0);
                sphereSource[i]->SetRadius(0.001);
            }

            sphereSource[i]->Update();
        }
    }


//    std::cout<<"update det."<<std::endl;

#if VTK890
    ui->vtk->renderWindow()->Render();
#else
    ui->vtk->GetRenderWindow()->Render();
#endif

    int64_t nend = std::chrono::system_clock::now().time_since_epoch().count();

    int64_t nuse = nend- nstart;
    std::cout<<" draw use : "<<nuse/1e6<<std::endl;


    nlastup = std::chrono::system_clock::now().time_since_epoch().count();

}

void MainWindow::on_actionReset_Camera_triggered()
{
    view->setCameraPosition(0,0,100,0,0,0,0,1,0);
    view->resetCamera();    //视角
}


void MainWindow::on_actionFull_Screem_triggered()
{
    showFullScreen();
    ui->menubar->setVisible(false);
    ui->statusbar->setVisible(false);
    ui->toolBar->setVisible(false);
    mbFull = true;
}


void MainWindow::keyReleaseEvent(QKeyEvent *event)
{
    if((event->key() == Qt::Key_Escape)||(event->key() == Qt::Key_F11))
    {
        if(mbFull)
        {
            showNormal();
            ui->menubar->setVisible(true);
            ui->statusbar->setVisible(true);
            ui->toolBar->setVisible(true);
            mbFull = false;
        }
    }
}


#ifdef USE_MODULECOMM
void MainWindow::UpdateDet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    iv::radar::radar4ddetectarray xdetarray;
    if(xdetarray.ParseFromArray(strdata,(int)nSize))
    {
        mmutexdet.lock();
        mdetarray.CopyFrom(xdetarray);
        mbdetupdate = true;
        mmutexdet.unlock();
        emit detupdate();
    }
    else
    {
        std::cout<<"detection parse fail."<<std::endl;
    }
}

void MainWindow::UpdateObj(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    iv::radar::radar4dobjectarray xobjarray;
    if(xobjarray.ParseFromArray(strdata,(int)nSize))
    {
        mmutexobj.lock();
        mobjarray.CopyFrom(xobjarray);
        mbobjupdate = true;
        mmutexobj.unlock();
        emit detupdate();
    }
    else
    {
        std::cout<<"object parse fail."<<std::endl;
    }
}
#else
void MainWindow::threaddet()
{
    while(mbThreadRun)
    {
        iv::radar::radar4ddetectarray xdetarray;
        if(mprecv->GetDetect(10,xdetarray) == 1)
        {
            mmutexdet.lock();
            mdetarray.CopyFrom(xdetarray);
            mbdetupdate = true;
            mmutexdet.unlock();
            emit detupdate();
        }
    }
}

void MainWindow::threadobj()
{
    while(mbThreadRun)
    {
        iv::radar::radar4dobjectarray xobjarray;
        if(mprecv->GetObj(10,xobjarray) == 1)
        {
            mmutexobj.lock();
            mobjarray.CopyFrom(xobjarray);
            mbobjupdate = true;
            mmutexobj.unlock();
            emit detupdate();
        }
    }
}
#endif



void MainWindow::on_actionAll_triggered()
{
    if(ui->actionAll->isChecked()){
        ui->actionNoClassification->setChecked(true);
        ui->actionNoise->setChecked(true);
        ui->actionGround->setChecked(true);
        ui->actionTraversableUnder->setChecked(true);
        ui->actionObstacle->setChecked(true);
        ui->actionInvalid->setChecked(true);
    }
    else
    {
        ui->actionNoClassification->setChecked(false);
        ui->actionNoise->setChecked(false);
        ui->actionGround->setChecked(false);
        ui->actionTraversableUnder->setChecked(false);
        ui->actionObstacle->setChecked(false);
        ui->actionInvalid->setChecked(false);
    }
}


void MainWindow::on_actionObjectAll_triggered()
{
    if(ui->actionObjectAll->isChecked()){
        ui->actionCar->setChecked(true);
        ui->actionTruck->setChecked(true);
        ui->actionMotorcycle->setChecked(true);
        ui->actionBicycle->setChecked(true);
        ui->actionPedestrian->setChecked(true);
        ui->actionAnimal->setChecked(true);
        ui->actionHazard->setChecked(true);
        ui->actionUnknown->setChecked(true);
    }
    else{
        ui->actionCar->setChecked(false);
        ui->actionTruck->setChecked(false);
        ui->actionMotorcycle->setChecked(false);
        ui->actionBicycle->setChecked(false);
        ui->actionPedestrian->setChecked(false);
        ui->actionAnimal->setChecked(false);
        ui->actionHazard->setChecked(false);
        ui->actionUnknown->setChecked(false);
    }
}