change driver_map_xodrload.

src/common/common/xodr/xodrfunc/xodrdijkstra.cpp

@@ -232,7 +232,7 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
         mroadedge[i].mbvertex = true;
         mroadedge[i].mvertexstart = 2*i;
         mroadedge[i].mvertexend = 2*i + 1;
-        qDebug("road:%d vertex %d %d ",mroadedge[i].mroadid,mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
+   //     qDebug("road:%d vertex %d %d ",mroadedge[i].mroadid,mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
 //        std::cout<<"road id "<<mroadedge[i].mroadid<<" lenght "<<mroadedge[i].mfedgelen<<std::endl;
     }
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1304,10 +1304,12 @@ static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,cons
         pp.x = x;
         pp.y = y;
         pp.hdg = hdg;
-        s = s+ fspace;
+
         pp.dis = pp.dis + fspace;;
         pp.mS = s_start + s;
         xvectorPP.push_back(pp);
+
+        s = s+ fspace;
     }
     return xvectorPP;
 }
@@ -1586,20 +1588,36 @@ struct lanewidthabcd
 
 double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
 {
+    if(nlane == 0)return 0;
     double frtn = 0;
 
+    std::vector<double> xvectorlanewidth;
 
-    int i;
-    int nlanecount = xvectorlwa.size();
-    for(i=0;i<nlanecount;i++)
+    if(nlane<0)
+        xvectorlanewidth = pRoad->GetLaneWidthVector(s,2);
+    else
+        xvectorlanewidth = pRoad->GetLaneWidthVector(s,1);
+
+    if(abs(nlane)<=xvectorlanewidth.size())
     {
-        if(nlane == xvectorlwa.at(i).nlane)
-        {
-            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
-            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
-            break;
-        }
+        return xvectorlanewidth[abs(nlane)-1];
+    }
+    else
+    {
+        std::cout<<" Warning: getwidth get lane width fault."<<std::endl;
+        return 0;
     }
+//    int i;
+//    int nlanecount = xvectorlwa.size();
+//    for(i=0;i<nlanecount;i++)
+//    {
+//        if(nlane == xvectorlwa.at(i).nlane)
+//        {
+//            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
+//            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
+//            break;
+//        }
+//    }
     return frtn;
 }
 
@@ -1700,27 +1718,34 @@ inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLW
 
 }
 
-double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
+
+double GetRoadWidth(Road * pRoad,int nlane,double s)
 {
-    double fwidth = 0;
-    int i;
-    double a,b,c,d;
+    if(nlane<0)return pRoad->GetRightRoadWidth(s);
+    return pRoad->GetLeftRoadWidth(s);
+}
 
-    int nsize = xvectorLWA.size();
-    for(i=0;i<nsize;i++)
-    {
-        if(nlane*(xvectorLWA[i].nlane)>0)
-        {
-            a = xvectorLWA[i].A;
-            b = xvectorLWA[i].B;
-            c = xvectorLWA[i].C;
-            d = xvectorLWA[i].D;
-            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
-        }
-    }
-    return fwidth;
+//double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
+//{
+//    double fwidth = 0;
+//    int i;
+//    double a,b,c,d;
 
-}
+//    int nsize = xvectorLWA.size();
+//    for(i=0;i<nsize;i++)
+//    {
+//        if(nlane*(xvectorLWA[i].nlane)>0)
+//        {
+//            a = xvectorLWA[i].A;
+//            b = xvectorLWA[i].B;
+//            c = xvectorLWA[i].C;
+//            d = xvectorLWA[i].D;
+//            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
+//        }
+//    }
+//    return fwidth;
+
+//}
 
 int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
 {
@@ -2187,7 +2212,7 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             pp.mfDisToLaneLeft = pp.mWidth/2.0;
             pp.lanmp = 0;
             pp.mfDisToRoadLeft = off1*(-1);
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            pp.mfRoadWidth = GetRoadWidth(xps.mpRoad,xps.mainsel,pp.mS);
             GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
 
             xvectorPP.push_back(pp);
@@ -2235,7 +2260,7 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             pp.mfDisToLaneLeft = pp.mWidth/2.0;
             pp.lanmp = 0;
             pp.mfDisToRoadLeft = off1;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            pp.mfRoadWidth = GetRoadWidth(xps.mpRoad,xps.mainsel,pp.mS);
             GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
 
             pp.hdg = pp.hdg + M_PI;
@@ -3004,26 +3029,26 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                         if(lr_start == 2)
                         {
                             PlanPoint pp = xvPP.at(i);
-                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            foff =  getoff(pRoad,nlane,pp.mS);// getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+   //                         std::cout<<"foff: "<<foff<<std::endl;
                             pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
                             pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
                             pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
                             pp.mfDisToLaneLeft = pp.mWidth/2.0;
                             pp.lanmp = 0;
                             pp.mfDisToRoadLeft = foff *(-1.0);
-                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            pp.mfRoadWidth = GetRoadWidth(pRoad,nlane,pp.mS);
                             pp.mLaneCur = abs(nlane);
                             pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
                             pp.mLaneCount = pRoad->GetRightDrivingLaneCount(pp.mS);
                             GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
                             xvectorPP.push_back(pp);
                             i++;
                         }
                         else
                         {
                             PlanPoint pp = xvPP.at(i);
-                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            foff = getoff(pRoad,nlane,pp.mS);// getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
                             pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
                             pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
                             pp.hdg = pp.hdg + M_PI;
@@ -3032,7 +3057,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                             pp.mfDisToLaneLeft = pp.mWidth/2.0;
                             pp.lanmp = 0;
                             pp.mfDisToRoadLeft = foff;
-                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            pp.mfRoadWidth = GetRoadWidth(pRoad,nlane,pp.mS);
                             pp.mLaneCur = abs(nlane);
                             pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
                             pp.mLaneCount = pRoad->GetLeftDrivingLaneCount(pp.mS);
@@ -3053,7 +3078,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                     xps.mbStartToMainChange = false;
                     xps.mbMainToEndChange = false;
          //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
-                    xPlan = xvectorPP;
+                    xPlan =  xvectorPP;
 
                     xvectorplans.push_back(xvectorPP);
                     xvectorroutedis.push_back(xvectorPP.size() * 0.1);