change map_collectfromveh, not complete.

src/include/proto/collectveh.proto

@@ -36,11 +36,26 @@ message collectvehroad
 	repeated collectvehroadpoint mpoints = 2;	
 };
 
+
+message roadconnect
+{
+	enum ConnectMode
+	{
+		STRAIGHT = 0;
+		TURN = 1;
+		UTURN = 2;
+	}
+	required ConnectMode mMode = 1;
+	required string strroad1 = 2;
+	required string strroad2 = 3;
+};
+
 message collectveh
 {
 	repeated collectvehroad mroads = 1;
 	required double mlon0 = 2;
 	required double mlat0 = 3;
+	repeated roadconnect mconnects = 4;
 };
 
 

src/tool/map_collectfromveh/collectconvert.cpp

@@ -0,0 +1,376 @@
+#include "collectconvert.h"
+
+#include "gnss_coordinate_convert.h"
+#include "geofit.h"
+
+int groadid = 10001;
+CollectConvert::CollectConvert(iv::map::collectveh & xcollectveh,OpenDrive * pxodr)
+{
+    mcollectveh.CopyFrom(xcollectveh);
+    mpxodr = pxodr;
+}
+
+void CollectConvert::StartConvert()
+{
+    threadconvert();
+}
+
+void CollectConvert::threadconvert()
+{
+    OpenDrive * pxodr = mpxodr;
+    pxodr->SetHeader(1,1,"adc",1.1,"2022",0,0,0,0,mcollectveh.mlat0(),mcollectveh.mlon0(),360);
+    int i;
+    for(i=0;i<mcollectveh.mroads_size();i++)
+    {
+        iv::map::collectvehroad * proad = mcollectveh.mutable_mroads(i);
+        ConvertRoad(proad,pxodr);
+
+    }
+}
+
+void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxodr)
+{
+    if(proad->mpoints_size() == 0)return;
+    double flon0,flat0;
+    pxodr->GetHeader()->GetLat0Lon0(flat0,flon0);
+//    pxodr->GetHeader()->GetLat0Lon0(flon0,flat0);
+    double x0,y0;
+    GaussProjCal(flon0,flat0,&x0,&y0);
+    int i;
+    std::vector<iv::collectxy> xvectorcollectxy;
+    for(i=0;i<proad->mpoints_size();i++)
+    {
+        iv::map::collectvehroadpoint * ppoint = proad->mutable_mpoints(i);
+        double x,y;
+//        double fl1 = ppoint->mflon();
+//        double fl2 = ppoint->mflat();
+        GaussProjCal(ppoint->mflon(),ppoint->mflat(),&x,&y);
+        iv::collectxy xc;
+        xc.height = 0;
+        xc.width = ppoint->mflanewidth();
+        xc.fhdg = (90 -ppoint->mfheading())*M_PI/180.0;
+        xc.x = x-x0 + ppoint->mfdis_left()*cos(xc.fhdg +M_PI/2.0);
+        xc.y = y-y0 + ppoint->mfdis_left()*sin(xc.fhdg +M_PI/2.0);
+        xvectorcollectxy.push_back(xc);
+        if(i>0)
+        {
+            double fdis =sqrt(pow(xvectorcollectxy[i].x - xvectorcollectxy[i-1].x,2)+pow(xvectorcollectxy[i].y-xvectorcollectxy[i-1].y,2));
+            xvectorcollectxy[i].fdistolast = fdis;
+
+        }
+    }
+
+
+    double LINE_ERROR = 0.20;
+
+
+    std::vector<geobase> xvectorgeo;
+
+    bool bComplete = false;
+    int j;
+    int ncurpos = 0;
+    int nrange = xvectorcollectxy.size();
+    bool bHaveLastxyhdg = false;
+    double fLastX,fLastY,fLastHdg;
+    double fEndX,fEndY,fEndHdg;
+    double fStartX,fStartY,fStartHdg;
+    while(bComplete == false)
+    {
+
+        VectorXd x_veh(nrange);
+        VectorXd y_veh(nrange);
+        for(j=0;j<nrange;j++)
+        {
+            x_veh[j] =xvectorcollectxy[j+ncurpos].x;//  xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fx;//mvectorrtkdata.at(j+ncurpos).mfrelx;
+            y_veh[j] = xvectorcollectxy[j+ncurpos].y;//xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fy;//mvectorrtkdata.at(j+ncurpos).mfrely;
+        }
+
+        bool bArcOk = false;
+        bool bLineOk = false;
+        double dismax = 0;
+        QPointF sp,ep;
+        double fR,flen;
+        double fhdgstart,fhdgend;
+        if(nrange >= 3)
+        {
+
+            geofit::Inst().arcfitanddis(x_veh,y_veh,dismax,fR,sp,ep,fhdgstart,fhdgend,flen);
+
+            if(dismax < LINE_ERROR)
+            {
+                bArcOk = true;
+                std::cout<<" a arc is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+            }
+        }
+
+
+        auto coeffs = polyfit(x_veh, y_veh, 1);
+
+        dismax = 0;
+        for(j=0;j<nrange;j++)
+        {
+            double A = coeffs[1];
+            double B = -1;
+            double C = coeffs[0];
+            double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
+            if(dis>dismax)dismax = dis;
+        }
+        if(dismax < LINE_ERROR)
+        {
+            bLineOk = true;
+            std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+        }
+
+        if((nrange <= 2)||bLineOk||bArcOk)
+        {
+            if((bLineOk)||(nrange<=2))
+            {
+                std::cout<<"use line"<<std::endl;
+                geobase xgeo;
+                xgeo.mnStartPoint = ncurpos;
+                xgeo.mnEndPoint = ncurpos + nrange -1 ;
+
+                double x0,y0,x1,y1;
+                geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[0],
+                        y_veh[0],x0,y0);
+                geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[nrange -1],
+                        y_veh[nrange - 1],x1,y1);
+                xgeo.mfA = coeffs[1];
+                xgeo.mfB = -1;
+                xgeo.mfC = coeffs[0];
+                xgeo.mfHdg = geofit::Inst().CalcHdg(x0,y0,x1,y1);
+                xgeo.mfX = x0;
+                xgeo.mfY = y0;
+                xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
+                xgeo.mnType = 0;
+                fStartX = x0;
+                fStartY = y0;
+                fStartHdg = xgeo.mfHdg;
+                fEndX = x1;
+                fEndY = y1;
+                fEndHdg = xgeo.mfHdg;
+
+                xvectorgeo.push_back(xgeo);
+            }
+            else
+            {
+                if(bArcOk)
+                {
+                    std::cout<<"use arc"<<std::endl;
+                    geobase xgeo;
+                    xgeo.mnStartPoint = ncurpos;
+                    xgeo.mnEndPoint = ncurpos + nrange -1;
+                    xgeo.mfHdg = fhdgstart;
+                    xgeo.mfHdgStart = fhdgstart;
+                    xgeo.mfHdgEnd = fhdgend;
+                    xgeo.mfX = sp.x();
+                    xgeo.mfY = sp.y();
+                    xgeo.mfLen = flen;
+                    xgeo.mnType = 1;
+                    xgeo.mfEndX = ep.x();
+                    xgeo.mfEndY = ep.y();
+                    xgeo.mR = fR;
+                    fStartX = xgeo.mfX;
+                    fStartY = xgeo.mfY;
+                    fStartHdg = xgeo.mfHdgStart;
+                    fEndX = xgeo.mfEndX;
+                    fEndY = xgeo.mfEndY;
+                    fEndHdg = xgeo.mfHdgEnd;
+                    xvectorgeo.push_back(xgeo);
+                }
+            }
+            //        std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+
+            if(bHaveLastxyhdg)
+            {
+                std::vector<geobase> xvectorgeobe = geofit::CreateBezierGeo(fLastX,fLastY,
+                                                                          fLastHdg,fStartX,fStartY,fStartHdg);
+                if(xvectorgeobe.size()>0)
+                {
+                    xvectorgeobe[0].mnStartPoint = ncurpos-1;
+                    xvectorgeobe[0].mnEndPoint = ncurpos;
+                    xvectorgeo.insert(xvectorgeo.begin()+(xvectorgeo.size()-1),xvectorgeobe[0]);
+                }
+            }
+            bHaveLastxyhdg = true;
+            fLastX = fEndX;
+            fLastY = fEndY;
+            fLastHdg = fEndHdg;
+
+
+ //           ncurpos = ncurpos + nrange -1;
+            ncurpos = ncurpos + nrange;  //Skip 1 point, use bezier
+            nrange = xvectorcollectxy.size() - ncurpos;
+
+        }
+        else
+        {
+            if(nrange > 30)
+                nrange = nrange/2;
+            else
+                nrange = nrange -1;
+            if(nrange<2)nrange = 2;
+        }
+        if(ncurpos>=(int)(xvectorcollectxy.size()-1))
+        {
+            if(ncurpos == (int)(xvectorcollectxy.size()-1))
+            {
+                std::cout<<"Last section, 2 Points, use line."<<std::endl;
+                VectorXd x_veh1(2);
+                VectorXd y_veh1(2);
+                x_veh1[0] = fLastX;
+                y_veh1[0] = fLastY;
+                x_veh1[1] =  xvectorcollectxy[ncurpos].x;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].fx;
+                y_veh1[1] = xvectorcollectxy[ncurpos].y;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].fy;
+                auto coeffs = polyfit(x_veh1, y_veh1, 1);
+
+                geobase xgeo;
+                xgeo.mnStartPoint = ncurpos-1;
+                xgeo.mnEndPoint = ncurpos;
+
+                xgeo.mfA = coeffs[1];
+                xgeo.mfB = -1;
+                xgeo.mfC = coeffs[0];
+                xgeo.mfHdg = geofit::Inst().CalcHdg(x_veh1[0],y_veh1[0],x_veh1[1],y_veh1[1]);
+                xgeo.mfX = x_veh1[0];
+                xgeo.mfY = y_veh1[0];
+                xgeo.mfLen = sqrt(pow(x_veh1[1]-x_veh1[0],2)+pow(y_veh1[1]-y_veh1[0],2));
+                xgeo.mnType = 0;
+                xvectorgeo.push_back(xgeo);
+            }
+            bComplete = true;
+        }
+    }
+
+    double snow = 0;
+    for(i=0;i<(int)xvectorgeo.size();i++)
+    {
+        geobase * pgeo = &xvectorgeo[i];
+//        std::cout<<"geo len: "<<pgeo->mfLen<<" type"<<pgeo->mnType<<std::endl;
+        double tems = 0;
+        for(j=(pgeo->mnStartPoint+1);j<=pgeo->mnEndPoint;j++)
+        {
+            xvectorcollectxy[j].s = snow + tems+xvectorcollectxy[j].fdistolast;
+  //          xvectorvehicle[xvectorvl[j].mvehindex].s = snow + tems + xvectorvehicle[xvectorvl[j].mvehindex].fdistolast;
+            tems = tems + xvectorcollectxy[j].fdistolast;
+        }
+        snow = snow + pgeo->mfLen;
+    }
+
+    std::vector<iv::widthabcd> xvectorwidthabcd;
+    bComplete = false;
+    ncurpos = 0;
+    nrange = xvectorcollectxy.size();
+    double flanewidtherror = 0.1; //1cm
+    while(bComplete == false)
+    {
+        double ele_coff[4];
+        for(j=0;j<4;j++)ele_coff[j] = 0;
+        if(nrange>0)ele_coff[0] = xvectorcollectxy[ncurpos].width;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].flanewidth;
+        int M = nrange;
+        VectorXd x_vehhg(M);
+        VectorXd y_vehhg(M);
+        for(j=0;j<M;j++)
+        {
+            x_vehhg[j] = xvectorcollectxy[ncurpos+j].s - xvectorcollectxy[ncurpos].s;//   xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].s - xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
+            y_vehhg[j] = xvectorcollectxy[ncurpos+j].width;//    xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].flanewidth;
+        }
+        int MX = 3;
+        if(M<4)MX = M -1;
+        if(MX>3)MX = 3;
+        if(MX>0)
+        {
+            VectorXd coeffs = polyfit(x_vehhg, y_vehhg, MX);
+            for(j=0;j<=MX;j++)
+            {
+                ele_coff[j] = coeffs[j];
+            }
+        }
+        double ferror = 0;
+        for(j=0;j<M;j++)
+        {
+            double s = x_vehhg[j];
+            double fvalue = fabs(ele_coff[0] + ele_coff[1]*s + ele_coff[2]*s*s+ele_coff[3]*s*s*s - y_vehhg[j]);
+            if(fvalue>ferror)ferror = fvalue;
+        }
+        if(ferror>flanewidtherror)
+        {
+            if(nrange>10)nrange = nrange/2;
+            else nrange = nrange -1;
+        }
+        else
+        {
+            iv::widthabcd xabcd;
+            xabcd.s =  xvectorcollectxy[ncurpos].s;//  xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
+            xabcd.A = ele_coff[0];
+            xabcd.B = ele_coff[1];
+            xabcd.C = ele_coff[2];
+            xabcd.D = ele_coff[3];
+            xvectorwidthabcd.push_back(xabcd);
+            ncurpos = ncurpos + nrange-1;
+            nrange = xvectorcollectxy.size() - ncurpos;
+        }
+
+        if(ncurpos>=(int)(xvectorcollectxy.size()-1))
+        {
+            bComplete = true;
+        }
+    }
+
+    pxodr->AddRoad(proad->strroadname(),xvectorcollectxy[xvectorcollectxy.size()-1].s, QString::number(groadid).toStdString(),"-1");
+    Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
+    groadid++;
+
+  //  p->AddElevation(0,xlaneheightcoff.A,xlaneheightcoff.B,xlaneheightcoff.C,xlaneheightcoff.D);
+
+    double s = 0;
+    j= 0;
+//        for(j=0;j<4;j++)
+    for(j=0;j<xvectorgeo.size();j++)
+    {
+
+        p->AddGeometryBlock();
+        GeometryBlock * pgb = p->GetGeometryBlock(j);
+
+        geobase * pline;
+        geobase * pbez;
+        geobase * parc;
+
+        switch(xvectorgeo[j].mnType)
+        {
+        case 0:
+            pline = &xvectorgeo[j];
+            pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
+            break;
+        case 1:
+            parc = &xvectorgeo[j];
+            pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
+            break;
+        case 2:
+            pbez = &xvectorgeo[j];
+            std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
+            pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
+                                       pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
+                                       pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
+                                       pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
+            break;
+        }
+        s = s + xvectorgeo[j].mfLen;
+    }
+
+    p->AddLaneSection(0);
+
+    LaneSection * pLS = p->GetLaneSection(0);
+
+    pLS->AddLane(0,0,"none",false);
+    pLS->AddLane(-1,(-1)*(pLS->GetRightLaneCount()+1),"driving",false);
+    Lane * pLane = pLS->GetLastRightLane();
+    for(j=0;j<(int)xvectorwidthabcd.size();j++)
+    {
+        iv::widthabcd * pwa = &xvectorwidthabcd[j];
+        pLane->AddWidthRecord(pwa->s,pwa->A,pwa->B,pwa->C,pwa->D);
+    }
+
+
+
+}

src/tool/map_collectfromveh/collectconvert.h

@@ -0,0 +1,47 @@
+#ifndef COLLECTCONVERT_H
+#define COLLECTCONVERT_H
+
+#include "collectveh.pb.h"
+
+#include <thread>
+#include "OpenDrive/OpenDrive.h"
+
+namespace iv {
+    struct collectxy
+    {
+        double s;
+        double x;
+        double y;
+        double width;
+        double height;
+        double fhdg;
+        double fdistolast = 0;
+    };
+    struct widthabcd
+    {
+        double s;
+        double A;
+        double B;
+        double C;
+        double D;
+    };
+
+}
+
+class CollectConvert
+{
+public:
+    CollectConvert(iv::map::collectveh & xcollectveh,OpenDrive * pxodr);
+    void StartConvert();
+
+private:
+    iv::map::collectveh mcollectveh;
+    OpenDrive * mpxodr;
+    void threadconvert();
+
+    void ConvertRoad(iv::map::collectvehroad * proad,OpenDrive * pxodr);
+
+
+};
+
+#endif // COLLECTCONVERT_H

src/tool/map_collectfromveh/dialogconvert.cpp

@@ -0,0 +1,14 @@
+#include "dialogconvert.h"
+#include "ui_dialogconvert.h"
+
+DialogConvert::DialogConvert(QWidget *parent) :
+    QDialog(parent),
+    ui(new Ui::DialogConvert)
+{
+    ui->setupUi(this);
+}
+
+DialogConvert::~DialogConvert()
+{
+    delete ui;
+}

src/tool/map_collectfromveh/dialogconvert.h

@@ -0,0 +1,22 @@
+#ifndef DIALOGCONVERT_H
+#define DIALOGCONVERT_H
+
+#include <QDialog>
+
+namespace Ui {
+class DialogConvert;
+}
+
+class DialogConvert : public QDialog
+{
+    Q_OBJECT
+
+public:
+    explicit DialogConvert(QWidget *parent = nullptr);
+    ~DialogConvert();
+
+private:
+    Ui::DialogConvert *ui;
+};
+
+#endif // DIALOGCONVERT_H

src/tool/map_collectfromveh/dialogconvert.ui

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DialogConvert</class>
+ <widget class="QDialog" name="DialogConvert">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>659</width>
+    <height>425</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Dialog</string>
+  </property>
+  <widget class="QProgressBar" name="progressBar">
+   <property name="geometry">
+    <rect>
+     <x>70</x>
+     <y>140</y>
+     <width>491</width>
+     <height>61</height>
+    </rect>
+   </property>
+   <property name="value">
+    <number>24</number>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/tool/map_collectfromveh/editwin.cpp

@@ -310,6 +310,56 @@ void EditWin::ondbclickxy(double x, double y)
     ui->statusbar->showMessage(strtoolshow);
 
     updateselecshow();
+
+    if((mnSelMode == 2)&&(mbConnecting))
+    {
+        if(mnConnectStep == 0)
+        {
+            if(mpselroad != NULL)
+            {
+                mstrfirstsel = mpselroad->strroadname();
+                mnConnectStep = 1;
+            }
+        }
+        else
+        {
+            if(mpselroad != NULL)
+            {
+                std::string strsec = mpselroad->strroadname();
+                if(strsec != mstrfirstsel)
+                {
+                    mbConnecting = false;
+                    mnConnectStep = 0;
+                    QString strmode;
+                    switch (mnConnectMode) {
+                    case 0:
+                        strmode = QString(tr("直连"));
+                        ui->actionLineConnect->setChecked(false);
+                        break;
+                    case 1:
+                        strmode = QString(tr("转弯"));
+                        ui->actionTurnConect->setChecked(false);
+                        break;
+                    case 2:
+                        strmode = QString(tr("调头"));
+                        ui->actionUTurnConnect->setChecked(false);
+                        break;
+                    default:
+                        break;
+                    }
+                    QString strinfo = QString(tr("是否在道路("))+QString(mstrfirstsel.data())
+                            +QString(tr(")和道路("))+QString(strsec.data())
+                            +QString(tr(")建立")) + strmode + QString(tr("连接？"));
+                    QMessageBox::StandardButton button;
+                    button=QMessageBox::question(this,tr("建立连接"),strinfo,QMessageBox::Yes|QMessageBox::No);
+                    if(button==QMessageBox::Yes)
+                    {
+                        CreateConnect(mnConnectMode,mstrfirstsel,strsec);
+                    }
+                }
+            }
+        }
+    }
 }
 
 void EditWin::on_actionMove_triggered()
@@ -814,6 +864,7 @@ void EditWin::on_actionRoadDelete_triggered()
     {
         if(mcollectveh.mutable_mroads(i) == mpselroad)
         {
+            DeleteRoadConnectByName(mpselroad->strroadname());
             mcollectveh.mutable_mroads()->erase(mcollectveh.mutable_mroads()->begin() + i);
             break;
         }
@@ -838,3 +889,112 @@ bool EditWin::IsShow()
 {
     return mbShow;
 }
+
+void EditWin::on_actionLineConnect_triggered()
+{
+    if(ui->actionLineConnect->isChecked() == false)
+    {
+        mbConnecting = false;
+        return;
+    }
+    ui->actionSelectRoad->setChecked(true);
+    ui->actionSelectNone->setChecked(false);
+    ui->actionSelectPoint->setChecked(false);
+    ui->actionTurnConect->setChecked(false);
+    ui->actionUTurnConnect->setChecked(false);
+    mnSelMode = 2;
+
+    mbConnecting = true;
+    mnConnectStep = 0;
+    mnConnectMode = 0;
+}
+
+void EditWin::on_actionTurnConect_triggered()
+{
+    if(ui->actionTurnConect->isChecked() == false)
+    {
+        mbConnecting = false;
+        return;
+    }
+    ui->actionSelectRoad->setChecked(true);
+    ui->actionSelectNone->setChecked(false);
+    ui->actionSelectPoint->setChecked(false);
+    ui->actionLineConnect->setChecked(false);
+    ui->actionUTurnConnect->setChecked(false);
+    mnSelMode = 2;
+    mbConnecting = true;
+    mnConnectStep = 0;
+    mnConnectMode = 1;
+}
+
+void EditWin::on_actionUTurnConnect_triggered()
+{
+    if(ui->actionUTurnConnect->isChecked() == false)
+    {
+        mbConnecting = false;
+        return;
+    }
+    ui->actionSelectRoad->setChecked(true);
+    ui->actionSelectNone->setChecked(false);
+    ui->actionSelectPoint->setChecked(false);
+    ui->actionLineConnect->setChecked(false);
+    ui->actionTurnConect->setChecked(false);
+    mnSelMode = 2;
+    mbConnecting = true;
+    mnConnectStep = 0;
+    mnConnectMode = 2;
+}
+
+void EditWin::CreateConnect(int nMode, std::string strroad1, std::string strroad2)
+{
+    iv::map::roadconnect_ConnectMode xmode;
+    switch (nMode) {
+    case 0:
+        xmode = iv::map::roadconnect_ConnectMode::roadconnect_ConnectMode_STRAIGHT;
+        break;
+    case 1:
+        xmode = iv::map::roadconnect_ConnectMode::roadconnect_ConnectMode_TURN;
+        break;
+    case 2:
+        xmode = iv::map::roadconnect_ConnectMode::roadconnect_ConnectMode_UTURN;
+        break;
+    default:
+        break;
+    }
+    bool bExist = false;
+    int i;
+    for(i=0;i<mcollectveh.mconnects_size();i++)
+    {
+        iv::map::roadconnect * pconnect = mcollectveh.mutable_mconnects(i);
+        if((pconnect->mmode() == xmode)&&(strroad1 == pconnect->strroad1())&&(strroad2 == pconnect->strroad2()))
+        {
+            bExist = true;
+            break;
+        }
+    }
+    if(bExist)
+    {
+        QMessageBox::warning(this,"Waring",QString(tr("道路连接已存在")),QMessageBox::YesAll);
+        return;
+    }
+
+    iv::map::roadconnect * pconnect = mcollectveh.add_mconnects();
+    pconnect->set_mmode(xmode);
+    pconnect->set_strroad1(strroad1);
+    pconnect->set_strroad2(strroad2);
+    mbChangeData = true;
+}
+
+void EditWin::DeleteRoadConnectByName(std::string strroad)
+{
+    int i;
+    for(i=0;i<mcollectveh.mconnects_size();i++)
+    {
+        iv::map::roadconnect * pconnect = mcollectveh.mutable_mconnects(i);
+        if((pconnect->strroad1() == strroad)||(pconnect->strroad2() == strroad))
+        {
+            mcollectveh.mutable_mconnects()->erase(mcollectveh.mutable_mconnects()->begin() + i);
+            i--;
+        }
+    }
+}

src/tool/map_collectfromveh/editwin.h

@@ -71,6 +71,12 @@ private slots:
 
     void on_actionRoadAdd_triggered();
 
+    void on_actionLineConnect_triggered();
+
+    void on_actionTurnConect_triggered();
+
+    void on_actionUTurnConnect_triggered();
+
 signals:
     void editwinclose();
 
@@ -94,12 +100,19 @@ private:
     void ResetPointView();
     void updateselecshow();
     QString  updatetoolbarstr(iv::map::collectvehroadpoint * pnear);
+    void CreateConnect(int nMode,std::string strroad1,std::string strroad2);
+    void DeleteRoadConnectByName(std::string strroad);
 
 private:
     int mnSelMode = 0; //0 none  1 point   2 road
 
     bool mbShow = false;
     bool mbChangeData = false;
+
+    bool mbConnecting = false;
+    int mnConnectStep = 0;
+    std::string mstrfirstsel;
+    int mnConnectMode = 0;
 };
 
 #endif // EDITWIN_H

src/tool/map_collectfromveh/editwin.qrc

@@ -12,5 +12,8 @@
         <file>icon/道路.png</file>
         <file>icon/点.png</file>
         <file>icon/点击.png</file>
+        <file>icon/调头.png</file>
+        <file>icon/直线.png</file>
+        <file>icon/转弯.png</file>
     </qresource>
 </RCC>

src/tool/map_collectfromveh/editwin.ui

@@ -60,9 +60,19 @@
     </widget>
     <addaction name="menuSelect"/>
    </widget>
+   <widget class="QMenu" name="menuTool">
+    <property name="title">
+     <string>Connect</string>
+    </property>
+    <addaction name="separator"/>
+    <addaction name="actionLineConnect"/>
+    <addaction name="actionTurnConect"/>
+    <addaction name="actionUTurnConnect"/>
+   </widget>
    <addaction name="menuPoint"/>
    <addaction name="menuRoad"/>
    <addaction name="menuView"/>
+   <addaction name="menuTool"/>
   </widget>
   <widget class="QStatusBar" name="statusbar"/>
   <widget class="QToolBar" name="toolBar">
@@ -90,6 +100,10 @@
    <addaction name="actionSelectNone"/>
    <addaction name="actionSelectPoint"/>
    <addaction name="actionSelectRoad"/>
+   <addaction name="separator"/>
+   <addaction name="actionLineConnect"/>
+   <addaction name="actionTurnConect"/>
+   <addaction name="actionUTurnConnect"/>
   </widget>
   <action name="actionMove">
    <property name="icon">
@@ -244,6 +258,51 @@
     <string>选择道路</string>
    </property>
   </action>
+  <action name="actionLineConnect">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset resource="editwin.qrc">
+     <normaloff>:/icon/直线.png</normaloff>:/icon/直线.png</iconset>
+   </property>
+   <property name="text">
+    <string>直连</string>
+   </property>
+   <property name="toolTip">
+    <string>建立道路直接连接</string>
+   </property>
+  </action>
+  <action name="actionTurnConect">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset resource="editwin.qrc">
+     <normaloff>:/icon/转弯.png</normaloff>:/icon/转弯.png</iconset>
+   </property>
+   <property name="text">
+    <string>转弯</string>
+   </property>
+   <property name="toolTip">
+    <string>建立转弯连接</string>
+   </property>
+  </action>
+  <action name="actionUTurnConnect">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset resource="editwin.qrc">
+     <normaloff>:/icon/调头.png</normaloff>:/icon/调头.png</iconset>
+   </property>
+   <property name="text">
+    <string>调头</string>
+   </property>
+   <property name="toolTip">
+    <string>建立掉头连接</string>
+   </property>
+  </action>
  </widget>
  <resources>
   <include location="editwin.qrc"/>

src/tool/map_collectfromveh/main.cpp

@@ -2,8 +2,13 @@
 
 #include <QApplication>
 
+#include "ivlog.h"
+
+iv::Ivlog * givlog;
+
 int main(int argc, char *argv[])
 {
+    givlog = new iv::Ivlog("map_collectfromveh");
     QApplication a(argc, argv);
     MainWindow w;
     w.show();

src/tool/map_collectfromveh/mainwindow.cpp

@@ -8,6 +8,7 @@
 #include <iostream>
 
 #include "gnss_coordinate_convert.h"
+#include "collectconvert.h"
 
 #include "commif.h"
 
@@ -103,6 +104,14 @@ MainWindow::MainWindow(QWidget *parent)
     connect(this,SIGNAL(updategps()),this,SLOT(ongpsupdate()));
     connect(mpEditWin,SIGNAL(editwinclose()),this,SLOT(onEditWinClose()));
 
+    double x,y;
+    GaussProjCal(120,0,&x,&y);
+//    x = 0;
+    x = x + 500000 + 2356.8309 + 1.5;
+    y = y + 4332295.5834 - 2.5;
+    double lon,lat;
+    GaussProjInvCal(x,y,&lon,&lat);
+
     setWindowTitle("Collect Data From Vehicle");
 
     gw = this;
@@ -357,6 +366,7 @@ void MainWindow::ongpsupdate()
 {
     const double thresh_headingdiff = 3.0;
     const double thresh_disdiff = 10.0;
+    const double thresh_dismin = 1.5;
     std::shared_ptr<iv::gps::gpsimu> xgpsimu_ptr;
     mMutex.lock();
     xgpsimu_ptr = mgpsimu_ptr;
@@ -410,7 +420,7 @@ void MainWindow::ongpsupdate()
         while(fheadingdiff<0)fheadingdiff = fheadingdiff + 360.0;
         while(fheadingdiff>=360)fheadingdiff = fheadingdiff -360.0;
 
-        if((fheadingdiff>=thresh_headingdiff)&&(fheadingdiff<=(360.0 - thresh_headingdiff))&&(fdis>0.5))
+        if((fheadingdiff>=thresh_headingdiff)&&(fheadingdiff<=(360.0 - thresh_headingdiff))&&(fdis>thresh_dismin))
         {
             AddPoint(xgpsimu_ptr);
             mbManualClick = false;
@@ -424,7 +434,7 @@ void MainWindow::ongpsupdate()
             }
             else
             {
-                if((fdis>0.5)&&(mbManualClick))
+                if((fdis>thresh_dismin)&&(mbManualClick))
                 {
                     AddPoint(xgpsimu_ptr);
                     mbManualClick = false;
@@ -716,3 +726,16 @@ void MainWindow::onEditWinClose()
         UpdateLoadVIew();
     }
 }
+
+#include "OpenDrive/OpenDriveXmlWriter.h"
+void MainWindow::on_actionConvert_triggered()
+{
+    OpenDrive xxodr;
+    CollectConvert * pco = new CollectConvert(mcollectveh,&xxodr);
+    pco->StartConvert();
+    delete pco;
+
+    OpenDriveXmlWriter x(&xxodr);
+    x.WriteFile("/home/yuchuli/test0331.xodr");
+
+}

src/tool/map_collectfromveh/mainwindow.h

@@ -57,6 +57,8 @@ private slots:
 
     void onEditWinClose();
 
+    void on_actionConvert_triggered();
+
 public:
      void resizeEvent(QResizeEvent *event);
 

src/tool/map_collectfromveh/mainwindow.ui

@@ -343,8 +343,15 @@
     </property>
     <addaction name="actionEdit_Collect"/>
    </widget>
+   <widget class="QMenu" name="menuTool">
+    <property name="title">
+     <string>Tool</string>
+    </property>
+    <addaction name="actionConvert"/>
+   </widget>
    <addaction name="menuFile"/>
    <addaction name="menuEdit"/>
+   <addaction name="menuTool"/>
   </widget>
   <widget class="QStatusBar" name="statusbar"/>
   <action name="actionLoad">
@@ -367,6 +374,11 @@
     <string>Edit Collect</string>
    </property>
   </action>
+  <action name="actionConvert">
+   <property name="text">
+    <string>Convert</string>
+   </property>
+  </action>
  </widget>
  <resources>
   <include location="editwin.qrc"/>

src/tool/map_collectfromveh/map_collectfromveh.pro

@@ -12,7 +12,18 @@ SOURCES += \
     ../../common/common/math/gnss_coordinate_convert.cpp \
     ../../include/msgtype/collectveh.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
+    ../map_lanetoxodr/TinyXML/tinystr.cpp \
+    ../map_lanetoxodr/TinyXML/tinyxml.cpp \
+    ../map_lanetoxodr/TinyXML/tinyxmlerror.cpp \
+    ../map_lanetoxodr/TinyXML/tinyxmlparser.cpp \
+    ../map_lanetoxodr/circlefitting.cpp \
+    ../map_lanetoxodr/const.cpp \
+    ../map_lanetoxodr/fresnl.cpp \
+    ../map_lanetoxodr/geofit.cpp \
+    ../map_lanetoxodr/polevl.c \
+    collectconvert.cpp \
     commif.cpp \
+    dialogconvert.cpp \
     editwin.cpp \
     lineitem.cpp \
     main.cpp \
@@ -23,13 +34,20 @@ HEADERS += \
     ../../common/common/math/gnss_coordinate_convert.h \
     ../../include/msgtype/collectveh.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
+    ../map_lanetoxodr/TinyXML/tinystr.h \
+    ../map_lanetoxodr/TinyXML/tinyxml.h \
+    ../map_lanetoxodr/circlefitting.h \
+    ../map_lanetoxodr/geofit.h \
+    collectconvert.h \
     commif.h \
+    dialogconvert.h \
     editwin.h \
     lineitem.h \
     mainwindow.h \
     myview.h
 
 FORMS += \
+    dialogconvert.ui \
     editwin.ui \
     mainwindow.ui
 
@@ -44,12 +62,32 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 
 !include(../../../include/ivprotobuf.pri ) {
     error( "Couldn't find the ivprotobuf.pri file!" )
+
+}
+
+!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
+    error( "Couldn't find the OpenDrive.pri file!" )
+}
+
+!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
+    error( "Couldn't find the xodrfunc.pri file!" )
 }
 
+INCLUDEPATH += $$PWD/../../common/common/xodr
+INCLUDEPATH += $$PWD/../../common/common/license
+
+INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
+
 INCLUDEPATH += ../../common/common/math
 
+INCLUDEPATH += ../map_lanetoxodr
+
+unix:INCLUDEPATH += /usr/include/eigen3
+win32:INCLUDEPATH += D:\File\soft\eigen
+
 RESOURCES += \
     editwin.qrc
 
-DISTFILES +=
+DISTFILES += \
+    ../map_lanetoxodr/TinyXML/TinyXML.pri
 

src/tool/map_lanetoxodr/TinyXML/TinyXML.pri

@@ -0,0 +1,9 @@
+HEADERS += \
+    $$PWD/tinystr.h \
+    $$PWD/tinyxml.h
+
+SOURCES += \
+    $$PWD/tinystr.cpp \
+    $$PWD/tinyxml.cpp \
+    $$PWD/tinyxmlerror.cpp \
+    $$PWD/tinyxmlparser.cpp