modularization/src/tool/map_lanetoxodr/ui/dialogparkingspacefromrtk.cpp

#include "dialogparkingspacefromrtk.h"
#include "ui_dialogparkingspacefromrtk.h"

#include "xodrfunc.h"

#include "mainwindow.h"

extern MainWindow * gw;

extern double glon0 ;
extern double glat0;


DialogParkingSpaceFromRTK::DialogParkingSpaceFromRTK(Road * pRoad,QWidget *parent) :
    QDialog(parent),
    ui(new Ui::DialogParkingSpaceFromRTK)
{
    ui->setupUi(this);

    mpRoad = pRoad;

    mLat0 = glat0;
    mLon0 = glon0;

    setWindowTitle(tr("ParkingSpace From RTK"));
}

DialogParkingSpaceFromRTK::~DialogParkingSpaceFromRTK()
{
    delete ui;
}

void DialogParkingSpaceFromRTK::on_pushButton_LoadData_clicked()
{
    QString strfilepath = QFileDialog::getOpenFileName(this,tr("Open RTK Data"),"",tr("RTK File(*.csv)"));
    if(strfilepath.isEmpty())return;

    QFile xFile;
    xFile.setFileName(strfilepath);

    std::vector<iv::rtkdata> xvectorrtkdata;
    xvectorrtkdata.clear();

    ui->plainTextEdit->clear();

    if(xFile.open(QIODevice::ReadWrite))
    {
        QByteArray ba;
        ba = xFile.read(xFile.size());
        QList<QByteArray> strlinedata=  ba.split('\n');
        int i;
        int nsize = strlinedata.size();
        const int nstartline = 1;
        for(i=nstartline;i<nsize;i++)
        {
            double flat,flon,fheight,frelx,frely;
            QList<QByteArray> strlistvalue = strlinedata.at(i).split(',');
            int ncount = strlistvalue.size();
            if(ncount>=4)
            {
                flon  = atof(strlistvalue.at(2).data());
                flat = atof(strlistvalue.at(1).data());
                QString strheight = strlistvalue.at(3);
                strheight = strheight.left(strheight.size() -2);
                fheight = strheight.toDouble();

                DialogAddRoadFromRTK::calcrelxy(mLon0,mLat0,flon,flat,frelx,frely);

                iv::rtkdata xdata;
                xdata.mflat = flat;
                xdata.mflon = flon;
                xdata.mfrelx = frelx;
                xdata.mfrely = frely;
                xdata.mheight = fheight;
                if(xvectorrtkdata.size() < 1)
                {
                    xdata.mfrels = 0;
                }
                else
                {
                    xdata.mfrels = xvectorrtkdata[xvectorrtkdata.size()-1].mfrels
                            +sqrt(pow(xvectorrtkdata[xvectorrtkdata.size()-1].mfrelx - frelx,2)+pow(xvectorrtkdata[xvectorrtkdata.size()-1].mfrely - frely,2));
                }
                xvectorrtkdata.push_back(xdata);

                QString strline = QString::number(flon,'f',7) + QString("\t")
                        +QString::number(flat,'f',7)+QString("\t")
                        +QString::number(fheight,'f',4)+QString("\t")
                        +QString::number(frelx,'f',2) + QString("\t")
                        +QString::number(frely,'f',2) + QString("\t");
                ui->plainTextEdit->appendPlainText(strline);
            }
        }
    }

    mvectorrtkdata = xvectorrtkdata;

    unsigned int nsize = mvectorrtkdata.size();
    ui->comboBox_LeftUp->clear();
    ui->comboBox_LeftDown->clear();
    ui->comboBox_RightUp->clear();
    ui->comboBox_RightDown->clear();
    if(nsize >= 3)
    {
        unsigned int i;
        for(i=0;i<nsize;i++)
        {
            ui->comboBox_LeftUp->addItem(QString::number(i));
            ui->comboBox_LeftDown->addItem(QString::number(i));
            ui->comboBox_RightUp->addItem(QString::number(i));
            ui->comboBox_RightDown->addItem(QString::number(i));
        }
        ui->comboBox_LeftUp->setCurrentIndex(0);
        ui->comboBox_LeftDown->setCurrentIndex(1);
        ui->comboBox_RightUp->setCurrentIndex(3);
        ui->comboBox_RightDown->setCurrentIndex(2);
    }


}

void DialogParkingSpaceFromRTK::on_pushButton_Create_clicked()
{
    Road * pRoad = mpRoad;
    if(mvectorrtkdata.size()<3)
    {
        QMessageBox::warning(this,"Warning","At Least 3 Points.",QMessageBox::YesAll);
        return;
    }

    int nLeftDown = ui->comboBox_LeftDown->currentIndex();
    int nLeftUp = ui->comboBox_LeftUp->currentIndex();
    int nRightUp = ui->comboBox_RightUp->currentIndex();
    int nRightDown = ui->comboBox_RightDown->currentIndex();

    double fHdg = xodrfunc::CalcHdg(QPointF(mvectorrtkdata[nLeftDown].mfrelx,mvectorrtkdata[nLeftDown].mfrely),
                                    QPointF(mvectorrtkdata[nLeftUp].mfrelx,mvectorrtkdata[nLeftUp].mfrely));

    double fCenter_x,fCenter_y;
    fCenter_x = (mvectorrtkdata[nLeftUp].mfrelx + mvectorrtkdata[nRightDown].mfrelx)/2.0;
    fCenter_y = (mvectorrtkdata[nLeftUp].mfrely + mvectorrtkdata[nRightDown].mfrely)/2.0;

    double fLen = sqrt(pow(mvectorrtkdata[nLeftUp].mfrelx - mvectorrtkdata[nLeftDown].mfrelx,2)
                       +pow(mvectorrtkdata[nLeftUp].mfrely - mvectorrtkdata[nLeftDown].mfrely,2));
    double fWidth = sqrt(pow(mvectorrtkdata[nLeftDown].mfrelx - mvectorrtkdata[nRightDown].mfrelx ,2)
                         +pow(mvectorrtkdata[nLeftDown].mfrely - mvectorrtkdata[nRightDown].mfrely,2));

    double fnears = 0;
    double froadx,froady,froadhdg;
    pRoad->GetGeometryCoords(0,froadx,froady,froadhdg);
    double fx,fy,fh,fs;
    fx = froadx;
    fy = froady;
    fh = froadhdg;
    fs = 0;
    double fneart = sqrt(pow(froadx - fCenter_x,2) + pow(froady - fCenter_y,2));
    double s = 0;
    double sstep = 0.1;
    s = s+ sstep;
    while(s<pRoad->GetRoadLength())
    {
        if(pRoad->GetGeometryCoords(s,froadx,froady,froadhdg) >= 0)
        {
            double fdis = sqrt(pow(froadx - fCenter_x,2) + pow(froady - fCenter_y,2));
            if(fdis<fneart)
            {
                fneart = fdis;
                fx = froadx;
                fy = froady;
                fh = froadhdg;
                fs = s;
            }
        }
        else
        {
            std::cout<<"DialogParkingSpaceFromRTK::on_pushButton_Create_clicked"<<" GetGeometryCoords error. s "
                    <<s<<" Road id:"<<pRoad->GetRoadId()<<" len:"<<pRoad->GetRoadLength()<<std::endl;
        }
        s= s+sstep;
    }

    if(fneart > 0)
    {
        double fhdg2 = xodrfunc::CalcHdg(QPointF(fx,fy),QPointF(fCenter_x,fCenter_y));
        double fhdgdiff = fhdg2 - fh;
        while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
        while(fhdgdiff >= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
        if(fhdgdiff > M_PI)fneart = fneart * (-1);

    }
    pRoad->AddObject("0",fs,fneart,0);
    Object * pObj = pRoad->GetLastAddedObject();
    pObj->Settype("parkingSpace");
    pObj->Sethdg(fHdg);
    pObj->Setlength(fLen);
    pObj->Setwidth(fWidth);

    qDebug("fhdg:%f flen:%f fwidth:%f",fHdg,fLen,fWidth);
}