modularization/src/fusion/fusion_DRG/mainwindow.cpp

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

#include <QPaintEvent>
#include <QPainter>

#include <iostream>
#include <math.h>
#include <algorithm>
MainWindow * mw;

#define APOLLO_FU   //宏开关，undef重新编译则恢复原来状态

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<<"DRG Listen fusion fail."<<std::endl;
        return;
    }
    std::cout << "DRG Listen fusion: " << xfusionobjarray.ByteSize() << " bytes" << std::endl;
    mw->generateDRG(xfusionobjarray);
    mw->updateDRG();
}

void ListenGPSFusion(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1) return;
    iv::fusion::gpsndt gpsdata;
    if(false == gpsdata.ParseFromArray(strdata,nSize))
    {
        std::cout<<"DRG Listen GPSfusion fail."<<std::endl;
        return;
    }
    std::cout << "DRG Listen GPSfusion: " << gpsdata.ByteSize() << " bytes" << std::endl;
}


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

    //监听融合信息
    iv::modulecomm::RegisterRecv("fusion",ListenFusion);
    iv::modulecomm::RegisterRecv("gpsndtfusion",ListenGPSFusion);


    //发送珊格图的结果
    drgFusion = iv::modulecomm::RegisterSend("drgfusion",160000*30,3);
    pa = iv::modulecomm::RegisterSend("drg_image",160000*30,3);

    QTimer * timer = new QTimer(this);
    connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
    timer->start(1000);  //1秒（1000毫秒）更新一次

    setWindowTitle("栅格图生成");

    QObject::connect(ui->checkBox,SIGNAL(stateChanged(int)),this,SLOT(on_checkBox_stateChanged));
}

MainWindow::~MainWindow()
{
    delete ui;
}
void MainWindow::onTimer()
{
    //updateDRG();
    return;

}
void MainWindow::updateDRG()
{
    if(isPreview) drawImage();
}


void MainWindow::generateDRG(iv::fusion::fusionobjectarray &arr){

    ui->msg->setText("收到 " + QString::number(arr.obj_size()) + " 个障碍物信息");

    int picSize = 160000;
    drg.clear_pixels();

    int marks[picSize] = {0};
    for(int i=0; i<arr.obj_size(); i++){
        //处理融合数据，分析每个障碍物特点
        iv::fusion::fusionobject obj = arr.obj(i);
        iv::fusion::PointXYZ center = obj.centroid();//中心点
        iv::fusion::Dimension space = obj.dimensions();//尺寸

        //每个障碍物占据一片像素点
        //先确定左上角和右下角
        int leftTopX = std::max(std::min( (int)( (center.x() - space.x()/2)*10 ) + 100, 199),0);
        int leftTopY = std::max(std::min( 400 - (int)((center.y() + space.y()/2)*10), 799),0);

        int rightBottomX = std::max(std::min( (int)( (center.x() + space.x()/2)*10 ) + 100, 199),0);
        int rightBottomY = std::max(std::min( 400 - (int)((center.y() - space.y()/2)*10), 799),0);

        //换算成数组下标，然后设置值
        for(int j=leftTopY; j<=rightBottomY; j++){
            int base = j*200;
            for(int k=leftTopX; k<=rightBottomX; k++){
                int index = base + k;
                if(index < picSize){
                    marks[index] = i+1;
                }
            }
        }
    }

    for(int i=0; i<picSize; i++){
        iv::fusion::DRGPix *p = drg.add_pixels();
        if(marks[i]>0){
            iv::fusion::fusionobject obj = arr.obj(marks[i]-1);
            //高度取z向空间
            if(obj.has_dimensions() && obj.dimensions().has_z()) p->set_height(obj.dimensions().z());

            if(obj.has_id()) p->set_id(obj.id());
            if(obj.has_type()) p->set_type(obj.type());
            if(obj.has_type_name()) p->set_type_name(obj.type_name());
            if(obj.has_prob()) p->set_prob(obj.prob());

            if(obj.has_vel_abs()){
                iv::fusion::VelXY * vel = new iv::fusion::VelXY(obj.vel_abs());
                p->set_allocated_vel_abs(vel);
                //动态障碍物置0
                if(vel->has_x()&&vel->has_y()&&(vel->x()*vel->x()+vel->y()*vel->y())>0.01){
                    p->set_value(0);
                }else{
                    p->set_value(1);
                }
            }else{
                p->set_value(1);
            }
            if(obj.has_vel_relative()) p->set_allocated_vel_relative(new iv::fusion::VelXY(obj.vel_relative()));

            if(obj.has_yaw()) p->set_yaw(obj.yaw());
            //评估危险程度，todo
            p->set_risklevel(0);

        }else{
            p->set_value(0);
            p->set_height(0);
            p->set_risklevel(0);
        }

    }

    //写入共享内存处理结果
    int nsize = drg.ByteSize();
    char * str = new char[nsize];
    ui->result->setText("发送结果：" + QString::number(nsize) + "字节");

    if(drg.SerializeToArray(str,nsize)){
        iv::modulecomm::ModuleSendMsg(drgFusion,str,nsize);
    }
    delete str;
}

void MainWindow::drawImage()
{
    QImage image(QSize(200,800),QImage::Format_ARGB32);
    image.fill("white");
    QPainter painter(&image);
    painter.save();

    painter.setRenderHint(QPainter::Antialiasing);   //线条抗锯齿
    painter.setRenderHint(QPainter::TextAntialiasing);

    painter.save();
    QBrush brush;
    brush.setColor(Qt::black);
    brush.setStyle(Qt::SolidPattern);
    painter.setBrush(brush);

    int psize = drg.pixels_size();
    if(psize>1){
        for(int i=0; i<psize; i++){
            if(drg.pixels(i).value()>0){
                //计算点的位置
                int x = i%200, y = i/200;
                painter.drawPoint(x,y);   //即widget窗口的整个矩形，大于定义的辐射填充区域有延展效果
            }
        }
    }
    painter.restore();
    painter.end();


    ui->imageLabel->setPixmap(QPixmap::fromImage(image));
}

void MainWindow::clearImage(){
    QImage image(QSize(200,800),QImage::Format_ARGB32);
    image.fill("gray");

#ifdef APOLLO_FU
    //image.fill("white");
    QPainter painter(&image);
    painter.setRenderHint(QPainter::Antialiasing);   //线条抗锯齿
    painter.setRenderHint(QPainter::TextAntialiasing);

    //painter.setPen(QColor(0, 160, 230));
    painter.setPen(QColor(0, 250, 0));   //RGB配色表   http://www.wahart.com.hk/rgb.htm
    for(int i=0;i<=80;i++){
        painter.drawLine(0,i*10,200,i*10);
    }
    for(int i=0;i<=20;i++){
        painter.drawLine(i*10,0,i*10,800);
    }
#endif

#ifdef APOLLO_FU
    int nsize = image.byteCount();
    unsigned char *data  = image.bits();
    iv::modulecomm::ModuleSendMsg(pa,(const char*)data,nsize);
#endif


    ui->imageLabel->setPixmap(QPixmap::fromImage(image));
}

void MainWindow::resizeEvent(QResizeEvent* size){
    //计算显示控件的尺寸
    int w = this->width()*0.9, h = this->height() - 20;
    if(h>w*4){
        h = w*4;
    }else{
        w = h/4;
    }
    ui->imageLabel->setGeometry((this->width() - w)/2,15,w,h);
    ui->checkBox->setGeometry(this->width()-100,10,100,15);
}


void MainWindow::on_checkBox_stateChanged(int arg1)
{
    if(arg1){
        isPreview = true;
    }else{
        clearImage();
        isPreview = false;
    }
}