change driver_map_xodr and map_lanetoxodr.

src/detection/detection_mobileye/main.cpp

@@ -42,6 +42,9 @@ void ShareResult()
 {
     char * str = new char[gmobileye.ByteSize()];
     int nsize = gmobileye.ByteSize();
+
+    gobs.pos_y();
+
     if(gmobileye.SerializeToArray(str,nsize))
     {
         iv::modulecomm::ModuleSendMsg(gpa,str,nsize);

src/driver/driver_map_xodrload/xodrdijkstra.cpp

@@ -1448,6 +1448,8 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
 
     for(i=(nsize-1);i>0;i--)
     {
+
+        int noldmainsel = xpathsection[i-1].mainsel;
         Road * pRoad = mroadedge[xvectorpath[i]].mpx;
         LaneSection * pLS = pRoad->GetLaneSection(mroadedge[xvectorpath[i]].mnsectionid);
         Lane * pLane = xodrfunc::GetLaneByID(pLS,xpathsection[i].mainsel);
@@ -1463,6 +1465,8 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
         if(nlr == 2)
         {
 
+
+
                 if((pLane2->IsSuccessorSet()) &&(nlr2 == 2))
                 {
                     if(pLane2->GetSuccessor() != xpathsection[i].mainsel)
@@ -1540,6 +1544,10 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
                                 xpathsection[i-1].mnEndLaneSel = pLane->GetPredecessor();
                                 bNeedChangeLane = false;
                             }
+                            else
+                            {
+                                bNeedChangeLane = false;
+                            }
 
                         }
                     }
@@ -1620,17 +1628,42 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
                 }
                 else
                 {
-                    if(pLane->IsSuccessorSet())
+                    if(nlr == 2)
+                    {
+                    if(pLane->IsPredecessorSet())
                     {
-                        if(pLane->GetSuccessor() != xpathsection[i-1].mainsel)
+                        if(pLane->GetPredecessor() != xpathsection[i-1].mainsel)
+                        {
+                            xpathsection[i-1].mainsel = pLane->GetPredecessor();
+                            xpathsection[i-1].mnStartLaneSel = pLane->GetPredecessor();
+                            xpathsection[i-1].mnEndLaneSel = pLane->GetPredecessor();
+                            bNeedChangeLane = false;
+                        }
+                        else
                         {
-                            xpathsection[i-1].mainsel = pLane->GetSuccessor();
-                            xpathsection[i-1].mnStartLaneSel = pLane->GetSuccessor();
-                            xpathsection[i-1].mnEndLaneSel = pLane->GetSuccessor();
                             bNeedChangeLane = false;
                         }
 
                     }
+                    }
+                    else
+                    {
+                        if(pLane->IsSuccessorSet())
+                        {
+                            if(pLane->GetSuccessor() != xpathsection[i-1].mainsel)
+                            {
+                                xpathsection[i-1].mainsel = pLane->GetSuccessor();
+                                xpathsection[i-1].mnStartLaneSel = pLane->GetSuccessor();
+                                xpathsection[i-1].mnEndLaneSel = pLane->GetSuccessor();
+                                bNeedChangeLane = false;
+                            }
+                            else
+                            {
+                                bNeedChangeLane = false;
+                            }
+
+                        }
+                    }
                 }
             }
 
@@ -1711,6 +1744,13 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
             xpathsection[i-1].secondsel = xpathsection[i-1].mainsel;
         }
 
+        if((noldmainsel * xpathsection[i-1].mainsel)<0)
+        {
+            std::cout<<" lane change sel error. "<<std::endl;
+            xpathsection[i-1].mainsel = noldmainsel;
+            xpathsection[i-1].secondsel = noldmainsel;
+        }
+
     }
 
     for(i=0;i<xpathsection.size();i++)

src/tool/map_lanetoxodr/dialogaddroadfromrtk.cpp

@@ -23,6 +23,19 @@ DialogAddRoadFromRTK::DialogAddRoadFromRTK(OpenDrive * pxodr,double lon0,double
     mlon0 = lon0;
     ui->setupUi(this);
     ui->pushButton_CreateRoad->setEnabled(false);
+
+    int i;
+    int nroadcount = mpxodr->GetRoadCount();
+    for(i=0;i<nroadcount;i++)
+    {
+        const char * strname = mpxodr->GetRoad(i)->GetRoadId().data();
+        ui->comboBox_Road->addItem(strname);
+        ui->comboBox_Road1->addItem(strname);
+        ui->comboBox_Road2->addItem(strname);
+
+    }
+
+//    MainWindow::ComboToString(strdefroad,ui->comboBox_Road);
 }
 
 DialogAddRoadFromRTK::~DialogAddRoadFromRTK()
@@ -519,3 +532,92 @@ void DialogAddRoadFromRTK::on_pushButton_CreateRoad_clicked()
 
     ui->pushButton_CreateRoad->setEnabled(false);
 }
+
+void DialogAddRoadFromRTK::on_pushButton_Tranfer_clicked()
+{
+    if(mpxodr->GetRoadCount()<1)
+    {
+        return;
+    }
+
+    Road * pRoad = mpxodr->GetRoad(ui->comboBox_Road->currentIndex());
+
+    if(pRoad == 0)return;
+
+
+    unsigned int i;
+    for(i=0;i<mvectorrtkdata.size();i++)
+    {
+        GeometryBlock * pgeob;
+        double fdis,nearx,neary,hdg;
+        double fs;
+        int nlane;
+        double rel_x = mvectorrtkdata[i].mfrelx;
+        double rel_y = mvectorrtkdata[i].mfrely;
+        if(xodrfunc::GetNearPointAtRoad(rel_x,rel_y,pRoad,&pgeob,fdis,nearx,neary,hdg,50,&fs,&nlane,true) == 0)
+        {
+            std::cout<<" dis is "<<fdis<<std::endl;
+            mvectorrtkdata[i].mfrelx =  rel_x + fdis * cos(hdg-M_PI/2.0) *0.5;
+            mvectorrtkdata[i].mfrely =  rel_y + fdis * sin(hdg -M_PI/2.0 )*0.5;
+        }
+    }
+
+}
+
+void DialogAddRoadFromRTK::on_pushButton_From2Road_clicked()
+{
+    if(mpxodr->GetRoadCount()<1)
+    {
+        return;
+    }
+    Road * pRoad1 = mpxodr->GetRoad(ui->comboBox_Road1->currentIndex());
+    Road * pRoad2 = mpxodr->GetRoad(ui->comboBox_Road2->currentIndex());
+
+    double fstep = 3.0;
+    int nsize = pRoad1->GetRoadLength()/3.0;
+
+    int i;
+    mvectorrtkdata.clear();
+    for(i=0;i<nsize;i++)
+    {
+        GeometryBlock * pgeob;
+        double fdis,nearx,neary,hdg;
+        double fs;
+        int nlane;
+        double rel_x;
+        double rel_y;
+        double rel_hdg;
+        xodrfunc::GetRoadXYByS(pRoad1,i*fstep,rel_x,rel_y,rel_hdg);
+        if(xodrfunc::GetNearPointAtRoad(rel_x,rel_y,pRoad2,&pgeob,fdis,nearx,neary,hdg,50,&fs,&nlane,true) == 0)
+        {
+            std::cout<<" dis is "<<fdis<<std::endl;
+            iv::rtkdata xdata;
+
+            double fhdg = xodrfunc::CalcHdg(QPointF(rel_x,rel_y),QPointF(nearx,neary));
+            double fdiff = fhdg - rel_hdg;
+            if(fdiff <0)fdiff = fdiff + 2.0*M_PI;
+            if(fdiff >= 2.0*M_PI)fdiff = fdiff - 2.0*M_PI;
+
+ //           xdata.mfrelx =  rel_x + fdis * cos(fhdg) *0.5;
+ //           xdata.mfrely =  rel_y + fdis * sin(fhdg )*0.5;
+            xdata.mfrels = i*fstep;
+            xodrfunc::GetRoadHeightByS(pRoad1,i*fstep,xdata.mheight);
+
+           if(fdiff < M_PI)
+           {
+            xdata.mfrelx =  rel_x + fdis * cos(hdg-M_PI/2.0) *0.5;
+            xdata.mfrely =  rel_y + fdis * sin(hdg -M_PI/2.0 )*0.5;
+           }
+           else
+           {
+               xdata.mfrelx =  rel_x + fdis * cos(hdg+M_PI/2.0) *0.5;
+               xdata.mfrely =  rel_y + fdis * sin(hdg +M_PI/2.0 )*0.5;
+           }
+
+            mvectorrtkdata.push_back(xdata);
+        }
+
+    }
+
+    ui->pushButton_CreateRoad->setEnabled(true);
+}

src/tool/map_lanetoxodr/dialogaddroadfromrtk.h

@@ -37,6 +37,10 @@ private slots:
 
     void on_pushButton_CreateRoad_clicked();
 
+    void on_pushButton_Tranfer_clicked();
+
+    void on_pushButton_From2Road_clicked();
+
 private:
     Ui::DialogAddRoadFromRTK *ui;
     OpenDrive * mpxodr;

src/tool/map_lanetoxodr/dialogaddroadfromrtk.ui

@@ -49,6 +49,124 @@
     </rect>
    </property>
   </widget>
+  <widget class="QPushButton" name="pushButton_Tranfer">
+   <property name="geometry">
+    <rect>
+     <x>660</x>
+     <y>250</y>
+     <width>89</width>
+     <height>25</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Transfer</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>33</x>
+     <y>240</y>
+     <width>110</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Base Road</string>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_Road">
+   <property name="geometry">
+    <rect>
+     <x>140</x>
+     <y>240</y>
+     <width>201</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_2">
+   <property name="geometry">
+    <rect>
+     <x>380</x>
+     <y>240</y>
+     <width>81</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Ratio</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_ratio">
+   <property name="geometry">
+    <rect>
+     <x>490</x>
+     <y>240</y>
+     <width>131</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_Road1">
+   <property name="geometry">
+    <rect>
+     <x>150</x>
+     <y>350</y>
+     <width>191</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_Road2">
+   <property name="geometry">
+    <rect>
+     <x>490</x>
+     <y>350</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_3">
+   <property name="geometry">
+    <rect>
+     <x>50</x>
+     <y>350</y>
+     <width>131</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Road1</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_4">
+   <property name="geometry">
+    <rect>
+     <x>390</x>
+     <y>350</y>
+     <width>131</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Road2</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_From2Road">
+   <property name="geometry">
+    <rect>
+     <x>700</x>
+     <y>360</y>
+     <width>101</width>
+     <height>25</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Create Data</string>
+   </property>
+  </widget>
  </widget>
  <resources/>
  <connections/>

src/tool/map_lanetoxodr/dialoglanefromrtk.cpp

@@ -87,11 +87,14 @@ void DialogLaneFromRTK::on_pushButton_CreateLane_clicked()
     double fs;
     int nlane;
 
+    pRoad = mpRoad;
     std::vector<iv::rtklanedis> xvectorrktlanedis;
 
     for(i=0;i<mvectorrtkdata.size();i++)
     {
-        if(xodrfunc::GetNearPoint(mvectorrtkdata.at(i).mfrelx,mvectorrtkdata.at(i).mfrely,mpxodr,&pRoad,&pgeob,fdis,nearx,neary,hdg,100,&fs,&nlane,true) == 0)
+//        if(xodrfunc::GetNearPoint(mvectorrtkdata.at(i).mfrelx,mvectorrtkdata.at(i).mfrely,mpxodr,&pRoad,&pgeob,fdis,nearx,neary,hdg,100,&fs,&nlane,true) == 0)
+//        {
+        if(xodrfunc::GetNearPointAtRoad(mvectorrtkdata.at(i).mfrelx,mvectorrtkdata.at(i).mfrely,pRoad,&pgeob,fdis,nearx,neary,hdg,100,&fs,&nlane,true) == 0)
         {
             double fhdgdir = geofit::CalcHdg(nearx,neary,
                                              mvectorrtkdata.at(i).mfrelx,mvectorrtkdata.at(i).mfrely);
@@ -243,6 +246,7 @@ void DialogLaneFromRTK::on_pushButton_CreateLane_clicked()
         pLane = pLS->GetLastLeftLane();
     }
 
+
     int nrange = xvectorrktlanedis.size();
     if(nrange == 1)
     {
@@ -258,10 +262,12 @@ void DialogLaneFromRTK::on_pushButton_CreateLane_clicked()
             x_veh[j] = xvectorrktlanedis.at(j).fs;
             y_veh[j] = xvectorrktlanedis.at(j).frealdis;
         }
-        VectorXd coeffs = polyfit(x_veh, y_veh, nrange -1);
+        int nnum = 3;
+        if(nrange<4)nnum = nrange -1;
+        VectorXd coeffs = polyfit(x_veh, y_veh, nnum);
         double a[4];
         for(j=0;j<4;j++)a[j] = 0;
-        for(j=0;j<nrange;j++)a[j] = coeffs[j];
+        for(j=0;j<nnum;j++)a[j] = coeffs[j];
         pLane->AddWidthRecord(0,a[0],a[1],a[2],a[3]);
     }
 

src/tool/map_lanetoxodr/xodrfunc.cpp

@@ -541,6 +541,99 @@ struct nearoption
 };
 }
 
+
+int xodrfunc::GetNearPointAtRoad(const double x, const double y, Road *pRoad, GeometryBlock **pgeo, double &fdis, double &nearx, double &neary, double &nearhead, const double nearthresh, double *pfs, int *pnlane, bool bnotuselane)
+{
+    double dismin = std::numeric_limits<double>::infinity();
+    fdis = dismin;
+    unsigned int i;
+    std::vector<iv::nearoption> xvectornearopt;
+
+    unsigned int j;
+    Road * proad = pRoad;
+    double nx,ny,nh,frels;
+
+    for(j=0;j<proad->GetGeometryBlockCount();j++)
+    {
+        GeometryBlock * pgb = proad->GetGeometryBlock(j);
+        double dis;
+        RoadGeometry * pg;
+        int nlane = 1000;
+        pg = pgb->GetGeometryAt(0);
+
+        if((sqrt(pow(x-pg->GetX(),2)+pow(y- pg->GetY(),2))-pg->GetLength())>nearthresh)
+        {
+            continue;
+        }
+
+        switch (pg->GetGeomType()) {
+        case 0:   //line
+            dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+            break;
+        case 1:
+            dis = GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
+            break;
+        case 2:  //arc
+            dis = GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
+            break;
+
+        case 3:
+            dis = GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
+            break;
+        case 4:
+            dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
+            break;
+        default:
+            dis = 100000.0;
+            break;
+        }
+
+        double fgeodis;
+
+        fgeodis = dis;
+        if((dis < 100)&&(bnotuselane == false))
+        {
+            double faccuratedis;
+            faccuratedis = GetAcurateDis(x,y,proad,frels+pg->GetS(),nx,ny,nh,&nlane);
+            if(faccuratedis < dis)dis = faccuratedis;
+
+        }
+
+        if(dis == 0)
+        {
+            iv::nearoption xopt;
+            xopt.fdis = dis;
+            xopt.fgeodis = fgeodis;
+            xopt.fs = frels +pg->GetS();
+            xopt.nearhead = nh;
+            xopt.nearx = nx;
+            xopt.neary = ny;
+            xopt.nlane = nlane;
+            xopt.pgeob = pgb;
+            xopt.pRoad = proad;
+            xvectornearopt.push_back(xopt);
+        }
+
+        if(dis < dismin)
+        {
+            dismin = dis;
+            nearx = nx;
+            neary = ny;
+            nearhead = nh;
+            fdis = dis;
+            *pgeo = pgb;
+            if(pfs != 0)*pfs = frels +pg->GetS();
+            if(pnlane != 0)*pnlane = nlane;
+        }
+    }
+
+    if(fdis<nearthresh)return 0;
+    return -1;
+
+
+
+}
+
 int xodrfunc::GetNearPoint(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad, GeometryBlock **pgeo,
                            double &fdis, double &nearx, double &neary, double &nearhead,
                            const double nearthresh,double * pfs,int * pnlane,bool bnotuselane)
@@ -1039,6 +1132,32 @@ int xodrfunc::GetParamPoly3XY(GeometryParamPoly3 *pparam3d, double soff, double
     return 0;
 }
 
+
+int xodrfunc::GetRoadHeightByS(Road *pRoad, const double s, double &fheight)
+{
+    if(pRoad->GetElevationCount() == 0)
+    {
+        fheight = 0;
+        return -1;
+    }
+
+    Elevation * pEle = pRoad->GetElevation(0);
+    unsigned int i;
+    for(i=0;i<(pRoad->GetElevationCount()-1);i++)
+    {
+        if(s<pRoad->GetElevation(i+1)->GetS())
+        {
+            pEle = pRoad->GetElevation(i);
+            break;
+        }
+    }
+
+    double frels = s - pEle->GetS();
+    fheight = pEle->GetA() + pEle->GetB() * frels
+            + pEle->GetC() * frels * frels + pEle->GetD() * frels * frels * frels;
+    return 0;
+}
+
 int xodrfunc::GetRoadXYByS(Road *pRoad, const double s, double &x, double &y, double &hdg)
 {
     if(s<0)return -1;
@@ -1053,7 +1172,7 @@ int xodrfunc::GetRoadXYByS(Road *pRoad, const double s, double &x, double &y, do
         if(s<pRoad->GetGeometryBlock(i+1)->GetGeometryAt(0)->GetS())
         {
 
-            pgeosel = pRoad->GetGeometryBlock(0)->GetGeometryAt(0);
+            pgeosel = pRoad->GetGeometryBlock(i)->GetGeometryAt(0);
             break;
         }
     }

src/tool/map_lanetoxodr/xodrfunc.h

@@ -49,6 +49,9 @@ public:
     static int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & fdis,double & nearx,
                      double & neary,double & nearhead,const double nearthresh,double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
 
+    static int GetNearPointAtRoad(const double x,const double y,Road *pRoad,GeometryBlock ** pgeo, double & fdis,double & nearx,
+                     double & neary,double & nearhead,const double nearthresh,double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
+
     static double GetAcurateDis(const double x,const double y,Road * pRoad,const  double  s,const double nearx,
                                 const double neary,const double nearhead,int * pnlane= 0);
 
@@ -64,6 +67,8 @@ public:
     static int GetParamPoly3XY(GeometryParamPoly3 * pparam3d,double soff,double &x, double & y, double & hdg);
 
     static int GetRoadIndex(OpenDrive * pxodr, Road * pRoad);
+
+    static int GetRoadHeightByS(Road * pRoad,const double s,double & fheight);
 };
 
 #endif // XODRFUNC_H