change driver_map_xodrload. now fix change lane, but not complete.

src/common/common/xodr/OpenDrive/OpenDrive.h

@@ -47,8 +47,8 @@ private:
 	/**
 	 * Copy constructor, makes the object non-copyable
 	 */
-	OpenDrive (const OpenDrive& openDrive){};
-	const OpenDrive& operator=(const OpenDrive& rhs){};
+    OpenDrive (const OpenDrive& openDrive){};
+    const OpenDrive& operator=(const OpenDrive& rhs){};
 
 public:
 	/**

src/common/common/xodr/OpenDrive/OpenDrive.pri

@@ -0,0 +1,21 @@
+HEADERS += \
+    $$PWD/Junction.h \
+    $$PWD/Lane.h \
+    $$PWD/ObjectSignal.h \
+    $$PWD/OpenDrive.h \
+    $$PWD/OpenDriveXmlParser.h \
+    $$PWD/OpenDriveXmlWriter.h \
+    $$PWD/OtherStructures.h \
+    $$PWD/Road.h \
+    $$PWD/RoadGeometry.h
+
+SOURCES += \
+    $$PWD/Junction.cpp \
+    $$PWD/Lane.cpp \
+    $$PWD/ObjectSignal.cpp \
+    $$PWD/OpenDrive.cpp \
+    $$PWD/OpenDriveXmlParser.cpp \
+    $$PWD/OpenDriveXmlWriter.cpp \
+    $$PWD/OtherStructures.cpp \
+    $$PWD/Road.cpp \
+    $$PWD/RoadGeometry.cpp

src/common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -656,6 +656,7 @@ bool OpenDriveXmlParser::ReadLane (LaneSection* laneSection, TiXmlElement *node,
 	TiXmlElement *subNode=node->FirstChildElement("link");
 	TiXmlElement *subSubNode;
 	if (subNode)
+    {
 		subSubNode=subNode->FirstChildElement("predecessor");
 			if (subSubNode)
 			{
@@ -663,7 +664,9 @@ bool OpenDriveXmlParser::ReadLane (LaneSection* laneSection, TiXmlElement *node,
 				if (checker==TIXML_SUCCESS)
 					lane->SetPredecessor(predecessor);
 			}
+    }
 	if (subNode)
+    {
 		subSubNode=subNode->FirstChildElement("successor");
 			if (subSubNode)
 			{
@@ -671,6 +674,7 @@ bool OpenDriveXmlParser::ReadLane (LaneSection* laneSection, TiXmlElement *node,
 				if (checker==TIXML_SUCCESS)
 					lane->SetSuccessor(successor);
 			}
+    }
 
 	//Proceed to the Road width
 	subNode=node->FirstChildElement("width");

src/common/common/xodr/OpenDrive/RoadGeometry.cpp

@@ -519,6 +519,27 @@ void GeometryPoly3::SetAll(double s, double x, double y, double hdg, double leng
 	ComputeVars();
 }
 
+//GetA to GetD, Added by Yuchuli
+double GeometryPoly3::GetA()
+{
+    return mA;
+}
+
+double GeometryPoly3::GetB()
+{
+    return mB;
+}
+
+double GeometryPoly3::GetC()
+{
+    return mC;
+}
+
+double GeometryPoly3::GetD()
+{
+    return mD;
+}
+
 
 //***********************************************************************************
 //Cubic Polynom geometry. Has to be implemented.  Added By Yuchuli
@@ -613,6 +634,10 @@ const GeometryBlock& GeometryBlock::operator=(const GeometryBlock& otherGeomBloc
 			{
 				delete poly;
 			}
+            else if(GeometryParamPoly3 * parampoly = dynamic_cast<GeometryParamPoly3 *>(*member) )
+            {
+                delete parampoly;
+            }
 		}
 		mGeometryBlockElement.clear();
 

src/common/common/xodr/OpenDrive/RoadGeometry.h

@@ -30,7 +30,7 @@ protected:
 	double mHdg;
 	double mLength;
 	double mS2;
-    short int mGeomType;	//0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
+    short int mGeomType;	//0-line, 2-arc, 1-spiral 3-poly3 4-parampoly3
 public:
 	/**
 	 * Constructor that initializes the base properties of teh record
@@ -294,6 +294,12 @@ public:
 	 */
 	void SetAll(double s, double x, double y, double hdg, double length, double a,double b,double c,double d);
 
+
+    double GetA();
+    double GetB();
+    double GetC();
+    double GetD();
+
 };
 
 //----------------------------------------------------------------------------------

src/common/common/xodr/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

src/common/common/xodr/xodrfunc.cpp

@@ -0,0 +1,1160 @@
+#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)
+{
+
+    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());
+
+    frels = 0.0;
+
+    //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()/abs(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()/abs(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));
+
+    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

@@ -0,0 +1,71 @@
+#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_cloud_grpc_client_BS/vehicle_control.cpp

@@ -150,10 +150,10 @@ std::string VehicleControlClient::changeCtrlMode(void)
 
 void VehicleControlClient::updateControlData(void)
 {
-//    std::cout<<"shift:"<<shiftCMD<<std::endl;
-//    std::cout<<"steeringWheelAngle:"<<steeringWheelAngleCMD<<std::endl;
+    std::cout<<"shift:"<<shiftCMD<<std::endl;
+    std::cout<<"steeringWheelAngle:"<<steeringWheelAngleCMD<<std::endl;
     std::cout<<"throttle:"<<throttleCMD<<std::endl;
-//    std::cout<<"brake:"<<brakeCMD<<std::endl;
+    std::cout<<"brake:"<<brakeCMD<<std::endl;
 }
 
 void VehicleControlClient::updateMapPOIData(void)

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -20,6 +20,7 @@ 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 \
@@ -34,6 +35,7 @@ SOURCES += main.cpp     \
 
 HEADERS += \
     ../../../include/ivexit.h \
+    ../../common/common/xodr/xodrfunc.h \
     ../../include/msgtype/v2x.pb.h \
     mconf.h \
     globalplan.h \
@@ -62,16 +64,15 @@ HEADERS += \
     error( "Couldn't find the iveigen.pri file!" )
 }
 
-#INCLUDEPATH += $$PWD/../../common/common/xodr
+INCLUDEPATH += $$PWD/../../common/common/xodr
 
-!include(../../tool/map_lanetoxodr/OpenDrive/OpenDrive.pri ) {
+!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
     error( "Couldn't find the OpenDrive.pri file!" )
 }
 
-!include(../../tool/map_lanetoxodr/TinyXML/TinyXML.pri ) {
+!include(../../common/common/xodr/TinyXML/TinyXML.pri ) {
     error( "Couldn't find the TinyXML.pri file!" )
 }
 
-INCLUDEPATH += $$PWD/../../tool/map_lanetoxodr
 
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -571,12 +571,13 @@ static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double &
   * @retval            if == 0 successfull  <0 fail.
 **/
 int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
-                 double & neary,double & nearhead,const double nearthresh)
+                 double & neary,double & nearhead,const double nearthresh,double &froads)
 {
 
     double dismin = std::numeric_limits<double>::infinity();
     s = dismin;
     int i;
+    double frels;
     for(i=0;i<pxodr->GetRoadCount();i++)
     {
         int j;
@@ -592,20 +593,21 @@ int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoa
 
             switch (pg->GetGeomType()) {
             case 0:   //line
-                dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+ //               dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
                 break;
             case 1:
-                dis = GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
                 break;
             case 2:  //arc
-                dis = GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
                 break;
 
             case 3:
-                dis = GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
                 break;
             case 4:
-                dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
                 break;
             default:
                 dis = 100000.0;
@@ -619,6 +621,7 @@ int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoa
                 neary = ny;
                 nearhead = nh;
                 s = dis;
+                froads = frels;
                 *pObjRoad = proad;
                 *pgeo = pgb;
             }
@@ -688,12 +691,13 @@ int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoa
 
 
 int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
-                 double & neary,double & nearhead,const double nearthresh)
+                 double & neary,double & nearhead,const double nearthresh,double &froads)
 {
 
     double dismin = std::numeric_limits<double>::infinity();
     s = dismin;
     int i;
+    double frels;
     std::vector<Road * > xvectorroad;
     std::vector<double > xvectordismin;
     std::vector<double > xvecotrnearx;
@@ -701,6 +705,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
     std::vector<double > xvectornearhead;
     std::vector<double > xvectors;
     std::vector<GeometryBlock *> xvectorgeob;
+    std::vector<double > xvectorfrels;
     double VECTORTHRESH = 5;
     for(i=0;i<pxodr->GetRoadCount();i++)
     {
@@ -715,6 +720,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
         double localneary = 0;
         double localnearhead = 0;
         double locals = 0;
+        double localfrels = 0;
         GeometryBlock * pgeolocal;
 
         for(j=0;j<proad->GetGeometryBlockCount();j++)
@@ -726,20 +732,20 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
 
             switch (pg->GetGeomType()) {
             case 0:   //line
-                dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
                 break;
             case 1:
-                dis = GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
                 break;
             case 2:  //arc
-                dis = GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
                 break;
 
             case 3:
-                dis = 100000.0;
+                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
                 break;
             case 4:
-                dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh);
+                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
                 break;
             default:
                 dis = 100000.0;
@@ -753,6 +759,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
                 neary = ny;
                 nearhead = nh;
                 s = dis;
+                froads = frels;
                 *pObjRoad = proad;
                 *pgeo = pgb;
 
@@ -765,6 +772,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
                 localneary = ny;
                 localnearhead = nh;
                 locals = dis;
+                localfrels = frels;
                 pgeolocal = pgb;
                 bchange = true;
             }
@@ -783,6 +791,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
             xvectors.push_back(locals);
             xvectorgeob.push_back(pgeolocal);
             xvectornearhead.push_back(localnearhead);
+            xvectorfrels.push_back(localfrels);
         }
     }
 
@@ -847,6 +856,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
                                 neary = xvectorneary[i];
                                 nearhead = xvectornearhead[i];
                                 s = xvectors[i];
+                                froads = xvectorfrels[i];
                                 *pObjRoad = xvectorroad[i];
                                 *pgeo = xvectorgeob[i];
                                 bHaveSame = true;
@@ -1015,30 +1025,31 @@ static int SelectRoadLeftRight(Road * pRoad,const double head1,const double  hea
 //}
 
 
-static std::vector<PlanPoint> getlinepoint(GeometryLine * pline)
+static std::vector<PlanPoint> getlinepoint(GeometryLine * pline,const double fspace = 0.1)
 {
     std::vector<PlanPoint> xvectorPP;
     int i;
     double s = pline->GetLength();
-    int nsize = s/0.1;
+    int nsize = s/fspace;
+    if(nsize ==0)nsize = 1;
 
     for(i=0;i<nsize;i++)
     {
         double x,y;
-        x = pline->GetX() + i *0.1 * cos(pline->GetHdg());
-        y = pline->GetY() + i *0.1 * sin(pline->GetHdg());
+        x = pline->GetX() + i *fspace * cos(pline->GetHdg());
+        y = pline->GetY() + i *fspace * sin(pline->GetHdg());
         PlanPoint pp;
         pp.x = x;
         pp.y = y;
         pp.dis = i*0.1;
         pp.hdg = pline->GetHdg();
-        pp.mS = pline->GetS() + i*0.1;
+        pp.mS = pline->GetS() + i*fspace;
         xvectorPP.push_back(pp);
     }
     return xvectorPP;
 }
 
-static std::vector<PlanPoint> getarcpoint(GeometryArc * parc)
+static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace = 0.1)
 {
     std::vector<PlanPoint> xvectorPP;
 
@@ -1051,9 +1062,9 @@ static std::vector<PlanPoint> getarcpoint(GeometryArc * parc)
     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 = 0.1/R;
-    int nsize = parc->GetLength()/0.1;
-
+    double arcdiff = fspace/R;
+    int nsize = parc->GetLength()/fspace;
+    if(nsize == 0)nsize = 1;
     int i;
     for(i=0;i<nsize;i++)
     {
@@ -1082,7 +1093,7 @@ static std::vector<PlanPoint> getarcpoint(GeometryArc * parc)
 }
 
 
-static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral)
+static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1)
 {
 
     double x,y,hdg;
@@ -1103,13 +1114,13 @@ static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral)
         pp.mS = pspiral->GetS() + s;
         xvectorPP.push_back(pp);
 
-        s = s+0.1;
+        s = s+fspace;
 
     }
     return xvectorPP;
 }
 
-static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly)
+static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly,const double fspace = 0.1)
 {
     double x,y;
     x = ppoly->GetX();
@@ -1119,7 +1130,7 @@ static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly)
     B = ppoly->GetB();
     C = ppoly->GetC();
     D = ppoly->GetD();
-    const double steplim = 0.1;
+    const double steplim = fspace;
     double du = steplim;
     double u = 0;
     double v = 0;
@@ -1139,6 +1150,7 @@ static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly)
     pp.dis = 0;
     pp.mS = ppoly->GetS();
     xvectorPP.push_back(pp);
+    u = du;
     while(flen < ppoly->GetLength())
     {
         double fdis = 0;
@@ -1168,7 +1180,7 @@ static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly)
 
 }
 
-static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc)
+static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1)
 {
     double s = 0.1;
     std::vector<PlanPoint> xvectorPP;
@@ -1228,7 +1240,7 @@ static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc)
 
 /*        if(disx > 0.1)s = s+ disx;
         else s = s+ 0.5*/;
-        s = s+ 0.1;
+        s = s+ fspace;
 
 //        s = s+ sqrt(pow(x-xold,2)+ pow(y-yold,2));
         xold = x;
@@ -1242,6 +1254,116 @@ static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc)
     return xvectorPP;
 }
 
+
+std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
+{
+    Road * pRoad = xpath.mpRoad;
+    double s_start,s_end;
+    LaneSection * pLS = pRoad->GetLaneSection(xpath.msectionid);
+    s_start = pLS->GetS();
+    if(xpath.msectionid == (pRoad->GetLaneSectionCount()-1))s_end = pRoad->GetRoadLength();
+    else
+    {
+        s_end = pRoad->GetLaneSection(xpath.msectionid+1)->GetS();
+    }
+
+    std::vector<PlanPoint> xvectorPPS;
+    double s = 0;
+
+    int i;
+    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
+    {
+        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
+        RoadGeometry * prg = pgb->GetGeometryAt(0);
+        std::vector<PlanPoint> xvectorPP;
+        if(i<(pRoad->GetGeometryBlockCount() -0))
+        {
+            if(prg->GetS()>s_end)
+            {
+                continue;
+            }
+            if((prg->GetS() + prg->GetLength())<s_start)
+            {
+                continue;
+            }
+        }
+
+        double s1 = prg->GetLength();
+        switch (prg->GetGeomType()) {
+        case 0:
+            xvectorPP = getlinepoint((GeometryLine *)prg,fspace);
+            break;
+        case 1:
+            xvectorPP = getspiralpoint((GeometrySpiral *)prg,fspace);
+            break;
+        case 2:
+            xvectorPP = getarcpoint((GeometryArc *)prg,fspace);
+
+            break;
+        case 3:
+
+            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg,fspace);
+            break;
+        case 4:
+            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg,fspace);
+            break;
+        default:
+            break;
+        }
+        int j;
+        if(prg->GetS()<s_start)
+        {
+            s1 = prg->GetLength() -(s_start - prg->GetS());
+        }
+        if((prg->GetS() + prg->GetLength())>s_end)
+        {
+            s1 = s_end - prg->GetS();
+        }
+        for(j=0;j<xvectorPP.size();j++)
+        {
+            PlanPoint pp = xvectorPP.at(j);
+            if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
+            {
+                if(s_start > prg->GetS())
+                {
+                    pp.dis = s + pp.dis -(s_start - prg->GetS());
+                }
+                else
+                {
+                    pp.dis = s+ pp.dis;
+                }
+                pp.nRoadID = atoi(pRoad->GetRoadId().data());
+                xvectorPPS.push_back(pp);
+            }
+//            if((prg->GetS()>s_start)&&((prg->GetS() + prg->GetLength())<s_end))
+//            {
+//                pp.dis = s + pp.dis;
+//                pp.nRoadID = atoi(pRoad->GetRoadId().data());
+//                xvectorPPS.push_back(pp);
+//            }
+//            else
+//            {
+//                if(prg->GetS()<s_start)
+//                {
+
+//                }
+//                else
+//                {
+//                    if(pp.dis<(s_end -prg->GetS()))
+//                    {
+//                        pp.dis = s + pp.dis;
+//                        pp.nRoadID = atoi(pRoad->GetRoadId().data());
+//                        xvectorPPS.push_back(pp);
+//                    }
+//                }
+//            }
+        }
+        s = s+ s1;
+
+    }
+    return xvectorPPS;
+}
+
 std::vector<PlanPoint> GetPoint(Road * pRoad)
 {
     std::vector<PlanPoint> xvectorPPS;
@@ -1877,16 +1999,40 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
     int i;
     if(xps.mainsel < 0)
     {
-        for(i=0;i<(nchange1- nchangepoint/2);i++)
-        {
 
+
+        for(i=0;i<nsize;i++)
+        {
             PlanPoint pp = xvPP.at(i);
-//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
 
-            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
+            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
             pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
 
+            pp.nlrchange = 1;
+
+            if(xps.mainsel != xps.secondsel)
+            {
+                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
+                pp.mfSecx = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+                pp.mfSecy = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+
+                if(xps.mainsel >xps.secondsel)
+                {
+                    pp.nlrchange = 1;
+                }
+                else
+                {
+                    pp.nlrchange = 2;
+                }
+            }
+            else
+            {
+                pp.mfSecx = pp.x ;
+                pp.mfSecy = pp.y ;
+            }
+
             pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
             pp.mfDisToLaneLeft = pp.mWidth/2.0;
             pp.lanmp = 0;
@@ -1895,159 +2041,179 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
 
             xvectorPP.push_back(pp);
-
         }
-        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
-        {
 
-            PlanPoint pp = xvPP.at(i);
-//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
-            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
-            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
-            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//        for(i=0;i<(nchange1- nchangepoint/2);i++)
+//        {
 
-            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//            PlanPoint pp = xvPP.at(i);
+////            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
 
-            pp.mfDisToRoadLeft = offx*(-1);
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
-            if(nlane1 == nlane2)
-            {
-                pp.mWidth = flanewidth1;
-                pp.mfDisToLaneLeft = pp.mWidth/2.0;
-                pp.lanmp = 0;
-                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-            }
-            else
-            {
-                if(nlane1 < nlane2)
-                {
-                    pp.lanmp = 1;
-                }
-                else
-                {
-                    pp.lanmp = -1;
-                }
+//            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
+//            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
 
-                if(i<nchange1)
-                {
-                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
-                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
-                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
-                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-                }
-                else
-                {
-                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
-                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
-                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
-                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-                }
+//            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off1*(-1);
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
 
-            }
+//            xvectorPP.push_back(pp);
 
-            xvectorPP.push_back(pp);
+//        }
+//        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
+//        {
 
-        }
-        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
-        {
-            PlanPoint pp = xvPP.at(i);
- //           off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            PlanPoint pp = xvPP.at(i);
+////            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+////            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
+//            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
+//            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
+//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
 
-            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
-            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
+//            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
 
-            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off2*(-1);
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            pp.mfDisToRoadLeft = offx*(-1);
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+//            if(nlane1 == nlane2)
+//            {
+//                pp.mWidth = flanewidth1;
+//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//                pp.lanmp = 0;
+//                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            }
+//            else
+//            {
+//                if(nlane1 < nlane2)
+//                {
+//                    pp.lanmp = 1;
+//                }
+//                else
+//                {
+//                    pp.lanmp = -1;
+//                }
 
-            xvectorPP.push_back(pp);
+//                if(i<nchange1)
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
+//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+//                else
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
+//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
 
-        }
-        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
-        {
+//            }
 
-            PlanPoint pp = xvPP.at(i);
-//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            xvectorPP.push_back(pp);
 
-            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
-            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
-            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
-            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//        }
+//        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+// //           off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
 
-            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
+//            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
 
-            pp.mfDisToRoadLeft = offx*(-1);
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
-            if(nlane2 == nlane3)
-            {
-                pp.mWidth = flanewidth1;
-                pp.mfDisToLaneLeft = pp.mWidth/2.0;
-                pp.lanmp = 0;
-                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-            }
-            else
-            {
-                if(nlane2 < nlane3)
-                {
-                    pp.lanmp = 1;
-                }
-                else
-                {
-                    pp.lanmp = -1;
-                }
+//            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off2*(-1);
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
 
-                if(i<nchange2)
-                {
-                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
-                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
-                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
-                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-                }
-                else
-                {
-                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
-                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
-                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
-                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
-                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-                }
+//            xvectorPP.push_back(pp);
 
-            }
+//        }
+//        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
+//        {
 
-            xvectorPP.push_back(pp);
+//            PlanPoint pp = xvPP.at(i);
+////            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+////            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
 
-        }
-        for(i=(nchange2 + nchangepoint/2);i<nsize;i++)
-        {
-            PlanPoint pp = xvPP.at(i);
-    //        off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
-            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
+//            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
+//            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
+//            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
+//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
 
-            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off3*(-1);
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+
+//            pp.mfDisToRoadLeft = offx*(-1);
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+//            if(nlane2 == nlane3)
+//            {
+//                pp.mWidth = flanewidth1;
+//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//                pp.lanmp = 0;
+//                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            }
+//            else
+//            {
+//                if(nlane2 < nlane3)
+//                {
+//                    pp.lanmp = 1;
+//                }
+//                else
+//                {
+//                    pp.lanmp = -1;
+//                }
+
+//                if(i<nchange2)
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
+//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+//                else
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
+//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
 
+//            }
 
-            xvectorPP.push_back(pp);
+//            xvectorPP.push_back(pp);
 
-        }
+//        }
+//        for(i=(nchange2 + nchangepoint/2);i<nsize;i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//    //        off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
+//            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off3*(-1);
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
 
     }
     else
@@ -2296,15 +2462,34 @@ static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection
                                           Road * pRoad_obj,GeometryBlock * pgeob_obj,
                                            const double x_now,const double y_now,const double head,
                                            double nearx,double neary, double nearhead,
-                                           double nearx_obj,double  neary_obj,const double fvehiclewidth)
+                                           double nearx_obj,double  neary_obj,const double fvehiclewidth,const double flen = 1000)
 {
 
   std::vector<PlanPoint> xvectorPPs;
 
-  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
+  double fspace = 0.1;
+
+  if(flen<2000)fspace = 0.1;
+  else
+  {
+      if(flen<5000)fspace = 0.3;
+      else
+      {
+          if(flen<10000)fspace = 0.5;
+          else
+              fspace = 1.0;
+      }
+  }
 
-  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
   int i;
+
+
+//  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
+  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0],fspace);
+
+
+  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
+
   std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth);
 
   if(xpathsection[0].mainsel < 0)
@@ -2334,22 +2519,25 @@ static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection
 //  }
   for(i=1;i<(npathlast);i++)
   {
-      if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
-      {
-          if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
-          {
-            continue;
-          }
-      }
-      if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
-      {
-          if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
-          {
-            break;
-          }
-      }
-      xvectorPP = GetPoint( xpathsection[i].mpRoad);
+//      if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
+//      {
+//          if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
+//          {
+//            continue;
+//          }
+//      }
+//      if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
+//      {
+//          if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
+//          {
+//            break;
+//          }
+//      }
+ //     xvectorPP = GetPoint( xpathsection[i].mpRoad);
+      xvectorPP = GetPoint( xpathsection[i],fspace);
       xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
+//      std::cout<<" road id: "<<xpathsection[i].mroadid<<" section: "
+//              <<xpathsection[i].msectionid<<" size is "<<xvectorPP.size()<<std::endl;
 //      std::cout<<" road id is "<<xpathsection[i].mpRoad->GetRoadId().data()<<" size is "<<xvPP.size()<<std::endl;
       int j;
       for(j=0;j<xvPP.size();j++)
@@ -2357,7 +2545,11 @@ static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection
           xvectorPPs.push_back(xvPP.at(j));
       }
   }
-  xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
+
+  xvectorPP = GetPoint(xpathsection[npathlast],fspace);
+
+
+//  xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
   int indexend = indexinroadpoint(xvectorPP,nearx_obj,neary_obj);
   xvPP = GetLanePoint(xpathsection[npathlast],xvectorPP,fvehiclewidth);
   int nlastsize;
@@ -2536,6 +2728,66 @@ void checktrace(std::vector<PlanPoint> & xPlan)
 }
 
 
+
+void ChangeLane(std::vector<PlanPoint> & xvectorPP)
+{
+    int i = 0;
+    int nsize = xvectorPP.size();
+    for(i=0;i<nsize;i++)
+    {
+        if((xvectorPP[i].mfSecx == xvectorPP[i].x)&&(xvectorPP[i].mfSecy == xvectorPP[i].y))
+        {
+
+        }
+        else
+        {
+            int k;
+            for(k=i;k<nsize;k++)
+            {
+                if((xvectorPP[k].mfSecx == xvectorPP[k].x)&&(xvectorPP[k].mfSecy == xvectorPP[k].y))
+                {
+                    break;
+                }
+            }
+            int nnum = k-i;
+            const int nchangepoint = 300;
+            int nstart = i + nnum/2 -nchangepoint/2;
+            int nend = i+nnum/2 + nchangepoint/2;
+            if(nnum<300)
+            {
+                nstart = i;
+                nend = k;
+            }
+            int j;
+            for(j=i;j<nstart;j++)
+            {
+                xvectorPP[j].x = xvectorPP[j].mfSecx;
+                xvectorPP[j].y = xvectorPP[j].mfSecy;
+            }
+            nnum = nend - nstart;
+            for(j=nstart;j<nend;j++)
+            {
+                double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
+                                   +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
+                double foff = fdis *(j-nstart)/nnum;
+                if(xvectorPP[j].nlrchange == 1)
+                {
+                    std::cout<<"foff is "<<foff<<std::endl;
+                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
+                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
+                }
+                else
+                {
+                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg - M_PI/2.0);
+                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg - M_PI/2.0);
+                }
+            }
+            i =k;
+
+        }
+    }
+}
+
 #include <QFile>
 
 /**
@@ -2562,20 +2814,23 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
     double  s;double nearx;
     double  neary;double  nearhead;
     Road * pRoad;GeometryBlock * pgeob;
+    double fs1,fs2;
 //    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
 
-    int nfind = GetNearPointWithHead(x_now,y_now,head,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
+    int nfind = GetNearPointWithHead(x_now,y_now,head,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh,fs1);
 
     if(nfind < 0)return -1;
+    fs1 =fs1 + pgeob->GetGeometryAt(0)->GetS();
 
     double  s_obj;double nearx_obj;
     double  neary_obj;double  nearhead_obj;
     Road * pRoad_obj;GeometryBlock * pgeob_obj;
     int nfind_obj = GetNearPoint(x_obj,y_obj,pxodr,&pRoad_obj,&pgeob_obj,s_obj,nearx_obj,neary_obj,
-                                 nearhead_obj,dstnearthresh);
+                                 nearhead_obj,dstnearthresh,fs2);
 
 
     if(nfind_obj < 0)return -2;
+    fs2 = fs2 + pgeob_obj->GetGeometryAt(0)->GetS();
 
 
     //计算终点在道路的左侧还是右侧
@@ -2607,7 +2862,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
     int lr_start = SelectRoadLeftRight(pRoad,head,nearhead);
 
     bool bNeedDikstra = true;
-    if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end))
+    if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
     {
         std::vector<PlanPoint> xvPP = GetPoint(pRoad);
         int nindexstart = indexinroadpoint(xvPP,nearx,neary);
@@ -2678,7 +2933,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
             pathsection xps;
             xps.mbStartToMainChange = false;
             xps.mbMainToEndChange = false;
-            CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
+ //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
             xPlan = xvectorPP;
 
         }
@@ -2686,12 +2941,14 @@ 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);
+    std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,fs1,fs2);
+    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);
 
     std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
-                 head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth);
+                 head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
 
+    ChangeLane(xvectorPP);
     xPlan = xvectorPP;
 
     }
@@ -2712,7 +2969,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
 //    }
 //    xFile.close();
 
-    checktrace(xPlan);
+ //   checktrace(xPlan);
 //    std::cout<<"pp size is "<<xvectorPP.size()<<std::endl;
     return 0;
 

src/driver/driver_map_xodrload/globalplan.h

@@ -4,6 +4,8 @@
 
 #include <vector>
 
+#include "xodrfunc.h"
+
 #include "xodrdijkstra.h"
 class PlanPoint
 {
@@ -33,6 +35,16 @@ public:
     int nSignal = -1;   //if 0 no signal point
 
     int nRoadID =-1;
+
+    double mfSecx;
+    double mfSecy;
+    int nlrchange; //1 left 2 right
+};
+
+class LaneChangePoint
+{
+    int nRoadID = -1;
+    double mS = 0;
 };
 
 int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,

src/driver/driver_map_xodrload/xodrdijkstra.cpp

@@ -94,6 +94,7 @@ Road * xodrdijkstra::GetRoadByID(int nRoadID)
     }
 
     std::cout<<"xodrdijkstra::GetRoadByID "<<nRoadID<<"  Fail."<<std::endl;
+    return 0;
 }
 
 
@@ -137,13 +138,43 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
             if(px->GetLaneSection(j)->GetLeftLaneCount()>0)
             {
-                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),1,j,px);
+
+                double fseclen = 0;
+                if(j<(px->GetLaneSectionCount() -1))
+                {
+
+                    fseclen = px->GetLaneSection(j+1)->GetS() - px->GetLaneSection(j)->GetS();
+
+                }
+                else
+                {
+                    fseclen = px->GetRoadLength() - px->GetLaneSection(j)->GetS();
+                }
+
+                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),fseclen,1,j,px);
+//                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),1,j,px);
+
+
                 //           xroad.mpx = px;
                 mroadedge.push_back(xroad);
             }
             if(px->GetLaneSection(j)->GetRightLaneCount()>0)
             {
-                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),2,j,px);
+                double fseclen = 0;
+                if(j<(px->GetLaneSectionCount() -1))
+                {
+
+                    fseclen = px->GetLaneSection(j+1)->GetS() - px->GetLaneSection(j)->GetS();
+
+                }
+                else
+                {
+                    fseclen = px->GetRoadLength() - px->GetLaneSection(j)->GetS();
+                }
+                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),fseclen,2,j,px);
+
+//                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),2,j,px);
+
                 //           xroad.mpx = px;
                 mroadedge.push_back(xroad);
             }
@@ -192,6 +223,9 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
             int road_from = atoi(pjc->GetIncomingRoad().data());
             int road_to = atoi(pjc->GetConnectingRoad().data());
+
+            Road * pRoad_From = GetRoadByID(road_from);
+            Road * pRoad_To = GetRoadByID(road_to);
             int k;
             for(k=0;k<pjc->GetJunctionLaneLinkCount();k++)
             {
@@ -234,6 +268,8 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 //                    if(lr_from == 2)
                     if(lr_to == 1)
                     {
+                        fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
+                        toedge = getroadedge(road_to,lr_to);
                         a = mroadedge[fromedge].mvertexend;
                         b = mroadedge[toedge].mvertexstart;
                         lnu.mpFromRoad = GetRoadByID(road_from);
@@ -245,6 +281,8 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                     }
                     else
                     {
+                        fromedge = getroadedge(road_from,lr_from);
+                        toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
                         a = mroadedge[fromedge].mvertexstart;
                         b = mroadedge[toedge].mvertexend;
                         lnu.mpFromRoad = GetRoadByID(road_to);
@@ -262,6 +300,8 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
  //                   if(lr_from == 1)
                     if(lr_to == 2)
                     {
+                        fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
+                        toedge = getroadedge(road_to,lr_to);
                         a = mroadedge[fromedge].mvertexend;
                         b = mroadedge[toedge].mvertexstart;
                         lnu.mpFromRoad = GetRoadByID(road_from);
@@ -273,6 +313,8 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                     }
                     else
                     {
+                        fromedge = getroadedge(road_from,lr_from);
+                        toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
                         a = mroadedge[fromedge].mvertexstart;
                         b = mroadedge[toedge].mvertexend;
                         lnu.mpFromRoad = GetRoadByID(road_to);
@@ -504,6 +546,14 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                     int nroadpre = atoi(px->GetPredecessor()->GetElementId().data());
                     int nroadcur = atoi(px->GetRoadId().data());
 
+                    Road * pRoad_Pre = GetRoadByID(nroadpre);
+
+                    if(pRoad_Pre == 0)
+                    {
+                        qDebug("Pre Road Missing.");
+                        continue;
+                    }
+
 
 //                    if(nroadcur == 30012)
 //                    {
@@ -527,7 +577,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                             {
                                 if(npre < 0)
                                 {
-                                    int nedgepre = getroadedge(nroadpre,2);
+                                    int nedgepre = getroadedge(nroadpre,2,pRoad_Pre->GetLaneSectionCount()-1);
                                     a = mroadedge[nedgepre].mvertexend;
                                     lnu.mpFromRoad = GetRoadByID(nroadpre);
                                     lnu.mpToRoad = GetRoadByID(nroadcur);
@@ -538,7 +588,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
                                 }
                                 else
                                 {
-                                    int nedgepre = getroadedge(nroadpre,1);
+                                    int nedgepre = getroadedge(nroadpre,1,pRoad_Pre->GetLaneSectionCount()-1);
                                     a = mroadedge[nedgepre].mvertexstart;
                                     lnu.mpFromRoad = GetRoadByID(nroadcur);
                                     lnu.mpToRoad = GetRoadByID(nroadpre);
@@ -978,6 +1028,15 @@ int xodrdijkstra::getroadedge(int nroad,int leftright,int nsection )
     int nrtn = -1;
 
     int i;
+
+//    for(i=0;i<mroadedge.size();i++)
+//    {
+//        if((mroadedge[i].mroadid == nroad) &&(mroadedge[i].mnleftright == leftright) )
+//        {
+//            qDebug("equal, road %d edge %d ",nroad,i);
+//        }
+//    }
+
     for(i=0;i<mroadedge.size();i++)
     {
         if((mroadedge[i].mroadid == nroad) &&(mroadedge[i].mnleftright == leftright) &&(nsection == mroadedge[i].mnsectionid))
@@ -1020,15 +1079,57 @@ inline double xodrdijkstra::getedgedis(int vs, int vd)
     return dis;
 }
 
-std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid, int ndstlr)
+int xodrdijkstra::GetRoadSectionIndexByS(Road *pRoad, const double s)
+{
+    int nrtn = -1;
+    if(s<0)return nrtn;
+    if(s>pRoad->GetRoadLength())return nrtn;
+    int i;
+    int nseccount = pRoad->GetLaneSectionCount();
+    if(nseccount== 1)return 0;
+    for(i=0;i<(nseccount-1);i++)
+    {
+        if(s<(pRoad->GetLaneSection(i+1)->GetS()))
+        {
+            break;
+        }
+    }
+    return i;
+}
+
+double xodrdijkstra::getpathlength(std::vector<int> xvectorpath)
+{
+    int i;
+    int nsize = xvectorpath.size();
+    double flen = 0;
+    for(i=0;i<nsize;i++)
+    {
+        flen = flen + mroadedge[xvectorpath[i]].mlen;
+    }
+    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> rtnpath;
     int nvertexnum = mvertexnum;
     int i;
 
-    int srcedge = getroadedge(srcroadid,nsrclr);
-    int dstedge = getroadedge(dstroadid,ndstlr);
+    int nsecsrc =0;
+    int nsecdst =0;
+    nsecsrc = GetRoadSectionIndexByS(GetRoadByID(srcroadid),s_src);
+    nsecdst = GetRoadSectionIndexByS(GetRoadByID(dstroadid),s_obj);
+    if((nsecsrc<0)||(nsecdst<0))
+    {
+        qDebug("getpath section error.");
+        return rtnpath;
+    }
+    int srcedge = getroadedge(srcroadid,nsrclr,nsecsrc);
+    int dstedge = getroadedge(dstroadid,ndstlr,nsecdst);
+
+//    roadedge * px1 = &mroadedge[srcedge];
+//    roadedge * px2 = &mroadedge[dstedge];
 
     if((srcedge == -1)||(dstedge == -1))
     {
@@ -1295,6 +1396,240 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
     //First Get Every Path's lane section for calculate point.
     int i;
     int nsize = xvectorpath.size();
+
+    for(i=0;i<nsize;i++)
+    {
+         Road * pRoad = mroadedge[xvectorpath[i]].mpx;
+         qDebug("road %d is %s",i,pRoad->GetRoadId().data());
+    }
+
+    Road * pLastRoad = mroadedge[xvectorpath[nsize-1]].mpx;
+
+
+
+    for(i=0;i<nsize;i++)
+    {
+        pathsection xps;
+        Road * pRoad = mroadedge[xvectorpath[i]].mpx;
+        int nsuggest = nSel;
+        if(mroadedge[xvectorpath[i]].mnleftright == 2)nsuggest  = nsuggest *(-1);
+        int nlane = xodrfunc::GetDrivingLane(pRoad,mroadedge[xvectorpath[i]].mnsectionid,nsuggest);
+        xps.mainsel = nlane;
+        xps.mnStartLaneSel = nlane;
+        xps.mnEndLaneSel = nlane;
+        xps.mainselindex = 0;//mainselindex;
+        xps.mroadid = mroadedge[xvectorpath[i]].mroadid;
+        xps.msectionid = mroadedge[xvectorpath[i]].mnsectionid;
+        xps.mnroadedgeid = xvectorpath[i];
+        xps.mpRoad = pRoad;
+        xpathsection.push_back(xps);
+    }
+
+    for(i=(nsize-1);i>0;i--)
+    {
+        Road * pRoad = mroadedge[xvectorpath[i]].mpx;
+        LaneSection * pLS = pRoad->GetLaneSection(mroadedge[xvectorpath[i]].mnsectionid);
+        Lane * pLane = xodrfunc::GetLaneByID(pLS,xpathsection[i].mainsel);
+
+        Road * pRoad2 = mroadedge[xvectorpath[i-1]].mpx;
+        LaneSection * pLS2 = pRoad2->GetLaneSection(mroadedge[xvectorpath[i-1]].mnsectionid);
+        Lane * pLane2 = xodrfunc::GetLaneByID(pLS2,xpathsection[i-1].mainsel);
+
+        int nlr = mroadedge[xvectorpath[i]].mnleftright;
+
+        bool bNeedChangeLane = true;
+        if(nlr == 2)
+        {
+            if(pLane2->IsSuccessorSet())
+            {
+                if(pLane2->GetSuccessor() != xpathsection[i].mainsel)
+                {
+                    bool bfindgood = false;
+                    int ngoodlane = 0;
+                    int j;
+                    for(j=0;j<pLS2->GetLaneCount();j++)
+                    {
+                        if(pLS2->GetLane(j)->IsSuccessorSet())
+                        {
+                            if(pLS2->GetLane(j)->GetSuccessor() == xpathsection[i].mainsel)
+                            {
+                                ngoodlane = pLS2->GetLane(j)->GetId();
+                                bfindgood = true;
+                                break;
+                            }
+                        }
+                    }
+                    if(bfindgood)
+                    {
+                        xpathsection[i-1].mainsel = ngoodlane;
+                        xpathsection[i-1].mnStartLaneSel = ngoodlane;
+                        xpathsection[i-1].mnEndLaneSel = ngoodlane;
+                        bNeedChangeLane = false;
+                    }
+                }
+                else
+                {
+                    bNeedChangeLane = false;
+                }
+            }
+            else
+            {
+                if(pLane->IsPredecessorSet())
+                {
+                    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
+        {
+            if(pLane2->IsPredecessorSet())
+            {
+                if(pLane2->GetPredecessor() != xpathsection[i].mainsel)
+                {
+                    bool bfindgood = false;
+                    int ngoodlane = 0;
+                    int j;
+                    for(j=0;j<pLS2->GetLaneCount();j++)
+                    {
+                        if(pLS2->GetLane(j)->IsPredecessorSet())
+                        {
+                            if(pLS2->GetLane(j)->GetPredecessor() == xpathsection[i].mainsel)
+                            {
+                                ngoodlane = pLS2->GetLane(j)->GetId();
+                                bfindgood = true;
+                                break;
+                            }
+                        }
+                    }
+                    if(bfindgood)
+                    {
+                        xpathsection[i-1].mainsel = ngoodlane;
+                        xpathsection[i-1].mnStartLaneSel = ngoodlane;
+                        xpathsection[i-1].mnEndLaneSel = ngoodlane;
+                        bNeedChangeLane = false;
+                    }
+                }
+                else
+                {
+                    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;
+                    }
+
+                }
+            }
+
+        }
+
+        if(bNeedChangeLane == true)
+        {
+            std::cout<<" Road "<<pRoad->GetRoadId()<<" need change lane "<<std::endl;
+            if(pRoad == pRoad2)
+            {
+                std::cout<<" Lane Change in road inter. "<<std::endl;
+            }
+            else
+            {
+                if(nlr == 2)
+                {
+                    if(pLane2->IsSuccessorSet())
+                    {
+                        xpathsection[i].secondsel = pLane2->GetSuccessor();
+                    }
+                    else
+                    {
+                        int k;
+                        int xlanecount = pLS->GetLaneCount();
+                        for(k=0;k<xlanecount;k++)
+                        {
+                            if(pLS->GetLane(k)->IsPredecessorSet())
+                            {
+                                if(pLS->GetLane(k)->GetPredecessor() == pLane2->GetSuccessor())
+                                {
+                                    xpathsection[i].secondsel = k;
+                                    break;
+                                }
+                            }
+                        }
+                    }
+                }
+                else
+                {
+                    if(pLane2->IsPredecessorSet())
+                    {
+                        xpathsection[i].secondsel = pLane2->GetPredecessor();
+                    }
+                    else
+                    {
+                        int k;
+                        int xlanecount = pLS->GetLaneCount();
+                        for(k=0;k<xlanecount;k++)
+                        {
+                            if(pLS->GetLane(k)->IsSuccessorSet())
+                            {
+                                if(pLS->GetLane(k)->GetSuccessor() == pLane2->GetPredecessor())
+                                {
+                                    xpathsection[i].secondsel = k;
+                                    break;
+                                }
+                            }
+                        }
+                    }
+
+                }
+
+            }
+
+            int k;
+            for(k=(nsize-1);k>i;k--)
+            {
+                if(xpathsection[k].mpRoad == xpathsection[i].mpRoad)
+                {
+                    xpathsection[k].secondsel = xpathsection[i].secondsel;
+                }
+            }
+
+        }
+        else
+        {
+            xpathsection[i].secondsel = xpathsection[i].mainsel;
+            xpathsection[i-1].secondsel = xpathsection[i-1].mainsel;
+        }
+
+    }
+
+    for(i=0;i<xpathsection.size();i++)
+    {
+        std::cout<<"path "<<i<<" road:"<<xpathsection[i].mroadid<<" section "<<xpathsection[i].msectionid
+                <<"  lane  "<<xpathsection[i].mainsel<<std::endl;
+    }
+
+    return xpathsection;
+
+
+//    for(i=(nsize-1);i>=0;i--)
+//    {
+//        Road * pRoad = mroadedge[xvectorpathi]
+//    }
+
+
     for(i=0;i<nsize;i++)
     {
         Road * pRoad = mroadedge[xvectorpath[i]].mpx;

src/driver/driver_map_xodrload/xodrdijkstra.h

@@ -10,6 +10,7 @@
 #include "ivfault.h"
 #include "ivlog.h"
 
+#include "xodrfunc.h"
 
 class roadedge
 {
@@ -68,6 +69,8 @@ public:
 
 
 
+
+
 };
 
 
@@ -94,7 +97,7 @@ class pathsection
   public:
     int mroadid;
     int msectionid;
-    int mainsel;       //lane 1 -1 eq.
+    int mainsel = -1;       //lane 1 -1 eq.
     int mainselindex;   //index of lane, 无效
     bool mbStartToMainChange = false;  //Is Start  To Main have Change.
     int mnStartLaneSel;
@@ -104,6 +107,7 @@ class pathsection
     int mnMainToEndLeftRight = 0;  // 1 To Left  2 To Right 0 No Change
     int mnroadedgeid;
     Road * mpRoad;
+    int secondsel = -1; //if not change lane mainsel equal secondsel
 };
 
 class xodrdijkstra
@@ -111,10 +115,12 @@ class xodrdijkstra
 public:
     xodrdijkstra(OpenDrive  * pxodr);
 
-    std::vector<int> getpath(int srcroadid,int nsrclr,int dstroadid,int ndstlr);  //nsrclr 1 left 2 right ndstlr 1 left 2 right
+    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<pathsection> getgpspoint(int srcroadid,int nsrclr,int dstroadid,int ndstlr,std::vector<int> xvectorpath,int nSel);
 
+    double getpathlength(std::vector<int> xvectorpath);
+
 private:
     OpenDrive * mpxodr;
 
@@ -138,12 +144,16 @@ private:
 
     Road * GetRoadByID(int nRoadID);
 
+    int GetRoadSectionIndexByS(Road * pRoad ,const double s);
+
     std::vector<lanenetunit> GetLaneNet(Road * p1,int nsec1,Road * p2,int nsec2);
 
     bool IsInLaneNet(std::vector<lanenetunit> xln,Road * p1,int nsec1,int nlane1,Road * p2,int nsec2,int nlane2);
 
 
 
+
+
     std::vector<vertexedge> mvectorvertexedge;