change tool/map_lanetoxodr. add some auto road contact code, but not complete.

src/common/common/xodr/xodrfunc.cpp

@@ -1,1164 +0,0 @@
-#include "xodrfunc.h"
-
-#include "limits"
-#include <iostream>
-
-#include <Eigen/Dense>
-#include <Eigen/Cholesky>
-#include <Eigen/LU>
-#include <Eigen/QR>
-#include <Eigen/SVD>
-
-#include <QDebug>
-#include <QPointF>
-
-xodrfunc::xodrfunc()
-{
-
-}
-
-
-inline double xodrfunc::calcpointdis(QPointF p1,QPointF p2)
-{
-    return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
-}
-
-bool xodrfunc::pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
-{
-    double hdg = CalcHdg(poingarc,point1);
-    if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
-    else hdg = hdg - M_PI/2.0;
-    if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
-    if(hdg < 0)hdg = hdg + 2.0*M_PI;
-
-    double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
-
-    double hdgdiff = hdg - parc->GetHdg();
-
-    if(hdgrange >= 0 )
-    {
-        if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
-    }
-    else
-    {
-        if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
-    }
-
-    if(fabs(hdgdiff ) < fabs(hdgrange))return true;
-    return false;
-
-}
-
-/**
-  * @brief CalcHdg
-  * 计算点0到点1的航向
-  * @param p0        Point 0
-  * @param p1        Point 1
-**/
-double xodrfunc::CalcHdg(QPointF p0, QPointF p1)
-{
-    double x0,y0,x1,y1;
-    x0 = p0.x();
-    y0 = p0.y();
-    x1 = p1.x();
-    y1 = p1.y();
-    if(x0 == x1)
-    {
-        if(y0 < y1)
-        {
-            return M_PI/2.0;
-        }
-        else
-            return M_PI*3.0/2.0;
-    }
-
-    double ratio = (y1-y0)/(x1-x0);
-
-    double hdg = atan(ratio);
-
-    if(ratio > 0)
-    {
-        if(y1 > y0)
-        {
-
-        }
-        else
-        {
-            hdg = hdg + M_PI;
-        }
-    }
-    else
-    {
-        if(y1 > y0)
-        {
-            hdg = hdg + M_PI;
-        }
-        else
-        {
-            hdg = hdg + 2.0*M_PI;
-        }
-    }
-
-    return hdg;
-}
-
-/**
- * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
- * @param parc
- * @param xnow
- * @param ynow
- * @param nearx
- * @param neary
- * @param nearhead
- * @return
- */
-double xodrfunc::GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead,double & frels)
-{
-
-    double s = 0.1;
-    double fdismin = 100000.0;
-    frels = 0;
-
-    double xold,yold;
-    xold = parc->GetX();
-    yold = parc->GetY();
-
-    double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
-    if(fdis<fdismin)
-    {
-        fdismin = fdis;
-        nearhead = parc->GetHdg();
-        nearx = parc->GetX();
-        neary = parc->GetY();
-    }
-
-    while(s < parc->GetLength())
-    {
-
-        double x, y,xtem,ytem;
-        xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
-        ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
-        x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
-        y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();
-
-        double hdg = CalcHdg(QPointF(xold,yold),QPointF(x,y));
-        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
-        if(fdis<fdismin)
-        {
-            fdismin = fdis;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-            frels = s;
-        }
-
-        xold = x;
-        yold = y;
-        s = s+ 0.1;
-    }
-
-    return fdismin;
-
-}
-
-
-/**
-  * @brief GetArcDis
-  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
-  * @param parc        pointer to a arc geomery
-  * @param x           current x
-  * @param y           current y
-  * @param nearx       near x
-  * @param neary       near y
-  * @param nearhead    nearhead
-**/
-
-double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
-                        double & neary,double & nearhead,double & frels)
-{
-    frels = 0.0;
-
-    if((parc->GetS()>370)&&(parc->GetS()<370.1))
-    {
-        int a = 1;
-    }
-
-    if(parc->GetCurvature() == 0.0)return 1000.0;
-
-    double R = fabs(1.0/parc->GetCurvature());
-
-
-
-    //calculate arc center
-    double x_center,y_center;
-    if(parc->GetCurvature() > 0)
-    {
-        x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
-        y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
-    }
-    else
-    {
-        x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
-        y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
-    }
-
-    double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
-
-
-    QPointF arcpoint;
-    arcpoint.setX(x_center);arcpoint.setY(y_center);
-
-    QPointF pointnow;
-    pointnow.setX(x);pointnow.setY(y);
-    QPointF point1,point2;
-    point1.setX(x_center + (R * cos(hdgltoa)));
-    point1.setY(y_center + (R * sin(hdgltoa)));
-    point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
-    point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
-
-    //calculat dis
-    bool bp1inarc,bp2inarc;
-    bp1inarc =pointinarc(parc,arcpoint,point1);
-    bp2inarc =pointinarc(parc,arcpoint,point2);
-    double fdis[4];
-    fdis[0] = calcpointdis(pointnow,point1);
-    fdis[1] = calcpointdis(pointnow,point2);
-    fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
-    QPointF pointend;
-    double hdgrange = parc->GetLength()*parc->GetCurvature();
-    double hdgend = parc->GetHdg() + hdgrange;
-    while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
-    while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
-
-    if(parc->GetCurvature() >0)
-    {
-        pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
-        pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
-    }
-    else
-    {
-        pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
-        pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
-    }
-
-    fdis[3] = calcpointdis(pointnow,pointend);
-    int indexmin = -1;
-    double fdismin = 1000000.0;
-    if(bp1inarc)
-    {
-        indexmin = 0;fdismin = fdis[0];
-    }
-    if(bp2inarc)
-    {
-        if(indexmin == -1)
-        {
-            indexmin = 1;fdismin = fdis[1];
-        }
-        else
-        {
-            if(fdis[1]<fdismin)
-            {
-                indexmin = 1;fdismin = fdis[1];
-            }
-        }
-    }
-    if(indexmin == -1)
-    {
-        indexmin = 2;fdismin = fdis[2];
-    }
-    else
-    {
-        if(fdis[2]<fdismin)
-        {
-            indexmin = 2;fdismin = fdis[2];
-        }
-    }
-    if(fdis[3]<fdismin)
-    {
-        indexmin = 3;fdismin = fdis[3];
-    }
-
-    double hdgdiff;
-    switch (indexmin) {
-    case 0:
-        nearx = point1.x();
-        neary = point1.y();
-        if(parc->GetCurvature()<0)
-        {
-            nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
-
-        }
-        else
-        {
-            nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
-        }
-        while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
-        while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
-        hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/fabs(parc->GetCurvature()));
-        if(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
-        if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
-        frels = hdgdiff * R;
-        break;
-    case 1:
-        nearx = point2.x();
-        neary = point2.y();
-        if(parc->GetCurvature()<0)
-        {
-            nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
-        }
-        else
-        {
-            nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
-        }
-        while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
-        while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
-        hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/fabs(parc->GetCurvature()));
-        if(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
-        if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
-        frels = hdgdiff * R;
-        break;
-    case 2:
-        nearx = parc->GetX();
-        neary = parc->GetY();
-        nearhead = parc->GetHdg();
-        frels = 0;
-        break;
-    case 3:
-        nearx = pointend.x();
-        neary = pointend.y();
-        nearhead = hdgend;
-        frels = parc->GetLength();
-        break;
-    default:
-        std::cout<<"error in arcdis "<<std::endl;
-        break;
-    }
-
-
-    while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
-    while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
-    return fdismin;
-}
-
-
-double xodrfunc::GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead,double & frels)
-{
-    double x,y,hdg;
-//    double s = 0.0;
-    double fdismin = 100000.0;
- //   double s0 = ppoly->GetS();
-
-    x = ppoly->GetX();
-    y = ppoly->GetY();
-    double A,B,C,D;
-    A = ppoly->GetA();
-    B = ppoly->GetB();
-    C = ppoly->GetC();
-    D = ppoly->GetD();
-    const double steplim = 0.3;
-    double du = steplim;
-    double u = 0;
-    double v = 0;
-    double oldx,oldy;
-    oldx = x;
-    oldy = y;
-    double xstart,ystart;
-    xstart = x;
-    ystart = y;
-
-    frels = 0;
-
-    double hdgstart = ppoly->GetHdg();
-    double flen = 0;
-    u = u+du;
-    while(flen < ppoly->GetLength())
-    {
-        double fdis = 0;
-        v = A + B*u + C*u*u + D*u*u*u;
-        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
-        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
-        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
-
-        if(fdis>(steplim*2.0))du = du/2.0;
-        flen = flen + fdis;
-        u = u + du;
-        hdg = xodrfunc::CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
-
-        double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
-        if(fdisnow<fdismin)
-        {
-            fdismin = fdisnow;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-            frels = flen;
-        }
-
-        oldx = x;
-        oldy = y;
-    }
-
-    return fdismin;
-}
-
-double xodrfunc::GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead,double & frels)
-{
-
-    double x,y,hdg;
-    double s = 0.0;
-    double fdismin = 100000.0;
-    double s0 = pspiral->GetS();
-
-    frels = 0;
-    while(s<pspiral->GetLength())
-    {
-        pspiral->GetCoords(s0+s,x,y,hdg);
-
-
-        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
-        if(fdis<fdismin)
-        {
-            fdismin = fdis;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-            frels = s;
-        }
-        s = s+0.1;
-    }
-
-    return fdismin;
-}
-
-/**
- * @brief GetLineDis 获得点到直线Geometry的距离。
- * @param pline
- * @param x
- * @param y
- * @param nearx
- * @param neary
- * @param nearhead
- * @return
- */
-double xodrfunc::GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
-                         double & neary,double & nearhead,double & frels)
-{
-    double fRtn = 1000.0;
-
-    double a1,a2,a3,a4,b1,b2;
-    double ratio = pline->GetHdg();
-    while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
-    while(ratio<0)ratio = ratio+2.0*M_PI;
-
-    double dis1,dis2,dis3;
-    double x1,x2,x3,y1,y2,y3;
-    x1 = pline->GetX();y1=pline->GetY();
-    if((ratio == 0)||(ratio == M_PI))
-    {
-        a1 = 0;a4=0;
-        a2 = 1;b1= pline->GetY();
-        a3 = 1;b2= x;
-    }
-    else
-    {
-        if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
-        {
-            a2=0;a3=0;
-            a1=1,b1=pline->GetX();
-            a4 = 1;b2 = y;
-        }
-        else
-        {
-            a1 = tan(ratio) *(-1.0);
-            a2 = 1;
-            a3 = tan(ratio+M_PI/2.0)*(-1.0);
-            a4 = 1;
-            b1 = a1*pline->GetX() + a2 * pline->GetY();
-            b2 = a3*x+a4*y;
-        }
-    }
-
-    y2 = y1 + pline->GetLength() * sin(ratio);
-    x2 = x1 + pline->GetLength() * cos(ratio);
-
-    Eigen::Matrix2d A;
-    A<<a1,a2,
-            a3,a4;
-    Eigen::Vector2d B(b1,b2);
-
-    Eigen::Vector2d opoint  = A.lu().solve(B);
-
-    x3 = opoint(0);
-    y3 = opoint(1);
-
-    dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
-    dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
-    dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
-
-
-    if((dis1>pline->GetLength())||(dis2>pline->GetLength()))  //Outoff line
-    {
- //       std::cout<<" out line"<<std::endl;
-        if(dis1<dis2)
-        {
-            fRtn = dis1;
-            nearx = x1;neary=y1;nearhead = pline->GetHdg();
-        }
-        else
-        {
-            fRtn = dis2;
-            nearx = x2;neary=y2;nearhead = pline->GetHdg();
-        }
-    }
-    else
-    {
-        fRtn = dis3;
-        nearx = x3;neary=y3;nearhead = pline->GetHdg();
-    }
-
-    frels = sqrt(pow(nearx - pline->GetX(),2)+pow(neary - pline->GetY(),2));
-
-//    qDebug("frels is %f",frels);
-
-    return fRtn;
-
-
-
-}
-
-
-namespace iv {
-struct nearoption
-{
-    Road * pRoad;
-    GeometryBlock * pgeob;
-    double fdis;
-    double nearx;
-    double neary;
-    double nearhead;
-    double fs;
-    int nlane;
-    double fgeodis;
-};
-}
-
-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)
-{
-    double dismin = std::numeric_limits<double>::infinity();
-    fdis = dismin;
-    unsigned int i;
-    *pObjRoad = 0;
-    std::vector<iv::nearoption> xvectornearopt;
-    for(i=0;i<pxodr->GetRoadCount();i++)
-    {
-        unsigned int j;
-        Road * proad = pxodr->GetRoad(i);
-        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;
-                *pObjRoad = proad;
-                *pgeo = pgb;
-                if(pfs != 0)*pfs = frels +pg->GetS();
-                if(pnlane != 0)*pnlane = nlane;
-            }
-        }
-    }
-
-    if(xvectornearopt.size() > 1)
-    {
-        double fgeodismin = 1000;
-        int nindex = 0;
-        for(i=0;i<xvectornearopt.size();i++)
-        {
-            if(xvectornearopt.at(i).fgeodis < fgeodismin)
-            {
-                fgeodismin = xvectornearopt.at(i).fgeodis;
-                nindex = i;
-            }
-        }
-        dismin = xvectornearopt.at(nindex).fdis;
-        nearx = xvectornearopt.at(nindex).nearx;
-        neary = xvectornearopt.at(nindex).neary;
-        nearhead = xvectornearopt.at(nindex).nearhead;
-        fdis = dismin;
-        *pObjRoad = xvectornearopt.at(nindex).pRoad;
-        *pgeo = xvectornearopt.at(nindex).pgeob;
-        if(pfs != 0)*pfs = xvectornearopt.at(nindex).fs;
-        if(pnlane != 0)*pnlane = xvectornearopt.at(nindex).nlane;
-
-    }
-
-
-    if(fdis > nearthresh)return -1;
-    return 0;
-}
-
-
-std::vector<iv::LanePoint> xodrfunc::GetAllLanePoint(Road *pRoad,  const double s,const double x, const double y,const double fhdg)
-{
-    int i;
-    int nLSCount = pRoad->GetLaneSectionCount();
-    double s_section = 0;
-
-
-    std::vector<iv::LanePoint> xvectorlanepoint;
-    for(i=0;i<nLSCount;i++)
-    {
-//        if((pRoad->GetRoadId() == "30012")&&(s>35))
-//        {
-//            int a= 1;
-//        }
-        LaneSection * pLS = pRoad->GetLaneSection(i);
-        if(i<(nLSCount -1))
-        {
-            if(pRoad->GetLaneSection(i+1)->GetS()<s)
-            {
-                continue;
-            }
-        }
-        s_section = pLS->GetS();
-        int nlanecount = pLS->GetLaneCount();
-        int j;
-        for(j=0;j<nlanecount;j++)
-        {
-            Lane * pLane = pLS->GetLane(j);
-
-            int nlanemarktype = -1; //default no lanetype
-            int nlanetype = 2; //driving
-            int nlanecolor = 0;
-            int k;
-            double s_lane = s-s_section;
-            for(k=0;k<pLane->GetLaneRoadMarkCount();k++)
-            {
-                 LaneRoadMark * plrm = pLane->GetLaneRoadMark(k);
-                 if(k<(pLane->GetLaneRoadMarkCount()-1))
-                 {
-                     if(pLane->GetLaneRoadMark(k+1)->GetS()<s_lane)
-                     {
-                         continue;
-                     }
-                 }
-                 else
-                 {
-                     if(s_lane<plrm->GetS())
-                     {
-                         continue;
-                     }
-                 }
-                 if(plrm->GetType() == "solid")
-                 {
-                     nlanemarktype = 0;
-                 }
-                 if(plrm->GetType() == "broken")
-                 {
-                     nlanemarktype = 1;
-                 }
-                 if(plrm->GetType() == "solid solid")
-                 {
-                     nlanemarktype = 2;
-                 }
-                 if(plrm->GetType() == "solid broken")
-                 {
-                     nlanemarktype = 3;
-                 }
-                 if(plrm->GetType() == "broken solid")
-                 {
-                     nlanemarktype = 4;
-                 }
-                 if(plrm->GetType() == "broken broken")
-                 {
-                     nlanemarktype = 5;
-                 }
-
-                 if(plrm->GetColor() == "standard")nlanecolor = 0;
-                 if(plrm->GetColor() == "blue")nlanecolor = 1;
-                 if(plrm->GetColor() == "green")nlanecolor = 2;
-                 if(plrm->GetColor() == "red")nlanecolor = 3;
-                 if(plrm->GetColor() == "white")nlanecolor = 4;
-                 if(plrm->GetColor() == "yellow")nlanecolor = 5;
-                 if(plrm->GetColor() == "orange")nlanecolor = 6;
-                 break;
-            }
-
-            if(pLane->GetType() == "shoulder")
-            {
-                nlanetype = 0;
-            }
-            if(pLane->GetType() == "border")
-            {
-                nlanetype = 1;
-            }
-            if(pLane->GetType() == "driving")
-            {
-                nlanetype = 2;
-            }
-            if(pLane->GetType() == "none")
-            {
-                nlanetype = 4;
-            }
-            if(pLane->GetType() == "biking")
-            {
-                nlanetype = 8;
-            }
-            if(pLane->GetType() == "sidewalk")
-            {
-                nlanetype = 9;
-            }
-
-
-            if(pLane->GetId() != 0)
-            {
-
-//                        if((pRoad->GetRoadId() == "10012")&&(pLane->GetId()==1))
-//                        {
-//                            int a= 1;
-//                        }
-
-//                int k;
-//                double s_lane = 0;
-                for(k=0;k<pLane->GetLaneWidthCount();k++)
-                {
-                    if(k<(pLane->GetLaneWidthCount()-1))
-                    {
-                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
-                        {
-                            continue;
-                        }
-                    }
-                    s_lane = pLane->GetLaneWidth(k)->GetS();
-                    break;
-                }
-                LaneWidth * pLW  = pLane->GetLaneWidth(k);
-                if(pLW == 0)
-                {
-                    std::cout<<"not find LaneWidth"<<std::endl;
-                    break;
-                }
-
-
-
-                iv::LanePoint lp;
-                lp.mnlanetype = nlanetype;
-                lp.mnlanemarktype = nlanemarktype;
-                lp.mnlanecolor = nlanecolor;
-                lp.mnlane = pLane->GetId();
-                lp.mnLaneSection = i;
-                double fds = s - s_lane - s_section;
-                lp.mflanewidth = pLW->GetA() + pLW->GetB() * fds
-                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
-                lp.mflanetocenter = 0;
-                lp.mnlanetype = nlanetype;
-                lp.mfhdg = fhdg;
-                lp.mS = s;
-                lp.mfGeoX = x;
-                lp.mfGeoY = y;
-                xvectorlanepoint.push_back(lp);
-
-            }
-            else
-            {
-                iv::LanePoint lp;
-                lp.mnlanetype = nlanetype;
-                lp.mnlanemarktype = nlanemarktype;
-                lp.mnlanecolor = nlanecolor;
-                lp.mnlane = 0;
-                lp.mnLaneSection = i;
-                lp.mflanewidth = 0;
-                lp.mflanetocenter = 0;
-                lp.mfhdg = fhdg;
-                lp.mS = s;
-                lp.mfGeoX = x;
-                lp.mfGeoY = y;
-                xvectorlanepoint.push_back(lp);
-            }
-        }
-
-        for(j=0;j<xvectorlanepoint.size();j++)
-        {
-            int k;
-            for(k=0;k<xvectorlanepoint.size();k++)
-            {
-                if(abs(xvectorlanepoint[k].mnlane)>abs((xvectorlanepoint[j].mnlane)))
-                {
-                    continue;
-                }
-                if(xvectorlanepoint[k].mnlane * xvectorlanepoint[j].mnlane <= 0)
-                {
-                    continue;
-                }
-                xvectorlanepoint[j].mflanetocenter = xvectorlanepoint[j].mflanetocenter + xvectorlanepoint[k].mflanewidth;
-            }
-        }
-
-        for(j=0;j<xvectorlanepoint.size();j++)
-        {
-            if(xvectorlanepoint[j].mnlane < 0)
-            {
-                xvectorlanepoint[j].mfX = x + xvectorlanepoint[j].mflanetocenter * cos(fhdg-M_PI/2.0);
-                xvectorlanepoint[j].mfY = y + xvectorlanepoint[j].mflanetocenter * sin(fhdg-M_PI/2.0);
-            }
-            else
-            {
-                xvectorlanepoint[j].mfX = x + xvectorlanepoint[j].mflanetocenter * cos(fhdg+M_PI/2.0);
-                xvectorlanepoint[j].mfY = y + xvectorlanepoint[j].mflanetocenter * sin(fhdg+M_PI/2.0);
-            }
-        }
-
-        break;
-
-    }
-
-    std::vector<iv::LanePoint> xvectorlanepointrtn;
-    bool bIsSort = true;
-    for(i=0;i<(xvectorlanepoint.size()-1);i++)
-    {
-        if(xvectorlanepoint[i].mnlane < xvectorlanepoint[i+1].mnlane)
-        {
-            bIsSort = false;
-            break;
-        }
-    }
-    if(bIsSort == false)
-    {
-        while(xvectorlanepoint.size() > 0)
-        {
-            int nlanemin;
-            nlanemin = 0;
-            int nlanenum = xvectorlanepoint[0].mnlane;
-            for(i=1;i<xvectorlanepoint.size();i++)
-            {
-                if(xvectorlanepoint[i].mnlane >= nlanenum)
-                {
-                    nlanenum = xvectorlanepoint[i].mnlane;
-                    nlanemin = i;
-                }
-            }
-            xvectorlanepointrtn.push_back(xvectorlanepoint[nlanemin]);
-            xvectorlanepoint.erase(xvectorlanepoint.begin() + nlanemin);
-        }
-    }
-    else
-    {
-        xvectorlanepointrtn = xvectorlanepoint;
-    }
-    return xvectorlanepointrtn;
-
-}
-
-double xodrfunc::GetAcurateDis(const double x, const double y, Road *pRoad,  const double s, const double nearx, const double neary, const double nearhead,int * pnlane)
-{
-    double fdismin = 1000;
-    if(pRoad->GetLaneSectionCount() < 1)return 1000;
-
-    std::vector<iv::LanePoint> xvectorlanepoint = GetAllLanePoint(pRoad,s,nearx,neary,nearhead);
-
-    double fdistoref = sqrt(pow(x-nearx,2)+pow(y-neary,2));
-
-    int i;
-    std::vector<double> xvectordis;
-    int nsize = xvectorlanepoint.size();
-    for(i=0;i<nsize;i++)
-    {
-        double fdis = sqrt(pow(x-xvectorlanepoint[i].mfX,2)+pow(y-xvectorlanepoint[i].mfY,2));
-        xvectordis.push_back(fdis);
-        if(fdismin>fdis)fdismin = fdis;
-
-    }
-    int nlane = -1000;
-    for(i=0;i<nsize;i++)
-    {
-        if((xvectordis[i]<=xvectorlanepoint[i].mflanewidth)&&(fdistoref <= xvectorlanepoint[i].mflanetocenter))
-        {
-            nlane = xvectorlanepoint[i].mnlane;
-            fdismin = 0; //On Lane, is very near.
-            break;
-        }
-    }
-
-    if(pnlane != 0)*pnlane = nlane;
-
-    return fdismin;
-
-
-}
-
-
-
-int xodrfunc::GetLineXY(GeometryLine *pline, double soff, double &x, double &y, double &hdg)
-{
-    if(soff<0)return -1;
-
-    hdg = pline->GetHdg();
-    x = pline->GetX() + soff*cos(hdg);
-    y = pline->GetY() + soff*sin(hdg);
-    return 0;
-}
-
-Road * xodrfunc::GetRoadByID(OpenDrive * pxodr,std::string strroadid)
-{
-    Road * pRoad = 0;
-    int nroadcount = pxodr->GetRoadCount();
-    int i;
-    for(i=0;i<nroadcount;i++)
-    {
-        if(pxodr->GetRoad(i)->GetRoadId() == strroadid)
-        {
-            pRoad = pxodr->GetRoad(i);
-            break;
-        }
-    }
-    return pRoad;
-}
-
-int xodrfunc::GetSpiralXY(GeometrySpiral *pspira, double soff, double &x, double &y, double &hdg)
-{
-    pspira->GetCoords(pspira->GetS() + soff,x,y,hdg);
-    return 0;
-}
-
-int xodrfunc::GetArcXY(GeometryArc *parc, double soff, double &x, double &y, double &hdg)
-{
-    if(parc->GetCurvature() == 0)return -1;
-    double R = fabs(1.0/parc->GetCurvature());
-
-    //calculate arc center
-    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
-    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
-
-    double arcdiff = soff/R;
-
-    if(parc->GetCurvature() > 0)
-    {
-        x = x_center + R * cos(parc->GetHdg() + arcdiff - M_PI/2.0);
-        y = y_center + R * sin(parc->GetHdg() + arcdiff - M_PI/2.0);
-        hdg = parc->GetHdg() + arcdiff;
-    }
-    else
-    {
-        x = x_center + R * cos(parc->GetHdg() -arcdiff + M_PI/2.0);
-        y = y_center + R * sin(parc->GetHdg() -arcdiff + M_PI/2.0);
-        hdg = parc->GetHdg() - arcdiff;
-    }
-
-
-
-    return 0;
-}
-
-int xodrfunc::GetParamPoly3XY(GeometryParamPoly3 *pparam3d, double soff, double &x, double &y, double &hdg)
-{
-    double xtem,ytem;
-    double ua,ub,uc,ud,va,vb,vc,vd;
-    ua = pparam3d->GetuA();ub= pparam3d->GetuB();uc= pparam3d->GetuC();ud = pparam3d->GetuD();
-    va = pparam3d->GetvA();vb= pparam3d->GetvB();vc= pparam3d->GetvC();vd = pparam3d->GetvD();
-//        xtem = parc->GetuA() + parc->GetuB() * s * len + parc->GetuC() * s*s *pow(len,2) + parc->GetuD() * s*s*s *pow(len,3);
-//        ytem = parc->GetvA() + parc->GetvB() * s* len + parc->GetvC() * s*s *pow(len,2) + parc->GetvD() * s*s*s *pow(len,3);
-    xtem = ua + ub * soff  + uc * soff * soff  + ud * soff *soff*soff ;
-    ytem = va + vb * soff + vc * soff*soff  + vd * soff*soff*soff ;
-    x = xtem*cos(pparam3d->GetHdg()) - ytem * sin(pparam3d->GetHdg()) + pparam3d->GetX();
-    y = xtem*sin(pparam3d->GetHdg()) + ytem * cos(pparam3d->GetHdg()) + pparam3d->GetY();
-    if(soff<0.3)hdg = pparam3d->GetHdg();
-    else
-    {
-        double soff1 = soff - 0.1;
-        double x1,y1;
-        xtem = ua + ub * soff1  + uc * soff1 * soff1  + ud * soff1 *soff1*soff1 ;
-        ytem = va + vb * soff1 + vc * soff1*soff1  + vd * soff1*soff1*soff1 ;
-        x1 = xtem*cos(pparam3d->GetHdg()) - ytem * sin(pparam3d->GetHdg()) + pparam3d->GetX();
-        y1 = xtem*sin(pparam3d->GetHdg()) + ytem * cos(pparam3d->GetHdg()) + pparam3d->GetY();
-        hdg = CalcHdg(QPointF(x1,y1),QPointF(x,y));
-    }
-    return 0;
-}
-
-int xodrfunc::GetRoadXYByS(Road *pRoad, const double s, double &x, double &y, double &hdg)
-{
-    if(s<0)return -1;
-    if(s>(pRoad->GetRoadLength()+0.1))return -2;
-    if(pRoad == 0)return -3;
-    if(pRoad->GetGeometryBlockCount()<1)return -4;
-    int i;
-    int nroadgeosize = pRoad->GetGeometryBlockCount();
-    RoadGeometry * pgeosel = pRoad->GetGeometryBlock(nroadgeosize -1)->GetGeometryAt(0);
-    for(i=0;i<(nroadgeosize-1);i++)
-    {
-        if(s<pRoad->GetGeometryBlock(i+1)->GetGeometryAt(0)->GetS())
-        {
-
-            pgeosel = pRoad->GetGeometryBlock(0)->GetGeometryAt(0);
-            break;
-        }
-    }
-
-
-    switch (pgeosel->GetGeomType()) {
-    case 0:
-        return GetLineXY((GeometryLine *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
-        break;
-    case 1:
-        return GetSpiralXY((GeometrySpiral *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
-        break;
-    case 2:
-        return GetArcXY((GeometryArc *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
-
-        break;
-    case 3:
-        break;
-    case 4:
-        return GetParamPoly3XY((GeometryParamPoly3 *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
-        break;
-    default:
-        break;
-    }
-    return -5;
-}
-
-int xodrfunc::GetRoadIndex(OpenDrive * pxodr, Road *pRoad)
-{
-    int nroadcount = pxodr->GetRoadCount();
-    int i;
-    for(i=0;i<nroadcount;i++)
-    {
-        if(pxodr->GetRoad(i) == pRoad)
-        {
-            return i;
-        }
-    }
-    return -1;
-}
-
-int xodrfunc::GetDrivingLane(Road *pRoad, const int nLS, const int nsuggestlane)
-{
-    int nrtn = nsuggestlane;
-    int nLSCount = pRoad->GetLaneSectionCount();
-    if(nLS<0)
-    {
-        qDebug("xodrfunc::GetDrivingLane nLS=%d is Error.",nLS);
-        return nrtn;
-
-    }
-
-    if(nLS>=(nLSCount))
-    {
-        qDebug("xodrfunc::GetDrivingLane nLS=%d is Error. LaneSection Count is %d.",nLS,nLSCount);
-        return nrtn;;
-    }
-
-    LaneSection * pLS = pRoad->GetLaneSection(nLS);
-    int nLaneCount = pLS->GetLaneCount();
-    int i;
-    for(i=0;i<nLaneCount;i++)
-    {
-        if(pLS->GetLane(i)->GetId() == nsuggestlane)
-        {
-            if(pLS->GetLane(i)->GetType() == "driving")
-            {
-                return nsuggestlane;
-                break;
-            }
-        }
-    }
-
-    nrtn = 1000;
-    int ndiff;
-    for(i=0;i<nLaneCount;i++)
-    {
-        Lane * pLane = pLS->GetLane(i);
-        if((pLane->GetId()*nsuggestlane>0)&&(pLane->GetType() == "driving"))
-        {
-            ndiff = pLane->GetId() - nrtn;
-            int xdiff = pLane->GetId() - nsuggestlane;
-            if(abs(xdiff)<abs(ndiff))
-            {
-                nrtn = pLane->GetId();
-            }
-        }
-    }
-    return nrtn;
-}
-
-Lane * xodrfunc::GetLaneByID(LaneSection *pLS, int nlane)
-{
-    int nlanecount = pLS->GetLaneCount();
-    int i;
-    for(i=0;i<nlanecount;i++)
-    {
-        if(pLS->GetLane(i)->GetId() == nlane)
-        {
-            return pLS->GetLane(i);
-        }
-    }
-    return 0;
-}

src/common/common/xodr/xodrfunc.h

@@ -1,71 +0,0 @@
-#ifndef XODRFUNC_H
-#define XODRFUNC_H
-
-#include <OpenDrive/OpenDrive.h>
-
-#include <QPointF>
-
-
-namespace iv {
-struct LanePoint
-{
-    int mnlane;
-    int mnLaneSection;
-    double mS;
-    double mflanewidth;
-    double mflanetocenter;
-    double mfX;
-    double mfY;
-    double mfhdg;
-    int mnlanetype;  //0 driving 1 border 2 none 3 bike
-    int mnlanemarktype; // -1 no 0 solid 1 broken 2 solidsolid
-    int mnlanecolor;
-    double mfGeoX;
-    double mfGeoY;
-
-};
-
-}
-
-class xodrfunc
-{
-public:
-    xodrfunc();
-
-public:
-    static inline double calcpointdis(QPointF p1,QPointF p2);
-    static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1);
-    static double CalcHdg(QPointF p0, QPointF p1);
-    static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
-                            double & neary,double & nearhead,double & frels);
-    static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
-                            double & neary,double & nearhead,double & frels);
-    static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
-                            double & neary,double & nearhead,double & frels);
-    static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
-                            double & neary,double & nearhead,double & frels);
-    static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
-                             double & neary,double & nearhead,double & frels);
-    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 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);
-
-    static int GetDrivingLane(Road * pRoad,const int nLS,const int nsuggestlane);
-    static Lane * GetLaneByID(LaneSection * pLS,int nlane);
-    static std::vector<iv::LanePoint> GetAllLanePoint(Road * pRoad,const double s,const double x, const double y,const double fhdg);
-
-public:
-    static Road * GetRoadByID(OpenDrive * pxodr,std::string strroadid);
-    static int GetRoadXYByS(Road * pRoad,const double s,double &x, double & y, double & hdg);
-
-    static int GetLineXY(GeometryLine * pline,double soff,double &x, double & y, double & hdg);
-    static int GetSpiralXY(GeometrySpiral * pspira,double soff,double &x, double & y, double & hdg);
-    static int GetArcXY(GeometryArc * parc,double soff,double &x, double & y, double & hdg);
-    static int GetParamPoly3XY(GeometryParamPoly3 * pparam3d,double soff,double &x, double & y, double & hdg);
-
-    static int GetRoadIndex(OpenDrive * pxodr, Road * pRoad);
-};
-
-#endif // XODRFUNC_H

src/driver/driver_map_xodrload/xodrdijkstra.cpp → src/common/common/xodr/xodrfunc/xodrdijkstra.cpp

@@ -1115,9 +1115,11 @@ double xodrdijkstra::getpathlength(std::vector<int> xvectorpath)
     return flen;
 }
 
-std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid, int ndstlr,const double s_src ,const double s_obj )
+std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid, int ndstlr,bool & bSuccess,
+                                       const double s_src ,const double s_obj )
 {
 
+    bSuccess = false;
     std::vector<int> rtnpath;
     int nvertexnum = mvertexnum;
     int i;
@@ -1250,6 +1252,11 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
             rtnpath.push_back(nedge);
             nend = nstart;
         }
+        bSuccess = true;
+    }
+    else
+    {
+        bSuccess = false;
     }
 
     qDebug("innner path distance is %f ",getpathlength(rtnpath));

src/driver/driver_map_xodrload/xodrdijkstra.h → src/common/common/xodr/xodrfunc/xodrdijkstra.h

@@ -115,7 +115,7 @@ class xodrdijkstra
 public:
     xodrdijkstra(OpenDrive  * pxodr);
 
-    std::vector<int> getpath(int srcroadid,int nsrclr,int dstroadid,int ndstlr,const double s_src = 0,const double s_obj = 0);  //nsrclr 1 left 2 right ndstlr 1 left 2 right
+    std::vector<int> getpath(int srcroadid,int nsrclr,int dstroadid,int ndstlr,bool & bSuccess,const double s_src = 0,const double s_obj = 0);  //nsrclr 1 left 2 right ndstlr 1 left 2 right
 
     std::vector<pathsection> getgpspoint(int srcroadid,int nsrclr,int dstroadid,int ndstlr,std::vector<int> xvectorpath,int nSel);
 

src/tool/map_lanetoxodr/xodrfunc.cpp → src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -1277,3 +1277,67 @@ int xodrfunc::GetRoadIndex(OpenDrive * pxodr, Road *pRoad)
     }
     return -1;
 }
+
+int xodrfunc::GetDrivingLane(Road *pRoad, const int nLS, const int nsuggestlane)
+{
+    int nrtn = nsuggestlane;
+    int nLSCount = pRoad->GetLaneSectionCount();
+    if(nLS<0)
+    {
+        qDebug("xodrfunc::GetDrivingLane nLS=%d is Error.",nLS);
+        return nrtn;
+
+    }
+
+    if(nLS>=(nLSCount))
+    {
+        qDebug("xodrfunc::GetDrivingLane nLS=%d is Error. LaneSection Count is %d.",nLS,nLSCount);
+        return nrtn;;
+    }
+
+    LaneSection * pLS = pRoad->GetLaneSection(nLS);
+    int nLaneCount = pLS->GetLaneCount();
+    int i;
+    for(i=0;i<nLaneCount;i++)
+    {
+        if(pLS->GetLane(i)->GetId() == nsuggestlane)
+        {
+            if(pLS->GetLane(i)->GetType() == "driving")
+            {
+                return nsuggestlane;
+                break;
+            }
+        }
+    }
+
+    nrtn = 1000;
+    int ndiff;
+    for(i=0;i<nLaneCount;i++)
+    {
+        Lane * pLane = pLS->GetLane(i);
+        if((pLane->GetId()*nsuggestlane>0)&&(pLane->GetType() == "driving"))
+        {
+            ndiff = pLane->GetId() - nrtn;
+            int xdiff = pLane->GetId() - nsuggestlane;
+            if(abs(xdiff)<abs(ndiff))
+            {
+                nrtn = pLane->GetId();
+            }
+        }
+    }
+    return nrtn;
+}
+
+Lane * xodrfunc::GetLaneByID(LaneSection *pLS, int nlane)
+{
+    int nlanecount = pLS->GetLaneCount();
+    int i;
+    for(i=0;i<nlanecount;i++)
+    {
+        if(pLS->GetLane(i)->GetId() == nlane)
+        {
+            return pLS->GetLane(i);
+        }
+    }
+    return 0;
+}

src/tool/map_lanetoxodr/xodrfunc.h → src/common/common/xodr/xodrfunc/xodrfunc.h

@@ -57,6 +57,7 @@ public:
 
     static std::vector<iv::LanePoint> GetAllLanePoint(Road * pRoad,const double s,const double x, const double y,const double fhdg);
 
+
 public:
     static Road * GetRoadByID(OpenDrive * pxodr,std::string strroadid);
     static Junction * GetJunctionByID(OpenDrive * pxodr,std::string strjunctionid);
@@ -70,6 +71,9 @@ public:
     static int GetRoadIndex(OpenDrive * pxodr, Road * pRoad);
 
     static int GetRoadHeightByS(Road * pRoad,const double s,double & fheight);
+
+    static int GetDrivingLane(Road * pRoad,const int nLS,const int nsuggestlane);
+    static Lane * GetLaneByID(LaneSection * pLS,int nlane);
 };
 
 #endif // XODRFUNC_H

src/common/common/xodr/xodrfunc/xodrfunc.pri

@@ -0,0 +1,7 @@
+HEADERS += \
+    $$PWD/xodrdijkstra.h \
+    $$PWD/xodrfunc.h
+
+SOURCES += \
+    $$PWD/xodrdijkstra.cpp \
+    $$PWD/xodrfunc.cpp

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -20,14 +20,12 @@ QMAKE_CXXFLAGS +=  -g
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += main.cpp     \
-    ../../common/common/xodr/xodrfunc.cpp \
     ../../include/msgtype/v2x.pb.cc \
     fresnl.cpp \
     const.cpp \
     polevl.c \
     globalplan.cpp \
     dubins.c \
-    xodrdijkstra.cpp \
     ../../include/msgtype/gps.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../include/msgtype/imu.pb.cc \
@@ -35,13 +33,11 @@ SOURCES += main.cpp     \
 
 HEADERS += \
     ../../../include/ivexit.h \
-    ../../common/common/xodr/xodrfunc.h \
     ../../include/msgtype/v2x.pb.h \
     mconf.h \
     globalplan.h \
     gps_type.h \
     dubins.h \
-    xodrdijkstra.h \
     ../../include/msgtype/gps.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/imu.pb.h \
@@ -64,7 +60,6 @@ HEADERS += \
     error( "Couldn't find the iveigen.pri file!" )
 }
 
-INCLUDEPATH += $$PWD/../../common/common/xodr
 
 !include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
     error( "Couldn't find the OpenDrive.pri file!" )
@@ -74,5 +69,12 @@ INCLUDEPATH += $$PWD/../../common/common/xodr
     error( "Couldn't find the TinyXML.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/xodr/xodrfunc
+
 
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -2889,8 +2889,13 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
 
     if(bNeedDikstra)
     {
-    std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,fs1,fs2);
+        bool bSuc = false;
+    std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
     if(xpath.size()<1)return -1;
+    if(bSuc == false)
+    {
+        return -1;
+    }
     double flen = pxd->getpathlength(xpath);
     std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
 

src/tool/map_lanetoxodr/autoroadcontact.cpp

@@ -4,6 +4,7 @@
 #include <iostream>
 
 #include "xodrfunc.h"
+#include "xodrdijkstra.h"
 
 AutoRoadContact::AutoRoadContact()
 {
@@ -596,7 +597,7 @@ bool AutoRoadContact::IsExist(OpenDrive *pxodr, iv::RoadContactUnit xRCU)
 int AutoRoadContact::MakeAllContact(OpenDrive *pxodr, const double fDisTolerance)
 {
     unsigned int nRoadCount = pxodr->GetRoadCount();
-    unsigned i;
+    int i;
     std::vector<iv::RoadContactUnit> xvectorRCU;
     for(i=0;i<nRoadCount;i++)
     {
@@ -632,6 +633,8 @@ int AutoRoadContact::MakeAllContact(OpenDrive *pxodr, const double fDisTolerance
         }
     }
 
+    xodrdijkstra * pxodrdj = new xodrdijkstra(pxodr);
+
     //Delete Repeate strait
     for(i=0;i<xvectorRCU.size();i++)
     {
@@ -646,10 +649,62 @@ int AutoRoadContact::MakeAllContact(OpenDrive *pxodr, const double fDisTolerance
                             <<" Road:"<<xvectorRCU[j].mpRoad2->GetRoadId()
                             <<" contact type: "<<xvectorRCU[j].mturnstraight<<std::endl;
                     xvectorRCU.erase(xvectorRCU.begin()+j);
+                    j = j-1;
+                }
+            }
+        }
+    }
+
+    int RCUSize = xvectorRCU.size();
+    for(i=0;i<RCUSize;i++)
+    {
+        int nlr1 = 0;
+        int nlr2 = 0;
+        if((xvectorRCU[i].mcontactype == 0)||(xvectorRCU[i].mcontactype == 1))
+        {
+            nlr1 = 1;
+        }
+        else
+        {
+            nlr1 = 2;
+        }
+
+        if((xvectorRCU[i].mcontactype == 1)||(xvectorRCU[i].mcontactype == 3))
+        {
+            nlr2 = 1;
+        }
+        else
+        {
+            nlr2 = 2;
+        }
+
+        bool bPath;
+        Road * pRoad1 = xvectorRCU[i].mpRoad1;
+        Road * pRoad2 = xvectorRCU[i].mpRoad2;
+        std::vector<int> xvectorpath = pxodrdj->getpath(atoi(pRoad1->GetRoadId().data()),nlr1,
+                         atoi(pRoad2->GetRoadId().data()),nlr2,
+                         bPath);
+        if(bPath)
+        {
+            if((xvectorpath.size()==2)||(xvectorpath.size() == 3))
+            {
+               std::vector<pathsection> xvp = pxodrdj->getgpspoint(atoi(pRoad1->GetRoadId().data()),nlr1,
+                                                                   atoi(pRoad2->GetRoadId().data()),nlr2,
+                                                                   xvectorpath,1);
+                unsigned int j;
+                std::cout<<"path have ";
+                for(j=0;j<xvp.size();j++)
+                {
+                    std::cout<<j<<": "<<xvp[j].mroadid<<" ";
                 }
+                std::cout<<std::endl;
+                xvectorRCU.erase(xvectorRCU.begin()+i);
+                i = i-1;
             }
         }
+
     }
 
     return 0;
 }
+

src/tool/map_lanetoxodr/autoroadcontact.h

@@ -34,6 +34,9 @@ public:
 
     static bool IsExist(OpenDrive * pxodr,iv::RoadContactUnit xRCU);
 
+//    static int CreateContactRoad(Road * pRoad1,Road * pRoad2,int  contacttype,int turnstraight,
+//                                 std::vector<iv::ARC> & xARCLane,std::vector<iv::ARC> & xARCOpLane);
+
 
 private:
     static std::vector<Road * > GetRelaRoad(OpenDrive * pxodr,Road * pRoad);

src/tool/map_lanetoxodr/main.cpp

@@ -52,6 +52,8 @@ bool requestPermission() {
 #endif
 
 
+#include "ivlog.h"
+iv::Ivlog *givlog;
 
 int main(int argc, char *argv[])
 {
@@ -82,6 +84,7 @@ int main(int argc, char *argv[])
         return 0;
     }
 #endif
+    givlog = new iv::Ivlog("map_lanetoxodr");
     MainWindow w;
 #endif
     w.show();

src/tool/map_lanetoxodr/map_lanetoxodr.pro

@@ -71,7 +71,6 @@ SOURCES += \
     ../../include/msgtype/imu.pb.cc \
     geofit.cpp \
     circlefitting.cpp \
-    xodrfunc.cpp \
     xodrmake.cpp \
     xodrscenfunc.cpp
 
@@ -115,7 +114,6 @@ HEADERS += \
     ../../include/msgtype/imu.pb.h \
     geofit.h \
     circlefitting.h \
-    xodrfunc.h \
     xodrmake.h \
     xodrscenfunc.h
 
@@ -146,6 +144,10 @@ FORMS += \
     error( "Couldn't find the OpenDrive.pri file!" )
 }
 
+!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
+    error( "Couldn't find the xodrfunc.pri file!" )
+}
+
 !include(../../common/common/license/adclicense.pri ) {
     error( "Couldn't find the adclicense.pri file!" )
 }
@@ -168,6 +170,8 @@ INCLUDEPATH += $$PWD/../../include/msgtype
 INCLUDEPATH += $$PWD/../../common/common/xodr
 INCLUDEPATH += $$PWD/../../common/common/license
 
+INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
+
 
 #DEFINES += OPENDRIVE_EDITONLY